Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

【类 型】：factor 清除冗余的 设置飞机模式 函数

Browse Source 
【原  因】：
【过  程】：
【影  响】：
This commit is contained in:
tk 2024-09-02 17:41:18 +08:00
parent 798be02055
commit 772e81baa0
3 changed files with 40 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/FoodDeliveryBase.cpp
View File

@ -686,47 +686,6 @@ void FoodCube::mav_send_text(uint8_t severity, const char *text)
SWrite(buf, len, mavlinkSerial);
}
/**
* @description: 飞控 控制
* @param {uint8_t} controlType 3:加解锁模式控制
* @param {uint16_t} param[]
*/
void FoodCube::mav_command(uint8_t controlType, uint16_t param[])
{
mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包
mavlink_command_long_t cmd;
cmd.target_system = 1;
cmd.target_component = 1;
cmd.confirmation = 0;
if (controlType == 3)
{ // 飞机加解锁
cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
cmd.param1 = param[0]; // 0:加锁 1：解锁
cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁
}
else if (controlType == 6)
{ // 测试起飞
float p = (float)param[0];
cmd.command = MAV_CMD_NAV_TAKEOFF;
cmd.param1 = 0;
cmd.param2 = 0;
cmd.param3 = 0;
cmd.param4 = 0;
cmd.param5 = 0;
cmd.param6 = 0;
cmd.param7 = p; // 起飞高度
}
else if (controlType == 8)
{ // 降落
cmd.command = MAV_CMD_NAV_LAND;
}
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd);
int len = mavlink_msg_to_send_buffer(buf, &msg);
// 通过串口发送
SWrite(buf, len, mavlinkSerial);
}
/**
* @description: 油门控制
* @param {uint16_t} chan[] 四个通道

1
src/FoodDeliveryBase.h
View File

@ -70,7 +70,6 @@ public:
void comm_receive(void (*pFun)(uint8_t));
void mav_mission_count(uint8_t taskcount);
void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z);
void mav_command(uint8_t controlType, uint16_t param[]);
void mav_channels_override(uint16_t chan[]);
void mav_send_text(uint8_t severity, const char *text);

124
src/commser.cpp
View File

@ -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", "acceState", "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", "questState", "acceState", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"};
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
/*触发发送 主题*/
@ -79,48 +79,28 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
String todo = value; // 转换值
writeRoute(todo); // 写入航点
}
// 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 == "setQuestState")
{ // 设置飞机状态
/*
*其中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
// 标记飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
}
else if (key == "unlock") // 解锁
{
@ -130,9 +110,13 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
}
else if (key == "lock") // 加锁
{
lock_or_unlock(true); // 加锁
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
set_mode(ALT_HOLD); // 设置飞控为定高模式
// 油门
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
fc.mav_channels_override(chan); // 控制油门
// 加锁
lock_or_unlock(true);
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
set_mode(ALT_HOLD); // 设置飞控为定高模式
}
else if (key == "guidedMode") // 指点
{
@ -162,32 +146,32 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
set_mode(AUTO); // 设置飞控为定高模式
// 油门
delay(500);
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
set_rc_channels(channel_values, 4);
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
fc.mav_channels_override(chan); // 控制油门
}
else if (key == "loiterMode") // 悬停定点loiter模式
{
// 油门
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
set_rc_channels(channel_values, 4);
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
fc.mav_channels_override(chan); // 控制油门
delay(500);
// 切模式
set_mode(LOITER); // 设置飞控为定高模式
// 油门
delay(500);
set_rc_channels(channel_values, 4);
fc.mav_channels_override(chan); // 控制油门
}
else if (key == "holdAltMode") // 定高
{
// 油门
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
set_rc_channels(channel_values, 4);
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
fc.mav_channels_override(chan); // 控制油门
delay(500);
// 切模式
set_mode(ALT_HOLD);
// 油门
delay(500);
set_rc_channels(channel_values, 4);
fc.mav_channels_override(chan); // 控制油门
}
else if (key == "landMode") // 降落
{
@ -230,7 +214,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
{
refreshRequest(); // 刷新各种请求
}
else if (key == "resetState")
else if (key == "resetQuestState")
{ // 恢复飞机为初始状态
String todo = value; // 转换值
topicPubMsg[10] = todo; // 恢复初始状态
@ -932,34 +916,6 @@ 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 模式