Food/FoodDelivery
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

mavlink 取消指针参数 在comm_receive函数内部声明

Browse Source
This commit is contained in:
AIR 2023-06-25 18:48:22 +08:00
parent d2c4d566fd
commit 3c16559578
1 changed files with 31 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
FoodDelivery.ino
View File

@ -58,7 +58,6 @@ void loop() {
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
/*保持mqtt心跳*/ /*保持mqtt心跳*/
fc.mqttLoop(topicSub, topicSubCount); fc.mqttLoop(topicSub, topicSubCount);
delay(50);
} }
/** /**
@ -164,24 +163,27 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
DynamicJsonDocument doc(0x2FFF); DynamicJsonDocument doc(0x2FFF);
deserializeJson(doc, topicStr); deserializeJson(doc, topicStr);
JsonObject obj = doc.as<JsonObject>(); JsonObject obj = doc.as<JsonObject>();
int8_t item=obj["item"];
int8_t val=obj["val"];
int8_t pitch=obj["pitch"];
int8_t yaw=obj["yaw"];
//相机控制 //相机控制
if (obj["item"] = "0") { //0:一键回中 if (item == 0) { //0:一键回中
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
size_t len = sizeof(command); size_t len = sizeof(command);
} else if (obj["item"] = "1") { //1:变焦 fc.udpSendToCamera(command, len);
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x0 }; } else if (item == 1) { //1:变焦
command[8] = (uint16_t)obj["val"].toInt(); uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 };
command[8] = val;
size_t len = sizeof(command); size_t len = sizeof(command);
} else if (obj["item"] = "2") { //2:俯仰 fc.udpSendToCamera(command, len);
} else if (item == 2) { //2:俯仰 旋转
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
command[9] = (uint16_t)obj["val"].toInt(); command[9] = pitch;
size_t len = sizeof(command); command[8] = yaw;
}else if (obj["item"] = "3") { //2:旋转
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
command[8] = (uint16_t)obj["val"].toInt();
size_t len = sizeof(command); size_t len = sizeof(command);
fc.udpSendToCamera(command, len);
} }
fc.udpSendToCamera(command, len);
} }
} }
@ -258,16 +260,18 @@ void writeRoute(String topicStr) {
* @param {mavlink_status_t*} pStatus * @param {mavlink_status_t*} pStatus
* @param {uint8_t} c 串口读取的缓存 * @param {uint8_t} c 串口读取的缓存
*/ */
void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) { void mavlink_receiveCallback(uint8_t c) {
mavlink_message_t msg;
mavlink_status_t status;
// 尝试从数据流里 解析数据 // 尝试从数据流里 解析数据
if (mavlink_parse_char(MAVLINK_COMM_0, c, pMsg, pStatus)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
// 通过msgid来判断 数据流的类别 // 通过msgid来判断 数据流的类别
char buf[10]; char buf[10];
switch (pMsg->msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
{ {
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象 mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
mavlink_msg_heartbeat_decode(pMsg, &heartbeat); // 解构msg数据 mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据
sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态 sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态
topicPubMsg[0] = buf; //心跳主题 topicPubMsg[0] = buf; //心跳主题
//从心跳里判断 飞机是否解锁 解锁改变飞机状态 //从心跳里判断 飞机是否解锁 解锁改变飞机状态
@ -335,7 +339,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS
{ {
mavlink_sys_status_t sys_status; // 解构的数据放到这个对象 mavlink_sys_status_t sys_status; // 解构的数据放到这个对象
mavlink_msg_sys_status_decode(pMsg, &sys_status); // 解构msg数据 mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
// 电压 // 电压
sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000); sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
if (topicPubMsg[1] != buf) { // 有更新 则更新数据 if (topicPubMsg[1] != buf) { // 有更新 则更新数据
@ -357,21 +361,21 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE
{ {
mavlink_param_value_t param_value; mavlink_param_value_t param_value;
mavlink_msg_param_value_decode(pMsg, &param_value); mavlink_msg_param_value_decode(&msg, &param_value);
} }
break; break;
case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU
{ {
mavlink_raw_imu_t raw_imu; mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(pMsg, &raw_imu); mavlink_msg_raw_imu_decode(&msg, &raw_imu);
} }
break; break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置 case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置
{ {
mavlink_global_position_int_t global_position_int; mavlink_global_position_int_t global_position_int;
mavlink_msg_global_position_int_decode(pMsg, &global_position_int); mavlink_msg_global_position_int_decode(&msg, &global_position_int);
// 高度 // 高度
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000); sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
if (topicPubMsg[4] != buf) { if (topicPubMsg[4] != buf) {
@ -383,7 +387,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘 case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘
{ {
mavlink_vfr_hud_t vfr_hud; mavlink_vfr_hud_t vfr_hud;
mavlink_msg_vfr_hud_decode(pMsg, &vfr_hud); mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
// 对地速度 ps:飞行速度 // 对地速度 ps:飞行速度
sprintf(buf, "%.2f", vfr_hud.groundspeed); sprintf(buf, "%.2f", vfr_hud.groundspeed);
if (topicPubMsg[5] != buf) { if (topicPubMsg[5] != buf) {
@ -395,7 +399,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS
{ {
mavlink_gps_raw_int_t gps_raw_int; mavlink_gps_raw_int_t gps_raw_int;
mavlink_msg_gps_raw_int_decode(pMsg, &gps_raw_int); mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
// 卫星数 // 卫星数
sprintf(buf, "%d", gps_raw_int.satellites_visible); sprintf(buf, "%d", gps_raw_int.satellites_visible);
if (topicPubMsg[6] != buf) { if (topicPubMsg[6] != buf) {
@ -455,10 +459,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈
{ {
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
mavlink_msg_mission_request_decode(pMsg, &mission_request); // 解构msg数据 mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
//日志 //日志
fc.logln("MsgID:"); fc.logln("MsgID:");
fc.logln(pMsg->msgid); fc.logln(msg.msgid);
fc.logln("No:"); fc.logln("No:");
fc.logln(mission_request.seq); fc.logln(mission_request.seq);
//飞控 反馈当前要写入航点的序号 //飞控 反馈当前要写入航点的序号
@ -469,10 +473,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈 case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈
{ {
mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象 mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象
mavlink_msg_mission_ack_decode(pMsg, &mission_ark); // 解构msg数据 mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据
/*日志*/ /*日志*/
fc.logln("MsgID:"); fc.logln("MsgID:");
fc.logln(pMsg->msgid); fc.logln(msg.msgid);
fc.logln("re:"); fc.logln("re:");
fc.logln(mission_ark.type); fc.logln(mission_ark.type);
/*记录航点的写入状态 */ /*记录航点的写入状态 */
@ -519,7 +523,7 @@ void pubThread() {
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/ */
void flashThread() { void flashThread() {
if (digitalRead(23) == HIGH) { if (digitalRead(23) == LOW) {
if (isPush) { //点击之后 if (isPush) { //点击之后
//请求注册 ps:发送esp8266的物理地址 到对频主题 //请求注册 ps:发送esp8266的物理地址 到对频主题
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());