【类 型】:feat 1校准加速度计函数 2.style 发送命令包成员函数加上注释说明

【原  因】:
【过  程】:接收到前端校准命令 执行校准 和一步 一步执行(两个函数发送命令帧)
【影  响】:
This commit is contained in:
tk 2024-08-30 18:53:24 +08:00
parent b0a8659b0a
commit dcbd67fb7f
2 changed files with 20 additions and 2 deletions

View File

@ -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 信息

View File

@ -206,11 +206,11 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
uint32_t todo = value.as<uint32_t>(); // 转换值
if (todo == 1)
{
initAcce();
initAcce(); // 发起校准
}
else
{
fc.sendCommandAck(1, 1);
fc.sendCommandAck(1, 1); // 摆好校准
}
}
else if (key == "refreshRequest")