diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 7d155f3..6b874b4 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -742,6 +742,12 @@ void FoodCube::mav_channels_override(uint16_t chan[]) SWrite(buf, len, mavlinkSerial); } +/** + * @brief 发送MAVLink命令 + * 该函数通过串口发送一个MAVLink `COMMAND_LONG` 命令。此命令可以用于向飞控发送各种控制命令, + * 包括但不限于模式切换、校准、任务启动等。 + * @param msg_cmd 参考传递的 `mavlink_command_long_t` 类型的 MAVLink 命令结构体。 + */ void FoodCube::mav_send_command(mavlink_command_long_t &msg_cmd) { mavlink_message_t msg; // mavlink协议信息(msg) @@ -752,6 +758,18 @@ void FoodCube::mav_send_command(mavlink_command_long_t &msg_cmd) SWrite(buf, len, mavlinkSerial); } +/** + * @brief 发送 COMMAND_ACK 消息 + * 该函数用于发送一个 `COMMAND_ACK` 消息,以响应飞控发送的命令确认请求。该消息通常用于通知飞控某个命令 + * 是否已被接收和处理,以及处理的结果状态。 + * @param command 表示要响应的命令 ID,通常与接收到的命令相对应。 + * @param result 表示命令的处理结果。常用值包括: + * - 0: MAV_RESULT_ACCEPTED (命令成功接受) + * - 1: MAV_RESULT_TEMPORARILY_REJECTED (命令被暂时拒绝) + * - 2: MAV_RESULT_DENIED (命令被拒绝) + * - 3: MAV_RESULT_UNSUPPORTED (命令不受支持) + * - 4: MAV_RESULT_FAILED (命令执行失败) + */ void FoodCube::sendCommandAck(uint16_t command, uint8_t result) { mavlink_message_t msg; // MAVLink 信息 diff --git a/src/commser.cpp b/src/commser.cpp index 81f60f2..4b9fc18 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -206,11 +206,11 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) uint32_t todo = value.as(); // 转换值 if (todo == 1) { - initAcce(); + initAcce(); // 发起校准 } else { - fc.sendCommandAck(1, 1); + fc.sendCommandAck(1, 1); // 摆好校准 } } else if (key == "refreshRequest")