【类 型】：factor 校准加速度计（未完成）
【原 因】： 【过 程】： 【影 响】：
This commit is contained in:
tk
parent
6ae6ff91b4
commit
b0a8659b0a
@ -751,3 +751,15 @@ void FoodCube::mav_send_command(mavlink_command_long_t &msg_cmd)
|
||||
// 通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
|
||||
void FoodCube::sendCommandAck(uint16_t command, uint8_t result)
|
||||
{
|
||||
mavlink_message_t msg; // MAVLink 信息
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送缓存
|
||||
// 打包 COMMAND_ACK 消息
|
||||
mavlink_msg_command_ack_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, command, result);
|
||||
// 将 MAVLink 消息转换为字节流
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
// 通过串口发送数据
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
|
||||
@ -26,6 +26,8 @@ public:
|
||||
int8_t writeSeq = -1; // 飞控反馈 需写入航点序列号
|
||||
int8_t futureSeq = 0; // 记录将来要写入 航点序列号
|
||||
int8_t missionArkType = -1; // 航点写入是否成功
|
||||
/*是否处于 加速度计校准状态*/
|
||||
boolean isAnalyzeAcce = false; // 在接收开始校准时更改为true 这个状态匹配文本信息判断 校准步骤给前端
|
||||
/*航点任务 送餐信息喊话*/
|
||||
String questVoiceStr;
|
||||
/*前端模拟遥控的油门通道*/
|
||||
@ -73,6 +75,7 @@ public:
|
||||
void mav_send_text(uint8_t severity, const char *text);
|
||||
|
||||
void mav_send_command(mavlink_command_long_t &msg_cmd);
|
||||
void sendCommandAck(uint16_t command, uint8_t result);
|
||||
|
||||
/*云台相机控制*/
|
||||
void udpSendToCamera(uint8_t *p_command, uint32_t len);
|
||||
|
||||
443
src/commser.cpp
443
src/commser.cpp
@ -14,12 +14,12 @@ extern void Hook_recovery();
|
||||
static const char *MOUDLENAME = "COMMSER";
|
||||
|
||||
/*项目对象*/
|
||||
char *ssid = "szMate40pro"; // wifi帐号
|
||||
char *password = "63587839ab"; // wifi密码
|
||||
// char *ssid = "szMate40pro"; // wifi帐号
|
||||
// char *password = "63587839ab"; // wifi密码
|
||||
// char* ssid = "szdot"; //wifi帐号
|
||||
// char* password = "63587839ab"; //wifi密码
|
||||
// char *ssid = "flicube"; // wifi帐号
|
||||
// char *password = "fxmf0622"; // wifi密码
|
||||
char *ssid = "flicube"; // wifi帐号
|
||||
char *password = "fxmf0622"; // wifi密码
|
||||
// char *ssid = "fxmf_sc01"; // 4g wifi帐号
|
||||
// char *password = "12345678"; // 4g wifi密码
|
||||
char *mqttServer = "szdot.top"; // mqtt地址
|
||||
@ -34,8 +34,8 @@ uint32_t udpServerPort = 37260; // 云台相机端口
|
||||
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象
|
||||
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */
|
||||
// 登记 json成员名字
|
||||
// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:海拔高度 5:对地速度 6:卫星数量 7:定位状态 8:磁罗盘校准进度 9：磁罗盘校准是否成功 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，相对高度} 16:电池总容量 17:返航点 18:文本信息 19：返航速度
|
||||
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "fixType", "completionPct", "reportCal", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"};
|
||||
// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:海拔高度 5:对地速度 6:卫星数量 7:定位状态 8:磁罗盘校准进度 9：磁罗盘校准是否成功 10:飞机状态 11:加速度计状态 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，相对高度} 16:电池总容量 17:返航点 18:文本信息 19：返航速度
|
||||
const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "fixType", "completionPct", "reportCal", "state", "acceState", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"};
|
||||
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
|
||||
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
||||
/*触发发送 主题*/
|
||||
@ -71,7 +71,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
||||
for (JsonPair kv : doc.as<JsonObject>())
|
||||
{
|
||||
key = kv.key().c_str(); // 获取键
|
||||
value = kv.value(); // 获取值
|
||||
value = kv.value(); // 获取值 取值时 还需指定类型
|
||||
}
|
||||
/*根据订阅内容 键值 做具体操作*/
|
||||
if (key == "questAss")
|
||||
@ -79,108 +79,139 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
||||
String todo = value; // 转换值
|
||||
writeRoute(todo); // 写入航点
|
||||
}
|
||||
else if (key == "setPlaneState")
|
||||
{ // 设置飞机状态
|
||||
/*
|
||||
*其中topicPubMsg[10]既飞机状态的值
|
||||
*二进制0000 0000 0000 0000
|
||||
*第0位： 初始状态 &1
|
||||
*第1位： 正在写入航线 &2
|
||||
*第2位： 已经写入航点 &4
|
||||
*第3位： 正在解锁 &8
|
||||
*第4位： 解锁成功 &16
|
||||
*第5位： 执行航点任务 &32
|
||||
*/
|
||||
// else if (key == "setPlaneState")
|
||||
// { // 设置飞机状态
|
||||
// /*
|
||||
// *其中topicPubMsg[10]既飞机状态的值
|
||||
// *二进制00 0000
|
||||
// *第0位： 初始状态 &1
|
||||
// *第1位： 正在写入航线 &2
|
||||
// *第2位： 已经写入航点 &4
|
||||
// *第3位： 正在解锁 &8
|
||||
// *第4位： 解锁成功 &16
|
||||
// *第5位： 执行航点任务 &32
|
||||
// */
|
||||
// String todoJson = value; // 转换值
|
||||
// /* json 反序列化 */
|
||||
// DynamicJsonDocument doc(50);
|
||||
// deserializeJson(doc, todoJson);
|
||||
// JsonObject obj = doc.as<JsonObject>();
|
||||
// uint8_t n = obj["bit"]; // 状态位数
|
||||
// uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
|
||||
// uint8_t count = obj["count"]; // 传过来
|
||||
// // 解构val数组参数
|
||||
// uint16_t param[count];
|
||||
// for (int i = 0; i < count; i++)
|
||||
// {
|
||||
// param[i] = obj["param"][i];
|
||||
// }
|
||||
// // 标记飞机状态
|
||||
// topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
|
||||
// // 飞控执行
|
||||
// if (n == 3)
|
||||
// { // 3操作飞机加解锁
|
||||
// uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
|
||||
// fc.mav_channels_override(chan); // 控制油门
|
||||
// fc.mav_set_mode(2); // 飞控设置成AltHold定高
|
||||
// fc.mav_command(n, param);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
|
||||
// fc.mav_channels_override(chan); // 控制油门
|
||||
// }
|
||||
// }
|
||||
else if (key == "unlock") // 解锁
|
||||
{
|
||||
|
||||
set_mode(ALT_HOLD); // 设置飞控为定高模式
|
||||
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
|
||||
lock_or_unlock(false); // 解锁
|
||||
}
|
||||
else if (key == "lock") // 加锁
|
||||
{
|
||||
lock_or_unlock(true); // 加锁
|
||||
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
|
||||
set_mode(ALT_HOLD); // 设置飞控为定高模式
|
||||
}
|
||||
else if (key == "guidedMode") // 指点
|
||||
{
|
||||
// 设置飞控为 Guided 模式
|
||||
set_mode(GUIDED);
|
||||
// 可以根据需要调整延迟时间，确保模式切换生效
|
||||
delay(2000);
|
||||
// JSON 反序列化 取经纬高
|
||||
String todoJson = value; // 转换值
|
||||
/* json 反序列化 */
|
||||
DynamicJsonDocument doc(500);
|
||||
DynamicJsonDocument doc(512); // 适当增加缓冲区大小
|
||||
deserializeJson(doc, todoJson);
|
||||
JsonObject obj = doc.as<JsonObject>();
|
||||
uint8_t n = obj["bit"]; // 状态位数
|
||||
uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
|
||||
uint8_t count = obj["count"]; // 传过来
|
||||
// 解构val数组参数
|
||||
uint16_t param[count];
|
||||
for (int i = 0; i < count; i++)
|
||||
double alt = obj["alt"].as<double>(); // 传过来的高度值
|
||||
double lat = 0;
|
||||
double lon = 0;
|
||||
if (obj.containsKey("lat") && obj.containsKey("lon")) // 检查是否存在经纬度
|
||||
{
|
||||
param[i] = obj["param"][i];
|
||||
lat = obj["lat"].as<double>();
|
||||
lon = obj["lon"].as<double>();
|
||||
}
|
||||
// 标记飞机状态
|
||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
|
||||
// 飞控执行
|
||||
if (n == 3)
|
||||
{ // 3操作飞机加解锁
|
||||
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
|
||||
fc.mav_channels_override(chan); // 控制油门
|
||||
fc.mav_set_mode(2); // 飞控设置成AltHold定高
|
||||
fc.mav_command(n, param);
|
||||
// 飞至指定位置
|
||||
fly_to_localtion(lat, lon, alt);
|
||||
}
|
||||
else if (key == "autoMode") // 自动auto模式
|
||||
{
|
||||
// 切模式
|
||||
set_mode(AUTO); // 设置飞控为定高模式
|
||||
// 油门
|
||||
delay(500);
|
||||
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
|
||||
set_rc_channels(channel_values, 4);
|
||||
}
|
||||
else if (key == "loiterMode") // 悬停定点loiter模式
|
||||
{
|
||||
// 油门
|
||||
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
|
||||
set_rc_channels(channel_values, 4);
|
||||
delay(500);
|
||||
// 切模式
|
||||
set_mode(LOITER); // 设置飞控为定高模式
|
||||
// 油门
|
||||
delay(500);
|
||||
set_rc_channels(channel_values, 4);
|
||||
}
|
||||
else if (key == "holdAltMode") // 定高
|
||||
{
|
||||
// 油门
|
||||
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
|
||||
set_rc_channels(channel_values, 4);
|
||||
delay(500);
|
||||
// 切模式
|
||||
set_mode(ALT_HOLD);
|
||||
// 油门
|
||||
delay(500);
|
||||
set_rc_channels(channel_values, 4);
|
||||
}
|
||||
else if (key == "landMode") // 降落
|
||||
{
|
||||
set_mode(LAND); // 切模式
|
||||
}
|
||||
else if (key == "rtlMode") // 返航
|
||||
{
|
||||
set_mode(RTL); // 切模式
|
||||
}
|
||||
else if (key == "resetCompass") // 校准磁罗盘
|
||||
{
|
||||
resetCompass();
|
||||
}
|
||||
else if (key == "initAcce")
|
||||
{
|
||||
uint32_t todo = value.as<uint32_t>(); // 转换值
|
||||
if (todo == 1)
|
||||
{
|
||||
initAcce();
|
||||
}
|
||||
else
|
||||
{
|
||||
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
|
||||
fc.mav_channels_override(chan); // 控制油门
|
||||
fc.sendCommandAck(1, 1);
|
||||
}
|
||||
if (n == 6)
|
||||
{ // 6测试起飞
|
||||
fc.mav_set_mode(4); // 飞控设置成Guided引导模式
|
||||
fc.mav_command(n, param); // 起飞
|
||||
}
|
||||
}
|
||||
else if (key == "autoMode")
|
||||
{ // 自动模式
|
||||
mavlink_command_long_t msg_cmd; // 命令结构体
|
||||
memset(&msg_cmd, 0, sizeof(msg_cmd)); // 初始化为0
|
||||
msg_cmd.command = MAV_CMD_NAV_WAYPOINT; // 使用适当的命令，这里示例用 MAV_CMD_NAV_WAYPOINT 代替自动模式
|
||||
msg_cmd.param1 = 0; // 根据需要设置其他参数
|
||||
msg_cmd.param2 = 0;
|
||||
msg_cmd.param3 = 0;
|
||||
msg_cmd.param4 = 0;
|
||||
msg_cmd.param5 = 0;
|
||||
msg_cmd.param6 = 0;
|
||||
msg_cmd.param7 = 0;
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
else if (key == "loiterMode")
|
||||
{ // 悬停 既定点
|
||||
mavlink_command_long_t msg_cmd; // 命令结构体
|
||||
memset(&msg_cmd, 0, sizeof(msg_cmd)); // 初始化为0
|
||||
msg_cmd.command = MAV_CMD_NAV_LOITER_TURNS; // 设置为悬停命令
|
||||
msg_cmd.param1 = 0; // 可选参数1 (具体值取决于命令的要求)
|
||||
msg_cmd.param2 = 0; // 可选参数2 (具体值取决于命令的要求)
|
||||
msg_cmd.param3 = 0; // 可选参数3 (具体值取决于命令的要求)
|
||||
msg_cmd.param4 = 0; // 可选参数4 (具体值取决于命令的要求)
|
||||
msg_cmd.param5 = 0; // 可选参数5 (具体值取决于命令的要求)
|
||||
msg_cmd.param6 = 0; // 可选参数6 (具体值取决于命令的要求)
|
||||
msg_cmd.param7 = 0; // 可选参数7 (具体值取决于命令的要求)
|
||||
fc.mav_send_command(msg_cmd); // 发送悬停命令
|
||||
}
|
||||
else if (key == "landMode")
|
||||
{ // 降落
|
||||
mavlink_command_long_t msg_cmd; // 命令结构体
|
||||
memset(&msg_cmd, 0, sizeof(msg_cmd)); // 初始化为0
|
||||
msg_cmd.command = MAV_CMD_NAV_LAND; // 设置为降落命令
|
||||
fc.mav_send_command(msg_cmd); // 发送降落命令
|
||||
}
|
||||
else if (key == "rtlMode")
|
||||
{ // 返航
|
||||
mavlink_command_long_t msg_cmd; // 命令结构体
|
||||
memset(&msg_cmd, 0, sizeof(msg_cmd)); // 初始化为0
|
||||
msg_cmd.command = MAV_CMD_NAV_RETURN_TO_LAUNCH; // #20 设置为返航命令
|
||||
fc.mav_send_command(msg_cmd); // 发送返航命令
|
||||
}
|
||||
else if (key == "resetCompass")
|
||||
{ // 校准磁罗盘
|
||||
mavlink_command_long_t msg_cmd; // 命令结构体
|
||||
memset(&msg_cmd, 0, sizeof(msg_cmd)); // 初始化为0
|
||||
msg_cmd.command = MAV_CMD_DO_START_MAG_CAL; // #42424 校准磁罗盘
|
||||
msg_cmd.param1 = 0; // 磁罗盘的bitmask（0表示所有）
|
||||
msg_cmd.param2 = 1; // 自动重试（0=不重试, 1=重试）
|
||||
msg_cmd.param3 = 0; // 是否自动保存（0=需要用户输入, 1=自动保存）
|
||||
msg_cmd.param4 = 0; // 延迟（秒）
|
||||
msg_cmd.param5 = 0; // 自动重启（0=用户重启, 1=自动重启）
|
||||
msg_cmd.param6 = 0; // 保留空参数
|
||||
msg_cmd.param7 = 0; // 保留空参数
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
else if (key == "refreshRequest")
|
||||
{
|
||||
@ -191,30 +222,6 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
||||
String todo = value; // 转换值
|
||||
topicPubMsg[10] = todo; // 恢复初始状态
|
||||
}
|
||||
else if (key == "chan1")
|
||||
{
|
||||
uint16_t todo = value; // 转换值
|
||||
fc.channels[0] = todo; // 恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); // 油门控制
|
||||
}
|
||||
else if (key == "chan2")
|
||||
{
|
||||
uint16_t todo = value; // 转换值
|
||||
fc.channels[1] = todo; // 恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); // 油门控制
|
||||
}
|
||||
else if (key == "chan3")
|
||||
{
|
||||
uint16_t todo = value; // 转换值
|
||||
fc.channels[2] = todo; // 恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); // 油门控制
|
||||
}
|
||||
else if (key == "chan4")
|
||||
{
|
||||
uint16_t todo = value; // 转换值
|
||||
fc.channels[3] = todo; // 恢复初始状态
|
||||
fc.mav_channels_override(fc.channels); // 油门控制
|
||||
}
|
||||
else if (key == "hookConteroller")
|
||||
{ // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
|
||||
uint16_t todo = value;
|
||||
@ -450,7 +457,21 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
topicPubMsg[8] = buf;
|
||||
}
|
||||
break;
|
||||
case MAVLINK_MSG_ID_COMMAND_ACK: // #77 处理 COMMAND_ACK 消息
|
||||
{
|
||||
mavlink_command_ack_t ack;
|
||||
mavlink_msg_command_ack_decode(&msg, &ack);
|
||||
|
||||
// 飞控接收到 校准加速度计命令
|
||||
if (ack.command == MAV_CMD_PREFLIGHT_CALIBRATION)
|
||||
{
|
||||
if (ack.result == 0)
|
||||
{
|
||||
fc.isAnalyzeAcce = true; // 标记进入 校准状态 此时匹配日志
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
|
||||
{
|
||||
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
|
||||
@ -614,6 +635,46 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
{
|
||||
topicPubMsg[18] = buf;
|
||||
}
|
||||
|
||||
// 解析加速度计校准状态
|
||||
if (fc.isAnalyzeAcce)
|
||||
{
|
||||
// 根据不同的校准状态更新 topicPubMsg[11]，并将结果发送到前端
|
||||
if (strstr(buf, "Place vehicle level and press any key") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "level";
|
||||
}
|
||||
else if (strstr(buf, "Place vehicle on its LEFT side and press any key") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "left";
|
||||
}
|
||||
else if (strstr(buf, "Place vehicle on its RIGHT side and press any key") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "right";
|
||||
}
|
||||
else if (strstr(buf, "Place vehicle nose DOWN and press any key") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "down";
|
||||
}
|
||||
else if (strstr(buf, "Place vehicle nose UP and press any key") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "up";
|
||||
}
|
||||
else if (strstr(buf, "Place vehicle on its BACK and press any key") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "back";
|
||||
}
|
||||
else if (strstr(buf, "Calibration successful") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "successful";
|
||||
fc.isAnalyzeAcce = false; // 结束校准状态
|
||||
}
|
||||
else if (strstr(buf, "Calibration FAILED") != nullptr)
|
||||
{
|
||||
topicPubMsg[11] = "failed";
|
||||
fc.isAnalyzeAcce = false; // 结束校准状态
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
@ -833,8 +894,6 @@ void pubThread()
|
||||
String jsonString;
|
||||
serializeJson(doc, jsonString);
|
||||
fc.pubMQTTmsg("planeState", jsonString);
|
||||
/*更新4G网络测速ping值*/
|
||||
// pingNetTest();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -859,3 +918,135 @@ void chanThread()
|
||||
{
|
||||
// mav_channels_override(channels);
|
||||
}
|
||||
|
||||
/**
|
||||
* @description:油门控制
|
||||
* @param: channel_values 油门值 ps:{1500,1500,1100,1500}
|
||||
* @param: num_channels 遍历RC通道 默认4个通道
|
||||
*/
|
||||
void set_rc_channels(uint16_t channel_values[], uint8_t num_channels = 4)
|
||||
{
|
||||
// 检查通道值数组的大小是否足够
|
||||
if (num_channels > 8)
|
||||
{
|
||||
// MAVLink 通道最大支持 8 个通道，如果有更多通道，请调整或验证
|
||||
num_channels = 8;
|
||||
}
|
||||
|
||||
// 遍历所有 RC 通道
|
||||
for (uint8_t channel = 1; channel <= num_channels; ++channel)
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_DO_SET_SERVO; // 使用伺服设置命令
|
||||
msg_cmd.param1 = static_cast<float>(channel); // 通道编号
|
||||
msg_cmd.param2 = static_cast<float>(channel_values[channel - 1]); // 设置的值
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 设置模式
|
||||
* @param 模式
|
||||
*/
|
||||
void set_mode(FlightMode mode)
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_DO_SET_MODE;
|
||||
msg_cmd.param1 = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
||||
msg_cmd.param2 = mode;
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 加解锁
|
||||
* @param islock true加锁 false解锁
|
||||
*/
|
||||
void lock_or_unlock(bool islock)
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
|
||||
if (islock)
|
||||
{
|
||||
msg_cmd.param1 = 0;
|
||||
msg_cmd.param2 = 21196;
|
||||
}
|
||||
else
|
||||
{
|
||||
msg_cmd.param1 = 1;
|
||||
msg_cmd.param2 = 0;
|
||||
}
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 飞到指定位置
|
||||
* @param lat 维度
|
||||
* @param lon 经度
|
||||
* @param alt 高度
|
||||
*/
|
||||
void fly_to_localtion(double lat, double lon, double alt)
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_NAV_TAKEOFF;
|
||||
msg_cmd.param1 = 0;
|
||||
msg_cmd.param2 = 0;
|
||||
msg_cmd.param3 = 0;
|
||||
msg_cmd.param4 = 0;
|
||||
msg_cmd.param5 = lat;
|
||||
msg_cmd.param6 = lon;
|
||||
msg_cmd.param7 = alt;
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 校准磁罗盘
|
||||
*/
|
||||
void resetCompass()
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_DO_START_MAG_CAL;
|
||||
msg_cmd.param1 = 0;
|
||||
msg_cmd.param2 = 1;
|
||||
msg_cmd.param3 = 0;
|
||||
msg_cmd.param4 = 0;
|
||||
msg_cmd.param5 = 0;
|
||||
msg_cmd.param6 = 0;
|
||||
msg_cmd.param7 = 0;
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 发起校准
|
||||
*/
|
||||
void initAcce()
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_PREFLIGHT_CALIBRATION; // 校准
|
||||
msg_cmd.param1 = 0; // Gyro Temperature calibration
|
||||
msg_cmd.param2 = 0; // Magnetometer calibration
|
||||
msg_cmd.param3 = 0; // Ground Pressure calibration
|
||||
msg_cmd.param4 = 0; // Remote Control calibration
|
||||
msg_cmd.param5 = 1; // 1:加速度计
|
||||
msg_cmd.param6 = 0; // Compass/Motor or Airspeed calibration
|
||||
msg_cmd.param7 = 0; // ESC or Baro calibration
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
// 发送校准命令
|
||||
fc.mav_send_command(msg_cmd);
|
||||
}
|
||||
@ -6,9 +6,29 @@
|
||||
#include "FoodDeliveryBase.h"
|
||||
#include "config.h"
|
||||
|
||||
// 枚举，表示飞行模式
|
||||
enum FlightMode
|
||||
{
|
||||
STABILIZE = 0, // 稳定模式
|
||||
ACRO = 1, // 特技模式
|
||||
ALT_HOLD = 2, // 高度保持模式
|
||||
AUTO = 3, // 自动模式
|
||||
GUIDED = 4, // 指点模式
|
||||
LOITER = 5, // 悬停模式
|
||||
RTL = 6, // 自动返航模式
|
||||
LAND = 9, // 降落模式
|
||||
POSHOLD = 16, // 位置保持模式
|
||||
};
|
||||
extern void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
|
||||
extern void pubThread();
|
||||
extern void refreshRequest();
|
||||
extern void set_rc_channels(uint16_t channel_values[], uint8_t num_channels);
|
||||
extern void set_mode(FlightMode mode);
|
||||
extern void resetCompass();
|
||||
extern void initAcce();
|
||||
extern void initAccea();
|
||||
extern void lock_or_unlock(bool islock);
|
||||
extern void fly_to_localtion(double lat, double lon, double alt);
|
||||
extern void writeRoute(String topicStr);
|
||||
extern void mavlink_receiveCallback(uint8_t c);
|
||||
extern const String topicPub[];
|
||||
|
||||
@ -42,7 +42,6 @@
|
||||
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
|
||||
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
|
||||
|
||||
|
||||
enum HookStatus
|
||||
{
|
||||
HS_UnInit, // 还未初始化
|
||||
|
||||
Loading…
Reference in New Issue
Block a user