【类 型】：factor 清除冗余的 设置飞机模式 函数
【原 因】： 【过 程】： 【影 响】：
This commit is contained in:
tk
parent
798be02055
commit
772e81baa0
@ -686,47 +686,6 @@ void FoodCube::mav_send_text(uint8_t severity, const char *text)
|
|||||||
SWrite(buf, len, mavlinkSerial);
|
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: 油门控制
|
* @description: 油门控制
|
||||||
* @param {uint16_t} chan[] 四个通道
|
* @param {uint16_t} chan[] 四个通道
|
||||||
|
|||||||
@ -70,7 +70,6 @@ public:
|
|||||||
void comm_receive(void (*pFun)(uint8_t));
|
void comm_receive(void (*pFun)(uint8_t));
|
||||||
void mav_mission_count(uint8_t taskcount);
|
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_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_channels_override(uint16_t chan[]);
|
||||||
void mav_send_text(uint8_t severity, const char *text);
|
void mav_send_text(uint8_t severity, const char *text);
|
||||||
|
|
||||||
|
|||||||
124
src/commser.cpp
124
src/commser.cpp
@ -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", "acceState", "getPlaneMode", "loadweight", "hookstatus", "position", "battCapacity", "homePosition", "statusText", "wpnavSpeed"};
|
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成员总数
|
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
|
||||||
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
|
||||||
/*触发发送 主题*/
|
/*触发发送 主题*/
|
||||||
@ -79,48 +79,28 @@ 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 == "setQuestState")
|
||||||
// { // 设置飞机状态
|
{ // 设置飞机状态
|
||||||
// /*
|
/*
|
||||||
// *其中topicPubMsg[10]既飞机状态的值
|
*其中topicPubMsg[10]既飞机任务状态的值
|
||||||
// *二进制00 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; // 转换值
|
String todoJson = value; // 转换值
|
||||||
// /* json 反序列化 */
|
/* json 反序列化 */
|
||||||
// DynamicJsonDocument doc(50);
|
DynamicJsonDocument doc(50);
|
||||||
// deserializeJson(doc, todoJson);
|
deserializeJson(doc, todoJson);
|
||||||
// JsonObject obj = doc.as<JsonObject>();
|
JsonObject obj = doc.as<JsonObject>();
|
||||||
// uint8_t n = obj["bit"]; // 状态位数
|
uint8_t n = obj["bit"]; // 状态位数
|
||||||
// uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
|
uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
|
||||||
// uint8_t count = obj["count"]; // 传过来
|
// 标记飞机状态
|
||||||
// // 解构val数组参数
|
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
|
||||||
// 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") // 解锁
|
else if (key == "unlock") // 解锁
|
||||||
{
|
{
|
||||||
|
|
||||||
@ -130,9 +110,13 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
|||||||
}
|
}
|
||||||
else if (key == "lock") // 加锁
|
else if (key == "lock") // 加锁
|
||||||
{
|
{
|
||||||
lock_or_unlock(true); // 加锁
|
// 油门
|
||||||
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
|
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
|
||||||
set_mode(ALT_HOLD); // 设置飞控为定高模式
|
fc.mav_channels_override(chan); // 控制油门
|
||||||
|
// 加锁
|
||||||
|
lock_or_unlock(true);
|
||||||
|
delay(2000); // 可以根据需要调整延迟时间，确保模式切换生效
|
||||||
|
set_mode(ALT_HOLD); // 设置飞控为定高模式
|
||||||
}
|
}
|
||||||
else if (key == "guidedMode") // 指点
|
else if (key == "guidedMode") // 指点
|
||||||
{
|
{
|
||||||
@ -162,32 +146,32 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
|||||||
set_mode(AUTO); // 设置飞控为定高模式
|
set_mode(AUTO); // 设置飞控为定高模式
|
||||||
// 油门
|
// 油门
|
||||||
delay(500);
|
delay(500);
|
||||||
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
|
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
|
||||||
set_rc_channels(channel_values, 4);
|
fc.mav_channels_override(chan); // 控制油门
|
||||||
}
|
}
|
||||||
else if (key == "loiterMode") // 悬停定点loiter模式
|
else if (key == "loiterMode") // 悬停定点loiter模式
|
||||||
{
|
{
|
||||||
// 油门
|
// 油门
|
||||||
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
|
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
|
||||||
set_rc_channels(channel_values, 4);
|
fc.mav_channels_override(chan); // 控制油门
|
||||||
delay(500);
|
delay(500);
|
||||||
// 切模式
|
// 切模式
|
||||||
set_mode(LOITER); // 设置飞控为定高模式
|
set_mode(LOITER); // 设置飞控为定高模式
|
||||||
// 油门
|
// 油门
|
||||||
delay(500);
|
delay(500);
|
||||||
set_rc_channels(channel_values, 4);
|
fc.mav_channels_override(chan); // 控制油门
|
||||||
}
|
}
|
||||||
else if (key == "holdAltMode") // 定高
|
else if (key == "holdAltMode") // 定高
|
||||||
{
|
{
|
||||||
// 油门
|
// 油门
|
||||||
uint16_t channel_values[] = {1500, 1500, 1500, 1500};
|
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 加解锁 油门到底
|
||||||
set_rc_channels(channel_values, 4);
|
fc.mav_channels_override(chan); // 控制油门
|
||||||
delay(500);
|
delay(500);
|
||||||
// 切模式
|
// 切模式
|
||||||
set_mode(ALT_HOLD);
|
set_mode(ALT_HOLD);
|
||||||
// 油门
|
// 油门
|
||||||
delay(500);
|
delay(500);
|
||||||
set_rc_channels(channel_values, 4);
|
fc.mav_channels_override(chan); // 控制油门
|
||||||
}
|
}
|
||||||
else if (key == "landMode") // 降落
|
else if (key == "landMode") // 降落
|
||||||
{
|
{
|
||||||
@ -230,7 +214,7 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
|||||||
{
|
{
|
||||||
refreshRequest(); // 刷新各种请求
|
refreshRequest(); // 刷新各种请求
|
||||||
}
|
}
|
||||||
else if (key == "resetState")
|
else if (key == "resetQuestState")
|
||||||
{ // 恢复飞机为初始状态
|
{ // 恢复飞机为初始状态
|
||||||
String todo = value; // 转换值
|
String todo = value; // 转换值
|
||||||
topicPubMsg[10] = todo; // 恢复初始状态
|
topicPubMsg[10] = todo; // 恢复初始状态
|
||||||
@ -932,34 +916,6 @@ 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 设置模式
|
* @brief 设置模式
|
||||||
* @param 模式
|
* @param 模式
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user