【类 型】：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);
|
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 writeSeq = -1; // 飞控反馈 需写入航点序列号
|
||||||
int8_t futureSeq = 0; // 记录将来要写入 航点序列号
|
int8_t futureSeq = 0; // 记录将来要写入 航点序列号
|
||||||
int8_t missionArkType = -1; // 航点写入是否成功
|
int8_t missionArkType = -1; // 航点写入是否成功
|
||||||
|
/*是否处于 加速度计校准状态*/
|
||||||
|
boolean isAnalyzeAcce = false; // 在接收开始校准时更改为true 这个状态匹配文本信息判断 校准步骤给前端
|
||||||
/*航点任务 送餐信息喊话*/
|
/*航点任务 送餐信息喊话*/
|
||||||
String questVoiceStr;
|
String questVoiceStr;
|
||||||
/*前端模拟遥控的油门通道*/
|
/*前端模拟遥控的油门通道*/
|
||||||
@ -73,6 +75,7 @@ public:
|
|||||||
void mav_send_text(uint8_t severity, const char *text);
|
void mav_send_text(uint8_t severity, const char *text);
|
||||||
|
|
||||||
void mav_send_command(mavlink_command_long_t &msg_cmd);
|
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);
|
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";
|
static const char *MOUDLENAME = "COMMSER";
|
||||||
|
|
||||||
/*项目对象*/
|
/*项目对象*/
|
||||||
char *ssid = "szMate40pro"; // wifi帐号
|
// char *ssid = "szMate40pro"; // wifi帐号
|
||||||
char *password = "63587839ab"; // wifi密码
|
// char *password = "63587839ab"; // wifi密码
|
||||||
// char* ssid = "szdot"; //wifi帐号
|
// char* ssid = "szdot"; //wifi帐号
|
||||||
// char* password = "63587839ab"; //wifi密码
|
// char* password = "63587839ab"; //wifi密码
|
||||||
// char *ssid = "flicube"; // wifi帐号
|
char *ssid = "flicube"; // wifi帐号
|
||||||
// char *password = "fxmf0622"; // wifi密码
|
char *password = "fxmf0622"; // wifi密码
|
||||||
// char *ssid = "fxmf_sc01"; // 4g wifi帐号
|
// char *ssid = "fxmf_sc01"; // 4g wifi帐号
|
||||||
// char *password = "12345678"; // 4g wifi密码
|
// char *password = "12345678"; // 4g wifi密码
|
||||||
char *mqttServer = "szdot.top"; // mqtt地址
|
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); // 创建项目对象
|
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象
|
||||||
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */
|
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */
|
||||||
// 登记 json成员名字
|
// 登记 json成员名字
|
||||||
// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:海拔高度 5:对地速度 6:卫星数量 7:定位状态 8:磁罗盘校准进度 9：磁罗盘校准是否成功 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，相对高度} 16:电池总容量 17:返航点 18:文本信息 19：返航速度
|
// 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"};
|
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成员总数
|
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
|
||||||
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
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>())
|
for (JsonPair kv : doc.as<JsonObject>())
|
||||||
{
|
{
|
||||||
key = kv.key().c_str(); // 获取键
|
key = kv.key().c_str(); // 获取键
|
||||||
value = kv.value(); // 获取值
|
value = kv.value(); // 获取值 取值时 还需指定类型
|
||||||
}
|
}
|
||||||
/*根据订阅内容 键值 做具体操作*/
|
/*根据订阅内容 键值 做具体操作*/
|
||||||
if (key == "questAss")
|
if (key == "questAss")
|
||||||
@ -79,108 +79,139 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
|||||||
String todo = value; // 转换值
|
String todo = value; // 转换值
|
||||||
writeRoute(todo); // 写入航点
|
writeRoute(todo); // 写入航点
|
||||||
}
|
}
|
||||||
else if (key == "setPlaneState")
|
// else if (key == "setPlaneState")
|
||||||
{ // 设置飞机状态
|
// { // 设置飞机状态
|
||||||
/*
|
// /*
|
||||||
*其中topicPubMsg[10]既飞机状态的值
|
// *其中topicPubMsg[10]既飞机状态的值
|
||||||
*二进制0000 0000 0000 0000
|
// *二进制00 0000
|
||||||
*第0位： 初始状态 &1
|
// *第0位： 初始状态 &1
|
||||||
*第1位： 正在写入航线 &2
|
// *第1位： 正在写入航线 &2
|
||||||
*第2位： 已经写入航点 &4
|
// *第2位： 已经写入航点 &4
|
||||||
*第3位： 正在解锁 &8
|
// *第3位： 正在解锁 &8
|
||||||
*第4位： 解锁成功 &16
|
// *第4位： 解锁成功 &16
|
||||||
*第5位： 执行航点任务 &32
|
// *第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; // 转换值
|
String todoJson = value; // 转换值
|
||||||
/* json 反序列化 */
|
DynamicJsonDocument doc(512); // 适当增加缓冲区大小
|
||||||
DynamicJsonDocument doc(500);
|
|
||||||
deserializeJson(doc, todoJson);
|
deserializeJson(doc, todoJson);
|
||||||
JsonObject obj = doc.as<JsonObject>();
|
JsonObject obj = doc.as<JsonObject>();
|
||||||
uint8_t n = obj["bit"]; // 状态位数
|
double alt = obj["alt"].as<double>(); // 传过来的高度值
|
||||||
uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
|
double lat = 0;
|
||||||
uint8_t count = obj["count"]; // 传过来
|
double lon = 0;
|
||||||
// 解构val数组参数
|
if (obj.containsKey("lat") && obj.containsKey("lon")) // 检查是否存在经纬度
|
||||||
uint16_t param[count];
|
|
||||||
for (int i = 0; i < count; i++)
|
|
||||||
{
|
{
|
||||||
param[i] = obj["param"][i];
|
lat = obj["lat"].as<double>();
|
||||||
|
lon = obj["lon"].as<double>();
|
||||||
}
|
}
|
||||||
// 标记飞机状态
|
// 飞至指定位置
|
||||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
|
fly_to_localtion(lat, lon, alt);
|
||||||
// 飞控执行
|
}
|
||||||
if (n == 3)
|
else if (key == "autoMode") // 自动auto模式
|
||||||
{ // 3操作飞机加解锁
|
{
|
||||||
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
|
// 切模式
|
||||||
fc.mav_channels_override(chan); // 控制油门
|
set_mode(AUTO); // 设置飞控为定高模式
|
||||||
fc.mav_set_mode(2); // 飞控设置成AltHold定高
|
// 油门
|
||||||
fc.mav_command(n, param);
|
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
|
else
|
||||||
{
|
{
|
||||||
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
|
fc.sendCommandAck(1, 1);
|
||||||
fc.mav_channels_override(chan); // 控制油门
|
|
||||||
}
|
}
|
||||||
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")
|
else if (key == "refreshRequest")
|
||||||
{
|
{
|
||||||
@ -191,30 +222,6 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
|||||||
String todo = value; // 转换值
|
String todo = value; // 转换值
|
||||||
topicPubMsg[10] = todo; // 恢复初始状态
|
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")
|
else if (key == "hookConteroller")
|
||||||
{ // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
|
{ // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
|
||||||
uint16_t todo = value;
|
uint16_t todo = value;
|
||||||
@ -450,7 +457,21 @@ void mavlink_receiveCallback(uint8_t c)
|
|||||||
topicPubMsg[8] = buf;
|
topicPubMsg[8] = buf;
|
||||||
}
|
}
|
||||||
break;
|
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 心跳
|
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
|
||||||
{
|
{
|
||||||
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
|
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
|
||||||
@ -614,6 +635,46 @@ void mavlink_receiveCallback(uint8_t c)
|
|||||||
{
|
{
|
||||||
topicPubMsg[18] = buf;
|
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;
|
break;
|
||||||
|
|
||||||
@ -833,8 +894,6 @@ void pubThread()
|
|||||||
String jsonString;
|
String jsonString;
|
||||||
serializeJson(doc, jsonString);
|
serializeJson(doc, jsonString);
|
||||||
fc.pubMQTTmsg("planeState", jsonString);
|
fc.pubMQTTmsg("planeState", jsonString);
|
||||||
/*更新4G网络测速ping值*/
|
|
||||||
// pingNetTest();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -859,3 +918,135 @@ void chanThread()
|
|||||||
{
|
{
|
||||||
// mav_channels_override(channels);
|
// 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 "FoodDeliveryBase.h"
|
||||||
#include "config.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 mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
|
||||||
extern void pubThread();
|
extern void pubThread();
|
||||||
extern void refreshRequest();
|
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 writeRoute(String topicStr);
|
||||||
extern void mavlink_receiveCallback(uint8_t c);
|
extern void mavlink_receiveCallback(uint8_t c);
|
||||||
extern const String topicPub[];
|
extern const String topicPub[];
|
||||||
|
|||||||
@ -42,7 +42,6 @@
|
|||||||
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
|
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
|
||||||
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
|
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
|
||||||
|
|
||||||
|
|
||||||
enum HookStatus
|
enum HookStatus
|
||||||
{
|
{
|
||||||
HS_UnInit, // 还未初始化
|
HS_UnInit, // 还未初始化
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user