mavlink === ###### tags: `code documentation` [TOC] # main.c ```c= mavlink_tx_register_task("mavlink publisher", 1024, tskIDLE_PRIORITY + 3); mavlink_rx_register_task("mavlink receiver", 2048, tskIDLE_PRIORITY + 3); ``` ## mavlink_task.c - mavlink_queue: - mavlink_status_t (mavlink自定義) ```c= typedef struct __mavlink_status { uint8_t msg_received; ///< Number of received messages uint8_t buffer_overrun; ///< Number of buffer overruns uint8_t parse_error; ///< Number of parse errors mavlink_parse_state_t parse_state; ///< Parsing state machine uint8_t packet_idx; ///< Index in current packet uint8_t current_rx_seq; ///< Sequence number of last packet received uint8_t current_tx_seq; ///< Sequence number of last packet sent uint16_t packet_rx_success_count; ///< Received packets uint16_t packet_rx_drop_count; ///< Number of packet drops uint8_t flags; ///< MAVLINK_STATUS_FLAG_* uint8_t signature_wait; ///< number of signature bytes left to receive struct __mavlink_signing *signing; ///< optional signing state struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps } mavlink_status_t; ``` - mavlink_queue_item_t(mavlink_task.h定義) ```c= typedef struct { mavlink_message_t mav_msg; } mavlink_queue_item_t; ``` - mavlink_message_t (mavlink自定義) ```c= MAVPACKED( typedef struct __mavlink_message { uint16_t checksum; ///< sent at end of packet uint8_t magic; ///< protocol magic marker uint8_t len; ///< Length of payload uint8_t incompat_flags; ///< flags that must be understood uint8_t compat_flags; ///< flags that can be ignored if not understood uint8_t seq; ///< Sequence of packet uint8_t sysid; ///< ID of message sender system/aircraft uint8_t compid; ///< ID of the message sender component uint32_t msgid:24; ///< ID of message in payload uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; uint8_t ck[2]; ///< incoming checksum bytes uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; }) mavlink_message_t; ``` - mavlink_recept_record_queue(mavlink_task.h定義) - msg 形式 還有 收到時間 - mavlink_recept_record_item_t ```c= typedef struct { int recvd_msg_id; float recvd_time; } mavlink_recpt_record_item_t; ``` - Register task ```c= void mavlink_tx_register_task(const char *task_name, configSTACK_DEPTH_TYPE stack_size, UBaseType_t priority) { mavlink_calib_status_text_queue = xQueueCreate(5, sizeof(mavlink_calib_status_text_item_t)); xTaskCreate(mavlink_tx_task, task_name, stack_size, NULL, priority, NULL); } void mavlink_rx_register_task(const char *task_name, configSTACK_DEPTH_TYPE stack_size, UBaseType_t priority) { mavlink_queue = xQueueCreate(MAVLINK_QUEUE_SIZE, sizeof(mavlink_queue_item_t)); mavlink_recpt_record_queue = xQueueCreate(50, sizeof(mavlink_recpt_record_item_t)); xTaskCreate(mavlink_rx_task, task_name, stack_size, NULL, priority, NULL); } ``` ## 全域變數 ```c= QueueHandle_t mavlink_queue; QueueHandle_t mavlink_calib_status_text_queue; QueueHandle_t mavlink_recpt_record_queue; mavlink_message_t mavlink_recpt_msg; ``` ## TX task 1. 如果有收到 mavlink_msg 則 received_mavlink_msg = 1, 如果資料有問題則為 0，實際操作為uart3拿到的raw message 會解碼後放到 mavlink_recpt_msg(mavlink_message_t 結構)，並且檢查 2. 如果資料確定沒問題，就放入 mavlink_queue 3. 如果需要debug，創造一個mavlink_recept_record_item 結構再去放入mavlink_recpt_record_queue中 ## RX task (uart3_puts -> xbee) ### 1. 發送資料 - 1hz - heart beat(**確認無人機還活著**) - MAV TYPE - MAV_MODE_FLAG - MAV_AUTOPILOT_MODE - MAV_STATE - system status - battery - sys_id - 10hz - for gps(not use right now) - 50hz - send_mavlink_attitude_quaternion() - send current quaternion, RPY - send_mavlink_rc_channels() - send 遙控器資訊 - sned_mavlink_local_position_ned() - send position and velocity - Everytime - 如果在 AUTOPILOT_TRAJECTORY_FOLLOWING_MODE，回傳當前 PVA 以及 desire PVA ### 2. Handler - 我們有一個parse_mavlink_received_msg, 可以跑一個for loop 去檢查來的資料是哪一個id，接著啟動相對應的handler。 - 調整參數的部分，其中 handler 有一句 ```c= save_param_list_to_flash() ``` 可以將參數燒到 flash 裡面，這樣我們用mavlink調整完參數就不用每次再重新燒錄一遍了! - 一堆handler ```c= /* register mavlink msg id to the handler function */ struct mavlink_parser_item cmd_list[] = { /* common mavlink messages */ MAV_CMD_DEF(mav_set_mode, 11), MAV_CMD_DEF(mav_mission_request_list, 43), MAV_CMD_DEF(mav_mission_count, 44), MAV_CMD_DEF(mav_mission_item_int, 73), MAV_CMD_DEF(mav_mission_request_int, 40), MAV_CMD_DEF(mav_mission_ack, 47), MAV_CMD_DEF(mav_mission_clear_all, 45), //MAV_CMD_DEF(mav_mission_set_current, 41), MAV_CMD_DEF(mav_command_long, 76), MAV_CMD_DEF(mav_param_request_list, 21), MAV_CMD_DEF(mav_param_request_read, 20), // 參數 MAV_CMD_DEF(mav_param_set, 23), /* extended mavlink messages */ // 路徑規劃 MAV_CMD_DEF(mav_polynomial_trajectory_write, 11000), MAV_CMD_DEF(mav_polynomial_trajectory_cmd, 11001), MAV_CMD_DEF(mav_polynomial_trajectory_item, 11003) }; ```