From b0a8659b0a1981569637737abf60ebf33bdc0eef Mon Sep 17 00:00:00 2001 From: tk Date: Fri, 30 Aug 2024 12:37:45 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Afactor=20=E6=A0=A1=E5=87=86=E5=8A=A0=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E8=AE=A1=EF=BC=88=E6=9C=AA=E5=AE=8C=E6=88=90=EF=BC=89=20?= =?UTF-8?q?=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91=EF=BC=9A=20=E3=80=90?= =?UTF-8?q?=E8=BF=87=20=20=E7=A8=8B=E3=80=91=EF=BC=9A=20=E3=80=90=E5=BD=B1?= =?UTF-8?q?=20=20=E5=93=8D=E3=80=91=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/FoodDeliveryBase.cpp | 12 ++ src/FoodDeliveryBase.h | 3 + src/commser.cpp | 445 ++++++++++++++++++++++++++++----------- src/commser.h | 20 ++ src/config.h | 49 +++-- 5 files changed, 377 insertions(+), 152 deletions(-) diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index a332e9c..7d155f3 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -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); +} diff --git a/src/FoodDeliveryBase.h b/src/FoodDeliveryBase.h index 01c7f7f..a2dd6bb 100644 --- a/src/FoodDeliveryBase.h +++ b/src/FoodDeliveryBase.h @@ -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); diff --git a/src/commser.cpp b/src/commser.cpp index b946423..81f60f2 100644 --- a/src/commser.cpp +++ b/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()) { 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 - */ - String todoJson = value; // 转换值 - /* json 反序列化 */ - DynamicJsonDocument doc(500); + // 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(); + // 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; // 转换值 + DynamicJsonDocument doc(512); // 适当增加缓冲区大小 deserializeJson(doc, todoJson); JsonObject obj = doc.as(); - 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 lat = 0; + double lon = 0; + if (obj.containsKey("lat") && obj.containsKey("lon")) // 检查是否存在经纬度 { - param[i] = obj["param"][i]; + lat = obj["lat"].as(); + lon = obj["lon"].as(); } - // 标记飞机状态 - 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(); // 转换值 + 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(channel); // 通道编号 + msg_cmd.param2 = static_cast(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); +} \ No newline at end of file diff --git a/src/commser.h b/src/commser.h index ed0619a..745c464 100644 --- a/src/commser.h +++ b/src/commser.h @@ -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[]; diff --git a/src/config.h b/src/config.h index 56e1dc0..c94d459 100644 --- a/src/config.h +++ b/src/config.h @@ -3,8 +3,8 @@ // 定义公共结构，变量，硬件接口等 /// // -#define VERSION "0.90" //软件版本 -#define VERSION_HW 1 //硬件版本1:第一块硬件 2:目前版本 +#define VERSION "0.90" // 软件版本 +#define VERSION_HW 1 // 硬件版本1:第一块硬件 2:目前版本 // 硬件接口定义//////////////////////////// // 按钮 #define BTN_UP 23 // 收线开关 接线：BTN_UP---GND @@ -12,37 +12,36 @@ #define BTN_CT 21 // 到顶检测开关 #define BTN_TEST 18 // 测试开关 // 称重传感器- HX711 -#define LOADCELL_DOUT_PIN 13 //16 -#define LOADCELL_SCK_PIN 33 //17 +#define LOADCELL_DOUT_PIN 13 // 16 +#define LOADCELL_SCK_PIN 33 // 17 /////////////////////////////////////////// #define SERVO_PIN 14 // 锁定舵机PWM控制脚 ////LED #define LED_DATA_PIN 25 #if (VERSION_HW == 1) - // Moto-CAN //第一版本硬件参数---1号机使用 - #define MOTO_CAN_RX 26 - #define MOTO_CAN_TX 27 - #define WEIGHT_SCALE 165 // //A通道是165,B通道是41 - #define HX711_GAIN 128 +// Moto-CAN //第一版本硬件参数---1号机使用 +#define MOTO_CAN_RX 26 +#define MOTO_CAN_TX 27 +#define WEIGHT_SCALE 165 // //A通道是165,B通道是41 +#define HX711_GAIN 128 #else - // Moto-CAN //第二版本硬件参数---2号机使用 - #define MOTO_CAN_RX 27 //PCB画板需要，做了调整 - #define MOTO_CAN_TX 26 //PCB画板需要，做了调整 - #define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41 - #define HX711_GAIN 32 //减少零点漂移用B通道的感度 +// Moto-CAN //第二版本硬件参数---2号机使用 +#define MOTO_CAN_RX 27 // PCB画板需要，做了调整 +#define MOTO_CAN_TX 26 // PCB画板需要，做了调整 +#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41 +#define HX711_GAIN 32 // 减少零点漂移用B通道的感度 #endif -///serial1 +/// serial1 #define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_RX 18 ///// -//#define WEIGHT_SCALE 41 // //A通道是165,B通道是41 -#define TM_INSTORE_WEIGHT_DELAY 200 //200 // 入仓动力延时关闭时间 ms -#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms +// #define WEIGHT_SCALE 41 // //A通道是165,B通道是41 +#define TM_INSTORE_WEIGHT_DELAY 200 // 200 // 入仓动力延时关闭时间 ms +#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms #define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms - enum HookStatus { HS_UnInit, // 还未初始化 @@ -67,9 +66,9 @@ enum Initstatus IS_CheckWeightZero, // 检查称重传感器是否归0 IS_OK, // 4.所有初始化已经OK，可以正常操作 IS_Error // 5.初始化遇到错误 -} ; -const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线 -const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停 -const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态 -const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 -const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下 +}; +const uint16_t MAV_CMD_FC_HOOK_AUTODOWN = 40; // 飞控发的---自动放线 +const uint16_t MAV_CMD_FC_HOOK_PAUSE = 41; // 飞控发的---暂停 +const uint16_t MAV_CMD_FC_HOOK_STATUS = 42; // 发给飞控的状态 +const uint16_t MAV_CMD_FC_HOOK_RECOVERY = 43; // 飞控发的---收线 +const uint16_t MAV_CMD_FC_DESCENTSTART = 44; // 飞开始下降，需要调整相机向下