【类 型】：fix
【原 因】：前端的“点飞”指令与起飞指令在后端均使用 TAKEOFF，导致行为不一致 【改 动】：将“点飞”命令逻辑由 TAKEOFF 改为使用 WAYPOINT，依据是否包含有效经纬度决定具体飞行指令 【过 程】： - 保留原有起飞逻辑，当未传入有效经纬度（0,0）时默认使用 TAKEOFF 起飞至指定高度 - 当传入有效经纬度时，使用 WAYPOINT 实现导航飞行 - 拆分 fly_to_location() 为 fly_to_location() 与 fly_to_altitude()，逻辑更清晰 【影 响】： - 实现点飞功能的正确性和可扩展性 - 避免重复起飞逻辑与误导性行为，提高系统稳定性和行为预期一致性
This commit is contained in:
air
parent
52c445f981
commit
3045d138b1
@ -1,5 +1,4 @@
|
||||
///MQTT以及Mavlink通讯控制
|
||||
|
||||
/// MQTT以及Mavlink通讯控制
|
||||
|
||||
#include "commser.h"
|
||||
#include "motocontrol.h"
|
||||
@ -162,23 +161,36 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
|
||||
{
|
||||
// 设置飞控为 Guided 模式
|
||||
set_mode(GUIDED);
|
||||
// 可以根据需要调整延迟时间，确保模式切换生效
|
||||
// 确保模式切换生效
|
||||
delay(2000);
|
||||
|
||||
// JSON 反序列化 取经纬高
|
||||
String todoJson = value; // 转换值
|
||||
DynamicJsonDocument doc(512); // 适当增加缓冲区大小
|
||||
String todoJson = value;
|
||||
DynamicJsonDocument doc(512);
|
||||
deserializeJson(doc, todoJson);
|
||||
JsonObject obj = doc.as<JsonObject>();
|
||||
double alt = obj["alt"].as<double>(); // 传过来的高度值
|
||||
|
||||
double alt = obj["alt"].as<double>(); // 高度值
|
||||
double lat = 0;
|
||||
double lon = 0;
|
||||
if (obj.containsKey("lat") && obj.containsKey("lon")) // 检查是否存在经纬度
|
||||
|
||||
if (obj.containsKey("lat") && obj.containsKey("lon"))
|
||||
{
|
||||
lat = obj["lat"].as<double>();
|
||||
lon = obj["lon"].as<double>();
|
||||
}
|
||||
// 飞至指定位置
|
||||
fly_to_localtion(lat, lon, alt);
|
||||
|
||||
addLogMessage("lat:" + String(lat) + " lon:" + String(lon) + " alt:" + String(alt)); // 日志
|
||||
|
||||
// 根据经纬度是否为0选择起飞或导航
|
||||
if (lat == 0.0 && lon == 0.0)
|
||||
{
|
||||
fly_to_altitude(alt);
|
||||
}
|
||||
else
|
||||
{
|
||||
fly_to_location(lat, lon, alt);
|
||||
}
|
||||
}
|
||||
else if (key == "autoMode") // 自动auto模式
|
||||
{
|
||||
@ -480,8 +492,8 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
// 没有激光高度直接退出
|
||||
if (rngalt_cm == 0)
|
||||
{
|
||||
//rngalt_cm == 500;
|
||||
// printf("exit rngalt_cm==0");
|
||||
// rngalt_cm == 500;
|
||||
// printf("exit rngalt_cm==0");
|
||||
addLogMessage("exit rngalt_cm==0，激光高度异常");
|
||||
// break;
|
||||
}
|
||||
@ -639,7 +651,7 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
topicPubMsg[12] = "稳定";
|
||||
topicPubMsg[12] = "自稳";
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
@ -988,7 +1000,7 @@ void pubThread()
|
||||
// 添加日志 到日志队列
|
||||
void addLogMessage(const String &msg)
|
||||
{
|
||||
portENTER_CRITICAL(&espMutex);
|
||||
portENTER_CRITICAL(&espMutex);
|
||||
|
||||
int nextTail = (logTail + 1) % LOG_QUEUE_SIZE;
|
||||
if (nextTail != logHead)
|
||||
@ -1068,22 +1080,44 @@ void lock_or_unlock(bool islock)
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 飞到指定位置
|
||||
* @param lat 维度
|
||||
* @param lon 经度
|
||||
* @param alt 高度
|
||||
* @brief 飞到指定位置（经纬度+高度），使用导航航点命令（WAYPOINT）
|
||||
* @param lat 纬度（单位：度）
|
||||
* @param lon 经度（单位：度）
|
||||
* @param alt 高度（单位：米）
|
||||
*/
|
||||
void fly_to_localtion(double lat, double lon, double alt)
|
||||
void fly_to_location(double lat, double lon, double alt)
|
||||
{
|
||||
mavlink_command_long_t msg_cmd = {};
|
||||
msg_cmd.command = MAV_CMD_NAV_WAYPOINT;
|
||||
msg_cmd.param1 = 0; // 等待时间，单位秒，0表示不停留直接飞
|
||||
msg_cmd.param2 = 0; // 接收半径，单位米，0表示默认值
|
||||
msg_cmd.param3 = 0; // 保留参数
|
||||
msg_cmd.param4 = 0; // 航向角，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 起飞到指定高度，使用起飞命令（TAKEOFF）
|
||||
* 一般用于地面起飞，alt为起飞高度
|
||||
* @param alt 高度（单位：米）
|
||||
*/
|
||||
void fly_to_altitude(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.param1 = 0; // 起飞时的俯仰角，0表示默认
|
||||
msg_cmd.param2 = 0; // 保留参数
|
||||
msg_cmd.param3 = 0; // 保留参数
|
||||
msg_cmd.param4 = 0; // 保留参数
|
||||
msg_cmd.param5 = 0; // 纬度，起飞时一般无效，设置为0
|
||||
msg_cmd.param6 = 0; // 经度，起飞时一般无效，设置为0
|
||||
msg_cmd.param7 = alt; // 起飞目标高度
|
||||
msg_cmd.target_system = 1;
|
||||
msg_cmd.target_component = 1;
|
||||
msg_cmd.confirmation = 0;
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
// 枚举，表示飞行模式
|
||||
enum FlightMode
|
||||
{
|
||||
STABILIZE = 0, // 稳定模式
|
||||
STABILIZE = 0, // 自稳模式
|
||||
ACRO = 1, // 特技模式
|
||||
ALT_HOLD = 2, // 高度保持模式
|
||||
AUTO = 3, // 自动模式
|
||||
@ -29,7 +29,8 @@ 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 fly_to_location(double lat, double lon, double alt);
|
||||
extern void fly_to_altitude(double alt);
|
||||
extern void writeRoute(String topicStr);
|
||||
extern void mavlink_receiveCallback(uint8_t c);
|
||||
extern const String topicPub[];
|
||||
|
||||
Loading…
Reference in New Issue
Block a user