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

测试 #9

Closed
tk wants to merge 3 commits from 测试 into dev
pull from: 测试
merge into: Food:dev
Conversation 0 Commits 3 Files Changed 5 +798 -715
1 changed files with 798 additions and 715 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a0a8004762 - Show all commits

289
src/main.cpp
View File

@ -66,7 +66,6 @@ unsigned long _tm_bengstop;
bool _bengstop = false; bool _bengstop = false;
bool _needweightalign = false; bool _needweightalign = false;
bool curr_armed = false; bool curr_armed = false;
uint8_t curr_mode = 0; uint8_t curr_mode = 0;
// 称重校准状态 // 称重校准状态
@ -106,9 +105,11 @@ const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
/*项目对象*/ /*项目对象*/
// char* ssid = "szdot"; //wifi帐号 // char* ssid = "szdot"; //wifi帐号
// char* password = "Ttaj@#*.com"; //wifi密码 // char* password = "Ttaj@#*.com"; //wifi密码
char* ssid = (char*)"fxmf_sc01"; //wifi帐号 // char* ssid = (char*)"fxmf_sc01"; //wifi帐号
char* password = (char*)"12345678"; //wifi密码 // char* password = (char*)"12345678"; //wifi密码
char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //mqtt地址 char *ssid = (char *)"flicube"; // wifi帐号
char *password = (char *)"fxmf0622"; // wifi密码
char *mqttServer = (char *)"152.32.162.75"; //"szdot.top"; //mqtt地址
int mqttPort = 1883; // mqtt端口 int mqttPort = 1883; // mqtt端口
char *mqttName = (char *)"admin"; // mqtt帐号 char *mqttName = (char *)"admin"; // mqtt帐号
char *mqttPassword = (char *)"123456"; // mqtt密码 char *mqttPassword = (char *)"123456"; // mqtt密码
@ -131,7 +132,24 @@ String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState"
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); // 订阅主题总数 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); // 订阅主题总数
/*有更新主动发送 主题*/ /*有更新主动发送 主题*/
// 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度) // 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度)
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus","position",}; String topicPub[] = {
"heartBeat",
"voltagBattery",
"currentBattery",
"batteryRemaining",
"positionAlt",
"groundSpeed",
"satCount",
"latitude",
"longitude",
"fixType",
"planeState",
"pingNet",
"getPlaneMode",
"loadweight",
"hookstatus",
"position",
};
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 发送主题总数 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 发送主题总数
String topicPubMsg[16]; // 发送数据存放 对应topicPub String topicPubMsg[16]; // 发送数据存放 对应topicPub
String oldMsg[16]; // 记录旧的数据 用来对比有没有更新 String oldMsg[16]; // 记录旧的数据 用来对比有没有更新
@ -159,9 +177,6 @@ void setup()
wei_offset = preferences.getLong("wei_offset", 0); wei_offset = preferences.getLong("wei_offset", 0);
printf("wei_offset:%d\n", wei_offset); printf("wei_offset:%d\n", wei_offset);
// 初始化按钮 // 初始化按钮
button_up.attachClick(upbtn_click); button_up.attachClick(upbtn_click);
button_up.attachDoubleClick(upbtn_dbclick); button_up.attachDoubleClick(upbtn_dbclick);
@ -191,7 +206,6 @@ void setup()
if (!motocontrol.init(&myservo)) // 初始化电机控制 if (!motocontrol.init(&myservo)) // 初始化电机控制
printf("motocontrol init fault\n"); printf("motocontrol init fault\n");
tksendinfo.start(); tksendinfo.start();
initstatus = IS_WaitStart; initstatus = IS_WaitStart;
_tm_waitinit = millis(); _tm_waitinit = millis();
@ -215,9 +229,6 @@ void setup()
// flashTicker.start(); //监听 按flash键时 主动发布对频主题 // flashTicker.start(); //监听 按flash键时 主动发布对频主题
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
// if (motocontrol.getstatus()==MS_Stop) // if (motocontrol.getstatus()==MS_Stop)
// { // {
// //slowup(); // //slowup();
@ -255,7 +266,6 @@ void showinfo()
msg_cmd.confirmation = 0; msg_cmd.confirmation = 0;
fc.mav_send_command(msg_cmd); fc.mav_send_command(msg_cmd);
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2); // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
} }
// 校准称重 // 校准称重
void begin_tare() void begin_tare()
@ -289,7 +299,9 @@ bool check_tare()
motocontrol.weightalign(true); motocontrol.weightalign(true);
printf("check_tare ok: %d,offset:%d \n", pullweight, wei_offset); printf("check_tare ok: %d,offset:%d \n", pullweight, wei_offset);
return true; return true;
}else _needweightalign = true; }
else
_needweightalign = true;
} }
else else
{ {
@ -351,7 +363,8 @@ void checkinited()
motocontrol.weightalign(true); motocontrol.weightalign(true);
_needweightalign = false; _needweightalign = false;
initstatus = IS_OK; initstatus = IS_OK;
}else }
else
{ {
printf("begin_tare \n"); printf("begin_tare \n");
begin_tare(); begin_tare();
@ -364,8 +377,8 @@ void checkinited()
if (_weightalign_status == WAS_AlignOK) if (_weightalign_status == WAS_AlignOK)
{ {
initstatus = IS_OK; initstatus = IS_OK;
}else }
if (_weightalign_status==WAS_Alignfault) else if (_weightalign_status == WAS_Alignfault)
{ {
printf("weightalign fault! again \n"); printf("weightalign fault! again \n");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
@ -373,7 +386,6 @@ void checkinited()
break; break;
} }
/* /*
case Initstatus::IS_LengthZero: case Initstatus::IS_LengthZero:
{ {
@ -423,7 +435,6 @@ void loop()
pubTicker.update(); // 定时 发布主题 pubTicker.update(); // 定时 发布主题
mavTicker.update(); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 mavTicker.update(); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
// sercomm.getcommand(); // 得到控制命令 // sercomm.getcommand(); // 得到控制命令
button_checktop.tick(); // 按钮 button_checktop.tick(); // 按钮
button_down.tick(); // 按钮 button_down.tick(); // 按钮
@ -463,7 +474,6 @@ void loop()
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
delay(1); delay(1);
} }
// 在核心0上执行耗时长的低优先级的 // 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters) void Task1(void *pvParameters)
@ -563,8 +573,6 @@ void showledidel()
led_show(255, 255, 255); // 白色 led_show(255, 255, 255); // 白色
break; break;
} }
} }
/* /*
@ -647,7 +655,8 @@ void down_action(float motospeed,float length = 0.0f)
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
{ {
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
}else }
else
{ {
motocontrol.setspeed(motospeed); motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Down, length); motocontrol.moto_run(MotoStatus::MS_Down, length);
@ -666,7 +675,8 @@ void up_action(float motospeed)
motocontrol.setspeed(motospeed); motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Up); motocontrol.moto_run(MotoStatus::MS_Up);
fc.playText("[v1]收线"); fc.playText("[v1]收线");
}else }
else
motocontrol.stoprun(); motocontrol.stoprun();
} }
// 放线按钮--单击 // 放线按钮--单击
@ -687,7 +697,8 @@ void downbtn_pressstart()
Serial.println("Down_pressstart"); Serial.println("Down_pressstart");
// 两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce > 2)
btn_pressatonce = 2;
if (btn_pressatonce == 2) if (btn_pressatonce == 2)
{ {
btn_presssatonce(); btn_presssatonce();
@ -700,7 +711,8 @@ void downbtn_pressend()
{ {
Serial.println("Down_pressend"); Serial.println("Down_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce < 0)
btn_pressatonce = 0;
// 不是恢复自动放线抬起按键就停止 // 不是恢复自动放线抬起按键就停止
if (!motocontrol.getcontrolstatus().is_autogoodsdown) if (!motocontrol.getcontrolstatus().is_autogoodsdown)
motocontrol.stoprun(); motocontrol.stoprun();
@ -733,7 +745,8 @@ void upbtn_pressstart()
Serial.println("UP_pressstart"); Serial.println("UP_pressstart");
// 两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce > 2)
btn_pressatonce = 2;
if (btn_pressatonce == 2) if (btn_pressatonce == 2)
{ {
btn_presssatonce(); btn_presssatonce();
@ -746,7 +759,8 @@ void upbtn_pressend()
{ {
Serial.println("UP_pressend"); Serial.println("UP_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce < 0)
btn_pressatonce = 0;
motocontrol.stoprun(); motocontrol.stoprun();
} }
@ -758,7 +772,8 @@ void Hook_autodown(float length_cm)
{ {
printf("Hook_autodown %.2f cm \n", length_cm); printf("Hook_autodown %.2f cm \n", length_cm);
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
}else }
else
Serial.println("autodown fault, not HS_Locked "); Serial.println("autodown fault, not HS_Locked ");
} }
void Hook_stop() void Hook_stop()
@ -772,7 +787,8 @@ void Hook_resume()
{ {
printf("Hook_resume\n"); printf("Hook_resume\n");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
}else }
else
Serial.println("resume fault, not HS_Stop "); Serial.println("resume fault, not HS_Stop ");
} }
void Hook_recovery() void Hook_recovery()
@ -820,23 +836,28 @@ void testbtn_click()
* @param {byte*} topic 订阅获取的内容 * @param {byte*} topic 订阅获取的内容
* @param {unsigned int} length 内容的长度 * @param {unsigned int} length 内容的长度
*/ */
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
{
/*获取 "订阅主题/macadd" 截取/macadd后的长度 */ /*获取 "订阅主题/macadd" 截取/macadd后的长度 */
int count = strlen(topic); int count = strlen(topic);
String cutTopic = ""; // 记录订阅主题 String cutTopic = ""; // 记录订阅主题
for (int i = 0; i < count - 13; i++) { for (int i = 0; i < count - 13; i++)
{
cutTopic += topic[i]; cutTopic += topic[i];
} }
/*解构mqtt发过来的内容*/ /*解构mqtt发过来的内容*/
String topicStr = ""; String topicStr = "";
for (int i = 0; i < length; i++) { for (int i = 0; i < length; i++)
{
topicStr += (char)payload[i]; topicStr += (char)payload[i];
} }
/*订阅回调主体*/ /*订阅回调主体*/
if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss if (cutTopic == topicSub[0])
{ // 0:飞行航点任务 questAss
writeRoute(topicStr); // 写入航点 writeRoute(topicStr); // 写入航点
} else }
if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState else if (cutTopic == topicSub[1])
{ // 1:设置飞机状态 setPlaneState
/* /*
*其中topicPubMsg[10]既飞机状态的值 *其中topicPubMsg[10]既飞机状态的值
*二进制0000 0000 0000 0000 *二进制0000 0000 0000 0000
@ -862,68 +883,85 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
uint8_t count = obj["count"]; // 传过来 uint8_t count = obj["count"]; // 传过来
// 解构val数组参数 // 解构val数组参数
uint16_t param[count]; uint16_t param[count];
for (int i = 0; i < count; i++) { for (int i = 0; i < count; i++)
{
param[i] = obj["param"][i]; param[i] = obj["param"][i];
} }
// 标记飞机状态 // 标记飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state); topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
// 飞控执行 // 飞控执行
if (n == 3) { //3操作飞机加解锁 if (n == 3)
{ // 3操作飞机加解锁
uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底 uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
fc.mav_channels_override(chan); // 控制油门 fc.mav_channels_override(chan); // 控制油门
fc.mav_set_mode(2); // 飞控设置成AltHold定高 fc.mav_set_mode(2); // 飞控设置成AltHold定高
fc.mav_command(n, param); fc.mav_command(n, param);
} else { }
else
{
uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中 uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
fc.mav_channels_override(chan); // 控制油门 fc.mav_channels_override(chan); // 控制油门
} }
if (n == 6) { //6测试起飞 if (n == 6)
{ // 6测试起飞
fc.mav_set_mode(4); // 飞控设置成Guided引导模式 fc.mav_set_mode(4); // 飞控设置成Guided引导模式
fc.mav_command(n, param); // 起飞 fc.mav_command(n, param); // 起飞
} }
if (n == 7) { //7 悬停 if (n == 7)
{ // 7 悬停
fc.mav_set_mode(5); // 飞控设置成Loiter留待模式 fc.mav_set_mode(5); // 飞控设置成Loiter留待模式
} }
if (n == 5) { //5 航点执行 if (n == 5)
{ // 5 航点执行
fc.mav_set_mode(3); // 飞控设置成auto自动模式 fc.mav_set_mode(3); // 飞控设置成auto自动模式
} }
if (n == 8) { //8降落* if (n == 8)
{ // 8降落*
fc.mav_command(n, param); fc.mav_command(n, param);
} }
if (n == 9) { //9返航 if (n == 9)
{ // 9返航
fc.mav_set_mode(6); // 飞控设置成RTL返航 fc.mav_set_mode(6); // 飞控设置成RTL返航
} }
if (n == 11) { //11磁罗盘校准 if (n == 11)
{ // 11磁罗盘校准
fc.mav_command(n, param); fc.mav_command(n, param);
} }
} }
if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState if (cutTopic == topicSub[2])
{ // 2:获取飞机状态 getPlaneState
fc.pubMQTTmsg("planeState", topicPubMsg[10]); // 终端主动get飞机状态 fc.pubMQTTmsg("planeState", topicPubMsg[10]); // 终端主动get飞机状态
}else }
if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState else if (cutTopic == topicSub[3])
{ // 3:恢复飞机为初始状态 resetState
topicPubMsg[10] = "1"; // 恢复初始状态 topicPubMsg[10] = "1"; // 恢复初始状态
}else }
if (cutTopic == topicSub[4]) { //4:油门通道1 else if (cutTopic == topicSub[4])
{ // 4:油门通道1
uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
fc.channels[0] = strInt; // 恢复初始状态 fc.channels[0] = strInt; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
}else }
if (cutTopic == topicSub[5]) { //5:油门通道2 else if (cutTopic == topicSub[5])
{ // 5:油门通道2
uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
fc.channels[1] = strInt; // 恢复初始状态 fc.channels[1] = strInt; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
}else }
if (cutTopic == topicSub[6]) { //6:油门通道3 else if (cutTopic == topicSub[6])
{ // 6:油门通道3
uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
fc.channels[2] = strInt; // 恢复初始状态 fc.channels[2] = strInt; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
}else }
if (cutTopic == topicSub[7]) { //7:油门通道4 else if (cutTopic == topicSub[7])
{ // 7:油门通道4
uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下 uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
fc.channels[3] = strInt; // 恢复初始状态 fc.channels[3] = strInt; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制 fc.mav_channels_override(fc.channels); // 油门控制
}else }
if (cutTopic == topicSub[8]) { //8:钩子控制 else if (cutTopic == topicSub[8])
{ // 8:钩子控制
uint16_t strInt = (uint16_t)topicStr.toInt(); // uint16_t strInt = (uint16_t)topicStr.toInt(); //
printf("hookcontrol command: %d \n", strInt); printf("hookcontrol command: %d \n", strInt);
switch (strInt) switch (strInt)
@ -950,7 +988,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
Hook_resume(); Hook_resume();
break; break;
} }
}else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController }
else if (cutTopic == topicSub[9])
{ // 9:云台相机控制 cameraController
// json 反序列化 // json 反序列化
DynamicJsonDocument doc(0x2FFF); DynamicJsonDocument doc(0x2FFF);
deserializeJson(doc, topicStr); deserializeJson(doc, topicStr);
@ -959,17 +999,21 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
uint8_t item = obj["item"]; uint8_t item = obj["item"];
size_t len; size_t len;
if (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};
len = sizeof(command); len = sizeof(command);
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
}
} else if (item == 1) { //1:变焦 else if (item == 1)
{ // 1:变焦
uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00}; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
command[8] = obj["val"]; command[8] = obj["val"];
len = sizeof(command); len = sizeof(command);
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
} else if (item == 2) { //2:俯仰,旋转 }
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};
int8_t pitchvalue = obj["pitch"]; int8_t pitchvalue = obj["pitch"];
int8_t yawvalue = obj["yaw"]; int8_t yawvalue = obj["yaw"];
@ -980,7 +1024,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
// printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue); // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
} }
}else if (cutTopic == topicSub[10]) { //通用命令 }
else if (cutTopic == topicSub[10])
{ // 通用命令
// json 反序列化 // json 反序列化
DynamicJsonDocument doc(0x2FFF); DynamicJsonDocument doc(0x2FFF);
deserializeJson(doc, topicStr); deserializeJson(doc, topicStr);
@ -1002,7 +1048,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
* @description: 写入航点 * @description: 写入航点
* @param {String} topicStr mqtt订阅执行任务的Json字符串 * @param {String} topicStr mqtt订阅执行任务的Json字符串
*/ */
void writeRoute(String topicStr) { void writeRoute(String topicStr)
{
if (fc.writeState) // 如果正在写入状态 跳出 if (fc.writeState) // 如果正在写入状态 跳出
{ {
fc.logln((char *)"正在写航点"); // 提示正在写入中 fc.logln((char *)"正在写航点"); // 提示正在写入中
@ -1021,10 +1068,12 @@ void writeRoute(String topicStr) {
fc.mav_mission_count(taskcount); // 向飞控请求写入航点的数量 fc.mav_mission_count(taskcount); // 向飞控请求写入航点的数量
fc.writeState = true; // 锁定写入状态 fc.writeState = true; // 锁定写入状态
// 监听飞控航点写入情况 // 监听飞控航点写入情况
while (true) { while (true)
{
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
if (fc.missionArkType == 0) { //写入成功 if (fc.missionArkType == 0)
{ // 写入成功
fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态
fc.logln((char *)"misson_bingo..."); fc.logln((char *)"misson_bingo...");
// 改变飞机状态 // 改变飞机状态
@ -1032,7 +1081,9 @@ void writeRoute(String topicStr) {
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
break; break;
} else if (fc.missionArkType > 0) { //写入失败 }
else if (fc.missionArkType > 0)
{ // 写入失败
fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态
fc.logln((char *)"misson_error..."); fc.logln((char *)"misson_error...");
// 改变飞机状态 // 改变飞机状态
@ -1042,7 +1093,8 @@ void writeRoute(String topicStr) {
return; // 写入失败 中断函数 return; // 写入失败 中断函数
} }
// 飞控返回 新的写入航点序号 // 飞控返回 新的写入航点序号
if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) { if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1)
{
// 从订阅信息里拿航点参数 // 从订阅信息里拿航点参数
uint8_t frame = obj["tasks"][fc.writeSeq]["frame"]; uint8_t frame = obj["tasks"][fc.writeSeq]["frame"];
uint8_t command = obj["tasks"][fc.writeSeq]["command"]; uint8_t command = obj["tasks"][fc.writeSeq]["command"];
@ -1079,18 +1131,21 @@ 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(uint8_t c) { void mavlink_receiveCallback(uint8_t c)
{
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t status; mavlink_status_t status;
// printf("mav:%d\n",c); //太频繁，导致电机控制处理不过来会乱 // printf("mav:%d\n",c); //太频繁，导致电机控制处理不过来会乱
// 尝试从数据流里 解析数据 // 尝试从数据流里 解析数据
if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{ // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
// 通过msgid来判断 数据流的类别 // 通过msgid来判断 数据流的类别
char buf[10]; char buf[10];
// printf("mav_id:%d\n",msg.msgid); // printf("mav_id:%d\n",msg.msgid);
switch (msg.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; // 解构的数据放到这个对象
@ -1098,7 +1153,8 @@ void mavlink_receiveCallback(uint8_t c) {
sprintf(buf, "%d", heartbeat.base_mode); // 飞控心跳状态 sprintf(buf, "%d", heartbeat.base_mode); // 飞控心跳状态
topicPubMsg[0] = buf; // 心跳主题 topicPubMsg[0] = buf; // 心跳主题
// 从心跳里判断 飞机是否解锁 解锁改变飞机状态 // 从心跳里判断 飞机是否解锁 解锁改变飞机状态
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (heartbeat.base_mode & 128)
{ // 从心跳里面 判断已经解锁
if (!curr_armed) if (!curr_armed)
{ {
printf("armed\n"); printf("armed\n");
@ -1106,7 +1162,9 @@ void mavlink_receiveCallback(uint8_t c) {
} }
curr_armed = true; curr_armed = true;
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
} else { }
else
{
if (curr_armed) if (curr_armed)
{ {
printf("disarm\n"); printf("disarm\n");
@ -1114,9 +1172,12 @@ void mavlink_receiveCallback(uint8_t c) {
} }
curr_armed = false; // 心跳里面 判断没有解锁 curr_armed = false; // 心跳里面 判断没有解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点 if ((uint8_t)topicPubMsg[10].toInt() & 4)
{ // 如果已经写入了航点
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态
} else { }
else
{
topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态 topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态
} }
} }
@ -1125,7 +1186,8 @@ void mavlink_receiveCallback(uint8_t c) {
{ {
curr_mode = heartbeat.custom_mode; curr_mode = heartbeat.custom_mode;
switch (heartbeat.custom_mode) { switch (heartbeat.custom_mode)
{
case 0: case 0:
{ {
topicPubMsg[12] = "姿态"; topicPubMsg[12] = "姿态";
@ -1183,17 +1245,20 @@ void mavlink_receiveCallback(uint8_t c) {
mavlink_msg_sys_status_decode(&msg, &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)
{ // 有更新 则更新数据
topicPubMsg[1] = buf; topicPubMsg[1] = buf;
} }
// 电流 // 电流
sprintf(buf, "%.2f", (double)sys_status.current_battery / 100); sprintf(buf, "%.2f", (double)sys_status.current_battery / 100);
if (topicPubMsg[2] != buf) { if (topicPubMsg[2] != buf)
{
topicPubMsg[2] = buf; topicPubMsg[2] = buf;
} }
// 电池电量 // 电池电量
sprintf(buf, "%d", sys_status.battery_remaining); sprintf(buf, "%d", sys_status.battery_remaining);
if (topicPubMsg[3] != buf) { if (topicPubMsg[3] != buf)
{
topicPubMsg[3] = buf; topicPubMsg[3] = buf;
} }
} }
@ -1220,18 +1285,18 @@ void mavlink_receiveCallback(uint8_t c) {
// 高度 // 高度
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) { {
topicPubMsg[4] = buf; topicPubMsg[4] = buf;
} }
// 海拔高度 // 海拔高度
sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}", sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}",
global_position_int.lon, global_position_int.lon,
global_position_int.lat, global_position_int.lat,
(double)global_position_int.alt / 1000 (double)global_position_int.alt / 1000);
);
if (topicPubMsg[15] != buf) { if (topicPubMsg[15] != buf)
{
topicPubMsg[15] = buf; topicPubMsg[15] = buf;
} }
} }
@ -1243,7 +1308,8 @@ void mavlink_receiveCallback(uint8_t c) {
mavlink_msg_vfr_hud_decode(&msg, &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)
{
topicPubMsg[5] = buf; topicPubMsg[5] = buf;
} }
} }
@ -1255,21 +1321,25 @@ void mavlink_receiveCallback(uint8_t c) {
mavlink_msg_gps_raw_int_decode(&msg, &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)
{
topicPubMsg[6] = buf; topicPubMsg[6] = buf;
} }
// 纬度 // 纬度
sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000); sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
if (topicPubMsg[7] != buf) { if (topicPubMsg[7] != buf)
{
topicPubMsg[7] = buf; topicPubMsg[7] = buf;
} }
// 经度 // 经度
sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000); sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
if (topicPubMsg[8] != buf) { if (topicPubMsg[8] != buf)
{
topicPubMsg[8] = buf; topicPubMsg[8] = buf;
} }
// 卫星状态 // 卫星状态
switch (gps_raw_int.fix_type) { switch (gps_raw_int.fix_type)
{
case 0: case 0:
{ {
topicPubMsg[9] = "No Fix"; topicPubMsg[9] = "No Fix";
@ -1390,7 +1460,6 @@ void mavlink_receiveCallback(uint8_t c) {
break; break;
} }
} }
} }
break; break;
default: default:
@ -1402,20 +1471,27 @@ void mavlink_receiveCallback(uint8_t c) {
/** /**
* @description: 发送主题线程 * @description: 发送主题线程
*/ */
void pubThread() { void pubThread()
{
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/ /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/
for (int i = 0; i < topicPubCount; i++) { //遍历向订阅主题 有数据更新时 发送信息 for (int i = 0; i < topicPubCount; i++)
if (topicPubMsg[i] != oldMsg[i]) { { // 遍历向订阅主题 有数据更新时 发送信息
if (i == 0) { //心跳包 每每向心跳主题发布信息 if (topicPubMsg[i] != oldMsg[i])
{
if (i == 0)
{ // 心跳包 每每向心跳主题发布信息
// 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 // 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
if (fc.getIsInit()) { if (fc.getIsInit())
{
fc.setIsInit(false); fc.setIsInit(false);
fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容 fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容
} }
// 发送心跳包 // 发送心跳包
fc.pubMQTTmsg(topicPub[0], topicPubMsg[0]); fc.pubMQTTmsg(topicPub[0], topicPubMsg[0]);
// printf("pub%s:%s\n",topicPub[0],topicPubMsg[0]); // printf("pub%s:%s\n",topicPub[0],topicPubMsg[0]);
} else { //非心跳包 有更新才向对应主题发布信息 }
else
{ // 非心跳包 有更新才向对应主题发布信息
fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]); fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]);
oldMsg[i] = topicPubMsg[i]; oldMsg[i] = topicPubMsg[i];
} }
@ -1428,14 +1504,19 @@ void pubThread() {
/** /**
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/ */
void flashThread() { void flashThread()
if (digitalRead(23) == HIGH) { {
if (isPush) { //点击之后 if (digitalRead(23) == HIGH)
{
if (isPush)
{ // 点击之后
// 请求注册 ps:发送esp8266的物理地址 到对频主题 // 请求注册 ps:发送esp8266的物理地址 到对频主题
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
} }
isPush = false; // 复位按钮 isPush = false; // 复位按钮
} else { }
else
{
// FLASH按下状态 // FLASH按下状态
isPush = true; isPush = true;
} }
@ -1444,13 +1525,15 @@ void flashThread() {
/** /**
* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
*/ */
void mavThread() { void mavThread()
{
fc.mav_request_data(); fc.mav_request_data();
} }
/** /**
* @description: 向飞控 发送油门指令 * @description: 向飞控 发送油门指令
*/ */
void chanThread() { void chanThread()
{
// mav_channels_override(channels); // mav_channels_override(channels);
} }