---
# System prepended metadata

title: Core
tags: [code documentation]

---

Core
===
###### tags: `code documentation`
[TOC]

## calibration/
* accel_calibration.c
* calibration_task.c
* compass_calibration.c
* esc_calibration.c


## controllers/
### actuator/
* motor_thrust_fitting.c


### autopilot/

[autopilot blockdiagram](https://hackmd.io/HKWfJTSDTemKHoI0TLg6Iw)

#### autopilot.c
飛行員前置作業，確認motor，下降速度，上升速度，燈號，目標位置等等。
- 控制任務
1. waypint mission
2. trajectory mission
3. landing
4. goto 
- debug_link(uart2)
    - waypoint list
    - waypoint status
#### fence.c
#### takeoff_landing.c
#### trajectory_following.c
#### waypoint_following.c

### multirotor_geometry/
#### multirotor_geometry_ctrl.c
#### geometry_ctrl_init()
- init_multirotor_geometry_param_list()
    - 初始化controller property
- autopilot_init()
    - 初始化 autopilot structure
    - enu frame
- autopilot_set_enu_rectangular_fence()
    - 防止 waypoint 超出某個長方形區域

#### estimate_uav_dynamics()


#### reset_geometry_tracking_error_integral
- rest tracking_error_integral

#### geometry_manual_ctrl
- return moment without knowing current property
```c=
/* control input M1, M2, M3 */
output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + mat_data(inertia_effect)[0];
output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + mat_data(inertia_effect)[1];
output_moments[2] = -_krz*mat_data(eR)[2] -_kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2];
```
- force is openloop outside the function
    - In multirotor_geometry_ctrl

    ```c=
    /* generate total thrust for quadrotor (open-loop) */
    control_force = 4.0f * convert_motor_cmd_to_thrust(rc->throttle * 0.01 /* [%] */);
    ```
#### geometry_tracking_ctrl()
- return force
```c=
arm_dot_prod_f32(neg_kxex_kvev_mge3_mxd_dot_dot, mat_data(Re3), 3, output_force);
```
-  return moment
```c=
/* control input M1, M2, M3 */
output_moments[0] = -krx*mat_data(eR)[0] -kwx*mat_data(eW)[0] + mat_data(inertia_effect)[0];
output_moments[1] = -kry*mat_data(eR)[1] -kwy*mat_data(eW)[1] + mat_data(inertia_effect)[1];
output_moments[2] = -krz*mat_data(eR)[2] -kwz*mat_data(eW)[2] + mat_data(inertia_effect)[2];
```

#### mr_geometry_ctrl_thrust_allocation()
- allocation matrix
```c=
/* quadrotor thrust allocation */
float distributed_force = total_force *= 0.25; //split force to 4 motors
float motor_force[4];
motor_force[0] = -l_div_4 * moment[0] + l_div_4 * moment[1] +
                 -b_div_4 * moment[2] + distributed_force;
motor_force[1] = +l_div_4 * moment[0] + l_div_4 * moment[1] +
                 +b_div_4 * moment[2] + distributed_force;
motor_force[2] = +l_div_4 * moment[0] - l_div_4 * moment[1] +
                 -b_div_4 * moment[2] + distributed_force;
motor_force[3] = -l_div_4 * moment[0] - l_div_4 * moment[1] +
                 +b_div_4 * moment[2] + distributed_force;

set_motor_value(MOTOR1, convert_motor_thrust_to_cmd(motor_force[0]));
set_motor_value(MOTOR2, convert_motor_thrust_to_cmd(motor_force[1]));
set_motor_value(MOTOR3, convert_motor_thrust_to_cmd(motor_force[2]));
set_motor_value(MOTOR4, convert_motor_thrust_to_cmd(motor_force[3]));
```
#### rc_mode_handler_geometry_ctrl
#### multirotor_geometry_control

1. RC Event
- rc_mode_handler_geometry_ctrl()
    - set autopilot_flyingmode 

2. Sensor status
- xy_pos_available()
- is_height_available()
- is_heading_availabel()

Get ... Data 
- 詳細請看 interface/system_state.c

3. Get IMU Data
- 先宣告 array，傳入位址到 function 之後 就可以取得sensor data
- get_accel_lpf(accel_lpf)
- get_gyro_lpf(gyro_lpf)
```c=
float accel_lpf[3];
get_accel_lpf(accel_lpf);

void get_accel_lpf(float *accel)
{
	mpu6500_get_filtered_accel(accel);
}
```
**system_state.c : 得到系統目前所有的姿態位置資訊
ins.c : (慣性導航系統, 裡面有ahrs, gps, vins_mono, optitrack .... 構成所以的慣性資訊)**

4. Get Attitude: from system_state.c -> ins.c 
5. Get R, P, Y : from system_state.c -> ins.c
6. 



#### send_geometry_moment_ctrl_debug
- moment debug
```c=
pack_debug_debug_message_header(payload, MESSAGE_ID_GEOMETRY_MOMENT_CTRL);
pack_debug_debug_message_float(&roll_error, payload);
pack_debug_debug_message_float(&pitch_error, payload);
pack_debug_debug_message_float(&yaw_error, payload);
pack_debug_debug_message_float(&wx_error, payload);
pack_debug_debug_message_float(&wy_error, payload);
pack_debug_debug_message_float(&wz_error, payload);
pack_debug_debug_message_float(&geometry_ctrl_feedback_moments[0], payload);
pack_debug_debug_message_float(&geometry_ctrl_feedback_moments[1], payload);
pack_debug_debug_message_float(&geometry_ctrl_feedback_moments[2], payload);
pack_debug_debug_message_float(&geometry_ctrl_feedfoward_moments[0], payload);
pack_debug_debug_message_float(&geometry_ctrl_feedfoward_moments[1], payload);
pack_debug_debug_message_float(&geometry_ctrl_feedfoward_moments[2], payload);
```
#### send_geometry_tracking_ctrl_debug
- pack position error
#### send_uav_dynamics_debug
- pack M = (J * W_dot) + (W X JW)
- pack M_rot = (J * W_dot)



#### multirotor_geometry_param.c
- init_multirotor_geometry_param_list
    - init controller gain, mass, thrust



### multirotor_pid/
* multirotor_pid_ctrl.c
* multirotor_pid_param.c
## debug_link/
* debug_link.c
* debug_msg.c

## filters/
* lpg.c
* mediam_filter.c

## mavlink/
* mav_command.c
* mav_mission.c
* mav_param.c
* mav_parser.c
* mav_publisher.c
* mav_trajectory.c

## param/
* common_list.c
* sys_param.c

## perf/
* perf.c

## radio_events/
* multirotor_rc.h

## shell/
* quadshell.c
* shell_cmds.c

## state_estimator/
### ahrs/
處理madgwick filter 等等姿態資訊

### ins/
- 慣性導航系統，比ahrs上層，可以加上 GPS, height sensor 等等一起fuse。
- 一般功能：
    - 取得 attitude
        - AHRS_ESKF
        - AHRS_COMPLEMENTARY FILTER
        - AHRS_MADWICK_FILTER
        - AHRS_OPTITRACK
- 延伸功能：
    - fuse position (但是我們現在都直接拿 optitrack)

#### dymmy_sensors.c
#### geographic_transform.c
#### ins_comp_filter.c
#### ins_eskf.c
#### ins_sensor_sync.c
#### ins.c (聖文學長論文有很多切換sensor的部份)
- ins_init
    - 初始化 eskf / complementary filter

- 拿 position , velocity, attitude
```c=
/* raw position getters */
void ins_get_raw_position_enu(float *pos);
float ins_get_raw_position_enu_x(void);
float ins_get_raw_position_enu_y(void);
float ins_get_raw_position_enu_z(void);
```
```c=
/* raw velocity getters */
void ins_get_raw_velocity_enu(float *vel);
float ins_get_raw_velocity_enu_x(void);
float ins_get_raw_velocity_enu_y(void);
float ins_get_raw_velocity_enu_z(void);
```
```c=
/* ins fused position getters */
void ins_get_fused_position_enu(float *pos);
float ins_get_fused_position_enu_x(void);
float ins_get_fused_position_enu_y(void);
float ins_get_fused_position_enu_z(void);
```
```c=
/* ins fused velocity getters */
void ins_get_fused_velocity_enu(float *vel);
float ins_get_fused_velocity_enu_x(void);
float ins_get_fused_velocity_enu_y(void);
float ins_get_fused_velocity_enu_z(void);
```
```c=
/* ins ahrs attitude getters */
void ins_ahrs_get_attitude_euler_angles(float *roll, float *pitch, float *yaw);
void ins_ahrs_get_attitude_quaternion(float *q);
void ins_ahrs_get_rotation_matrix_b2i(float **R_b2i);
void ins_ahrs_get_rotation_matrix_i2b(float **R_i2b);
```

### interface/
#### system_state.c
### misc/
- 太空計畫（閒置）
### sensor_switching/

## tasks/


### debug_link_task

- 一次只儲存一種debug資料
- 傳輸無人機上的計算資料，並透過地面站接收程式畫成表格，並可以存成excel格式供開發人員分析使用。
- ahrs -> 姿態
- ins -> 位置
- raw date -> 從sensor拿到的原始數據



num 為傳輸數據個數（不含header）

debug function name | num | 說明 | 定義處
---|---|---|---|
send_imu_debug_message|17|加速度三軸raw data/濾波後三軸加速度/gyro 三軸raw data/濾波後gyro/溫度|[flight_ctrl_task.c]()
send_compass_debug_message|5|磁力計三軸 raw data / 強度 / 更新頻率|[compass.c]()
send_attitude_euler_debug_message|3|姿態的roll pitch yaw|[flight_ctrl_task.c]()
send_attitude_quaternion_debug_message|4|姿態的quaternion|[flight_ctrl_task.c]()
send_attitude_imu_debug_message|9|姿態的roll pitch yaw / 濾波後三軸加速度 / gyro|[flight_ctrl_task.c]()
send_pid_debug_message|6|pid 誤差與誤差微分/p,i,d三個分別數值/最終計算數值|[multirotor_pid_ctrl.c]()
send_motor_debug_message|4|4個馬達的pwm pulse|[multirotor_pid_ctrl.c]()
send_optitrack_position_debug_message|3|xyz from optitrack (ENU)|[optitrack.c]()
send_optitrack_quaternion_debug_message|4|quaternion from optitrack|[optitrack.c]()
send_optitrack_velocity_debug_message|6|vel (ENU)/ 濾波後vel|[optitrack.c]()
send_geometry_ctrl_debug|12|pitch roll yaw error/ wx wy wz error / feedback_moments / feedfoward_moments|[multirotor_geometry_ctrl.c]()
send_geometry_tracking_ctrl_debug|3|x y z 追蹤位置誤差|[multirotor_geometry_ctrl.c]()
send_uav_dynamics_debug|6|m / m_rot |[multirotor_geometry_ctrl.c]()
send_free_fall_debug_message|2|加速度 norm 值 / 是否為自由落體的狀態 |[free_fall.c]()
send_barometer_debug_message|5|barometer更新頻率 / 壓力值（bar）/ ms5611 raw temperature / ms5611 relative altitude (m) / ms5611 low pass filtered relative altitude rate (m/s) |[ms5611.c]()
send_alt_est_debug_message|4|估測器的z / optitrack的z / 估測器的vz / optitrack的vz|[debug_msg.c]()
send_ins_sensor_debug_message|15|濾波後三軸加速度 / gyro 三軸raw data/ 磁力計 三軸 raw data / 經度 / 緯度 / gps 高度 / 三軸速度 from gps |[debug_msg.c]()
send_ins_sensor_debug_message （有定義 SELECT_HEIGHT_SENSOR ）|17|濾波後三軸加速度 / gyro 三軸raw data/ 磁力計 三軸 raw data / 經度 / 緯度 / gps 高度 / 三軸速度 from gps / height / height_velocity |[debug_msg.c]()
send_ins_raw_position_debug_message|3|raw pos x y z |[debug_msg.c]()
send_ins_fusion_debug_message|15|現在時間/satellite_num/gps 更新時間/pos raw x y z/ pos fused x y z / vel raw x y z / vel fused x y z|[debug_msg.c]()
send_ahrs_compass_quality_check_debug_message|8|磁力計三軸raw data / 磁力計強度 / 更新頻率 / compass 可否信賴 / compass yaw rate / ahrs yaw rate |[ahrs.c]()
send_ins_eskf1_covariance_matrix_debug_message|9|EKF covariance_matrix P 對角線數值|[ins_esfk.c]()
send_vins_mono_position_debug_message|3|vins mono 三軸位置|[vins_mono.c]()
send_vins_mono_quaternion_debug_message|4|vins mono quaternion|[vins_mono.c]()
send_vins_mono_velocity_debug_message|7|三軸速度 raw data / 濾波後三軸速度 / vins mono 更新頻率|[vins_mono.c]()
send_gps_accuracy_debug_message|3|現在時間 / 水平精度 / 垂直精度|[debug_msg.c]()
send_rangefinder_debug_message|3|-|[lidar_lite.c]()
send_ins_eskf_correct_freq_debug_message|4|sensor 更新頻率 依序為: mag / baro / rangefinder / gps |[ins_esfk.c]()
send_optitrack_vio_debug_message|13|現在時間 / optitrack 三軸位置 / vio 三軸位置 / optitrack roll pitch yaw / vio roll pitch yaw |[debug_msg.c]()
send_gnss_ins_cov_norm_debug_message|2|gps position uncertainty / ins_eskf covariance norm|[debug_msg.c]()



### flight_ctrl_task
飛控主程式，接收遙控器以及sensor資訊
![](https://i.imgur.com/DmIrfPu.png)

- task_flight_ctrl()

    1. controller init 
    ```c=
    #if (SELECT_CONTROLLER == QUADROTOR_USE_PID)
        multirotor_pid_controller_init();
    #elif (SELECT_CONTROLLER == QUADROTOR_USE_GEOMETRY)
        geometry_ctrl_init();
    #endif
    ```
    2. sensor + actuator initialization and check
        - imu 是否初始化
        - calibration mode 
        - rc safety : 遙控器安全鎖
        - esc_range_calibration : 電變校正
        - motor_init : 給馬達初始pwm
        - barometer compass  
        - arhs_init & ins_init : 姿態估測初始化
        
    
    3. control loop 
        - 接收位置資訊
        ```c=
        #if (SELECT_NAVIGATION_DEVICE1 == NAV_DEV1_USE_GPS)
                ublox_m8n_gps_update();
        #elif (SELECT_NAVIGATION_DEVICE1 == NAV_DEV1_USE_OPTITRACK)
                optitrack_update();
        #endif
        ```
        - 從無人機上電腦接收資訊
        ```c=
        #if (SELECT_NAVIGATION_DEVICE2 == NAV_DEV2_USE_VINS_MONO)
                vins_mono_camera_trigger_20hz();
                vins_mono_send_imu_200hz();
                vins_mono_update();
        #endif
          ```  
         - 讀取遙控器資訊
         ```c=
         sbus_rc_read(&rc);
        ```
        
         - 更新姿態(放在ins.c 裡面一個叫做attitude的變數，之後controller會去拿它)
         ```c=
		/* attitude estimation */
		perf_start(PERF_AHRS_INS);
		{
			ins_state_estimate();
		}
		perf_end(PERF_AHRS_INS);
         ```
         - controller update
         ```c=
        #if (SELECT_CONTROLLER == QUADROTOR_USE_PID)
                    multirotor_pid_control(&rc);
        #elif (SELECT_CONTROLLER == QUADROTOR_USE_GEOMETRY)
                    multirotor_geometry_control(&rc);
        #endif
        ```

- RTOS排程
    - In main
    ```c=
    /* flight controller task (highest priority) */
    flight_controller_register_task("flight controller", 4096, tskIDLE_PRIORITY + 6);
    ```

    - Code
    ```c=
    void flight_controller_register_task(const char *task_name, 
            configSTACK_DEPTH_TYPE stack_size,UBaseType_t priority);
    void flight_ctrl_semaphore_handler(void);
    ```
- debug
    ```c=
    void send_imu_debug_message(debug_msg_t *payload);
    void send_attitude_euler_debug_message(debug_msg_t *payload);
    void send_attitude_quaternion_debug_message(debug_msg_t *payload);
    void send_attitude_imu_debug_message(debug_msg_t *payload);
    ```


### mavlink_task


### shell_task
可以即時跟無人機溝通，得知當前資訊，但無法儲存

## main.c

### perf_init()
- 紀錄執行時間
    - AHRS 
    - CONTROLLER
    - FLIGHT_CONTROL_LOOP
    - FLIGHT_CONTROL_TRIGGER_TIME
```c=
typedef struct {
	char *name;
	float start_time;
	float end_time;
	float exec_time;
} perf_t;

void perf_init(perf_t *perf_list, int list_size)
{
	perf_ptr = perf_list;
	perf_list_size = list_size;
}
void perf_start(int id)
{
	perf_ptr[id].start_time = get_sys_time_s();
}

void perf_end(int id)
{
	perf_ptr[id].end_time = get_sys_time_s();
	perf_ptr[id].exec_time = perf_ptr[id].end_time - perf_ptr[id].start_time;
}
```
- ins_sync_buffer_init()
    - 使用 xQueueCreate 來存 barometer, gps, compass, rangfinder 等等的資料
```c=
void ins_sync_buffer_init(void)
{
	ins_sync_barometer_queue =
	        xQueueCreate(INS_SYNC_BAROMETER_BUF_SIZE,
	                     sizeof(ins_sync_barometer_item_t));

	ins_sync_gps_queue =
	        xQueueCreate(INS_SYNC_GPS_BUF_SIZE, sizeof(ins_sync_gps_item_t));

	ins_sync_compass_queue =
	        xQueueCreate(INS_SYNC_COMPASS_BUF_SIZE, sizeof(ins_sync_compass_item_t));

	ins_sync_rangefinder_queue =
	        xQueueCreate(INS_SYNC_RANGEFINDER_BUF_SIZE, sizeof(ins_sync_rangefinder_item_t));

	sync_buffer_is_ready = true;
}
```
### board_init()
- 初始化sensor 參數
- whole picture
![](https://i.imgur.com/jmEzPSA.png)

### rgb_led_service_init()
- 定義燈號
- calibration_mode: purple
- rc_not_ready: red
- sensor_error: pink
- motor_lock: blue
- motor_unlock: red
- navigation on : light red
- navigation off: dark red

### flight_controller_register_task()
- 把 task_flight_ctrl 放進 scheduler中

```c=
void flight_controller_register_task(const char *task_name, configSTACK_DEPTH_TYPE stack_size,
                                     UBaseType_t priority)
{
	flight_ctrl_semphr = xSemaphoreCreateBinary();
	xTaskCreate(task_flight_ctrl, task_name, stack_size, NULL, priority, NULL);
}
```
### degub_link_register_task()
- 把 task_debug_link 放進 scheduler 中

```c=

void debug_link_register_task(const char *task_name, configSTACK_DEPTH_TYPE stack_size,
                              UBaseType_t priority)
{
	debug_link_task_semphr = xSemaphoreCreateBinary();
	xTaskCreate(task_debug_link, task_name, stack_size, NULL, priority, NULL);
}

```
### calibration_register_task()
- 把 task_calibration 放進 scheduler 中
```c=
void calibration_register_task(const char *task_name, configSTACK_DEPTH_TYPE stack_size,
                               UBaseType_t priority)
{
	xTaskCreate(task_calibration, task_name, stack_size, NULL, priority, &calib_task_handle);
	vTaskSuspend(calib_task_handle);
}

```

## proj_config.h
[Link](https://hackmd.io/Tei-eB1sSde6DJZwvV--1g)