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)