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

Compare commits

base: Food:4629a32b69a7883fd397a8b23e5427f3c53efd00
Branches Tags
Food:dev
Food:更换mqtt库
Food:master
Food:Branch_test_mqtt
Food:Branch_mqtttemp
Food:Branch_mqtttemp1
..
compare: Food:034bde27dff38200f80b4727cd234deb39ec59bc
Branches Tags
Food:dev
Food:更换mqtt库
Food:master
Food:Branch_test_mqtt
Food:Branch_mqtttemp
Food:Branch_mqtttemp1

No commits in common. "4629a32b69a7883fd397a8b23e5427f3c53efd00" and "034bde27dff38200f80b4727cd234deb39ec59bc" have entirely different histories.
4629a32b69 ... 034bde27df

5 changed files with 738 additions and 820 deletions
Show Stats Expand all files Collapse all files

2
platformio.ini
View File

@ -12,7 +12,7 @@
platform = espressif32 platform = espressif32
board = esp32doit-devkit-v1 board = esp32doit-devkit-v1
framework = arduino framework = arduino
monitor_speed = 57600 monitor_speed = 115200
upload-port = COM[14] upload-port = COM[14]
lib_deps = lib_deps =
bogde/HX711@^0.7.5 bogde/HX711@^0.7.5

6
src/config.h
View File

@ -18,8 +18,8 @@
////LED ////LED
#define LED_DATA_PIN 25 #define LED_DATA_PIN 25
// Moto-CAN // Moto-CAN
#define MOTO_CAN_RX 27 #define MOTO_CAN_RX 26
#define MOTO_CAN_TX 26 #define MOTO_CAN_TX 27
///serial1 ///serial1
#define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_TX 5
#define SERIAL_REPORT_RX 18 #define SERIAL_REPORT_RX 18
@ -29,6 +29,8 @@
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms #define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms #define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
enum HookStatus enum HookStatus
{ {
HS_UnInit, // 还未初始化 HS_UnInit, // 还未初始化

289
src/main.cpp
View File

@ -66,6 +66,7 @@ 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;
// 称重校准状态 // 称重校准状态
@ -105,11 +106,9 @@ 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 *ssid = (char *)"flicube"; // wifi帐号 char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //mqtt地址
char *password = (char *)"fxmf0622"; // wifi密码
char *mqttServer = (char *)"szdot.top"; //"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密码
@ -132,24 +131,7 @@ 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[] = { String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus","position",};
"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]; //记录旧的数据 用来对比有没有更新
@ -177,6 +159,9 @@ 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);
@ -206,6 +191,7 @@ 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();
@ -229,6 +215,9 @@ void setup()
//flashTicker.start(); //监听 按flash键时 主动发布对频主题 //flashTicker.start(); //监听 按flash键时 主动发布对频主题
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
// if (motocontrol.getstatus()==MS_Stop) // if (motocontrol.getstatus()==MS_Stop)
// { // {
// //slowup(); // //slowup();
@ -266,6 +255,7 @@ 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()
@ -299,9 +289,7 @@ 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
{ {
@ -363,8 +351,7 @@ 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();
@ -377,8 +364,8 @@ void checkinited()
if (_weightalign_status==WAS_AlignOK) if (_weightalign_status==WAS_AlignOK)
{ {
initstatus = IS_OK; initstatus = IS_OK;
} }else
else if (_weightalign_status == WAS_Alignfault) if (_weightalign_status==WAS_Alignfault)
{ {
printf("weightalign fault! again \n"); printf("weightalign fault! again \n");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
@ -386,6 +373,7 @@ void checkinited()
break; break;
} }
/* /*
case Initstatus::IS_LengthZero: case Initstatus::IS_LengthZero:
{ {
@ -435,6 +423,7 @@ 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(); // 按钮
@ -474,6 +463,7 @@ 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)
@ -573,6 +563,8 @@ void showledidel()
led_show(255, 255, 255); // 白色 led_show(255, 255, 255); // 白色
break; break;
} }
} }
/* /*
@ -655,8 +647,7 @@ 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);
@ -675,8 +666,7 @@ 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();
} }
// 放线按钮--单击 // 放线按钮--单击
@ -697,8 +687,7 @@ void downbtn_pressstart()
Serial.println("Down_pressstart"); Serial.println("Down_pressstart");
//两个同时按用于对频 //两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce > 2) if (btn_pressatonce>2) btn_pressatonce=2;
btn_pressatonce = 2;
if (btn_pressatonce==2) if (btn_pressatonce==2)
{ {
btn_presssatonce(); btn_presssatonce();
@ -711,8 +700,7 @@ void downbtn_pressend()
{ {
Serial.println("Down_pressend"); Serial.println("Down_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce < 0) if (btn_pressatonce<0) btn_pressatonce=0;
btn_pressatonce = 0;
//不是恢复自动放线抬起按键就停止 //不是恢复自动放线抬起按键就停止
if (!motocontrol.getcontrolstatus().is_autogoodsdown) if (!motocontrol.getcontrolstatus().is_autogoodsdown)
motocontrol.stoprun(); motocontrol.stoprun();
@ -745,8 +733,7 @@ void upbtn_pressstart()
Serial.println("UP_pressstart"); Serial.println("UP_pressstart");
//两个同时按用于对频 //两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce > 2) if (btn_pressatonce>2) btn_pressatonce=2;
btn_pressatonce = 2;
if (btn_pressatonce==2) if (btn_pressatonce==2)
{ {
btn_presssatonce(); btn_presssatonce();
@ -759,8 +746,7 @@ void upbtn_pressend()
{ {
Serial.println("UP_pressend"); Serial.println("UP_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce < 0) if (btn_pressatonce<0) btn_pressatonce=0;
btn_pressatonce = 0;
motocontrol.stoprun(); motocontrol.stoprun();
} }
@ -772,8 +758,7 @@ 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()
@ -787,8 +772,7 @@ 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()
@ -836,28 +820,23 @@ 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]) if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss
{ // 0:飞行航点任务 questAss
writeRoute(topicStr); //写入航点 writeRoute(topicStr); //写入航点
} } else
else if (cutTopic == topicSub[1]) if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState
{ // 1:设置飞机状态 setPlaneState
/* /*
*其中topicPubMsg[10]既飞机状态的值 *其中topicPubMsg[10]既飞机状态的值
*二进制0000 0000 0000 0000 *二进制0000 0000 0000 0000
@ -883,85 +862,68 @@ 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) if (n == 3) { //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) if (n == 6) { //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) if (n == 7) { //7 悬停
{ // 7 悬停
fc.mav_set_mode(5); //飞控设置成Loiter留待模式 fc.mav_set_mode(5); //飞控设置成Loiter留待模式
} }
if (n == 5) if (n == 5) { //5 航点执行
{ // 5 航点执行
fc.mav_set_mode(3); //飞控设置成auto自动模式 fc.mav_set_mode(3); //飞控设置成auto自动模式
} }
if (n == 8) if (n == 8) { //8降落*
{ // 8降落*
fc.mav_command(n, param); fc.mav_command(n, param);
} }
if (n == 9) if (n == 9) { //9返航
{ // 9返航
fc.mav_set_mode(6); //飞控设置成RTL返航 fc.mav_set_mode(6); //飞控设置成RTL返航
} }
if (n == 11) if (n == 11) { //11磁罗盘校准
{ // 11磁罗盘校准
fc.mav_command(n, param); fc.mav_command(n, param);
} }
} }
if (cutTopic == topicSub[2]) if (cutTopic == topicSub[2]) { //2:获取飞机状态 getPlaneState
{ // 2:获取飞机状态 getPlaneState
fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态 fc.pubMQTTmsg("planeState", topicPubMsg[10]); //终端主动get飞机状态
} }else
else if (cutTopic == topicSub[3]) if (cutTopic == topicSub[3]) { //3:恢复飞机为初始状态 resetState
{ // 3:恢复飞机为初始状态 resetState
topicPubMsg[10] = "1"; //恢复初始状态 topicPubMsg[10] = "1"; //恢复初始状态
} }else
else if (cutTopic == topicSub[4]) if (cutTopic == topicSub[4]) { //4:油门通道1
{ // 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
else if (cutTopic == topicSub[5]) if (cutTopic == topicSub[5]) { //5:油门通道2
{ // 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
else if (cutTopic == topicSub[6]) if (cutTopic == topicSub[6]) { //6:油门通道3
{ // 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
else if (cutTopic == topicSub[7]) if (cutTopic == topicSub[7]) { //7:油门通道4
{ // 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
else if (cutTopic == topicSub[8]) if (cutTopic == topicSub[8]) { //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)
@ -988,9 +950,7 @@ 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);
@ -999,21 +959,17 @@ 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) if (item ==0) { //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) } else if (item == 1) { //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"];
@ -1024,9 +980,7 @@ 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);
@ -1048,8 +1002,7 @@ 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*)"正在写航点"); // 提示正在写入中
@ -1068,12 +1021,10 @@ 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...");
//改变飞机状态 //改变飞机状态
@ -1081,9 +1032,7 @@ 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...");
//改变飞机状态 //改变飞机状态
@ -1093,8 +1042,7 @@ 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"];
@ -1131,21 +1079,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(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)) if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
{ // 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; // 解构的数据放到这个对象
@ -1153,8 +1098,7 @@ 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");
@ -1162,9 +1106,7 @@ 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");
@ -1172,12 +1114,9 @@ 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"; //没写航点 设置为初始化状态
} }
} }
@ -1186,8 +1125,7 @@ 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] = "姿态";
@ -1245,20 +1183,17 @@ 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;
} }
} }
@ -1285,18 +1220,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;
} }
} }
@ -1308,8 +1243,7 @@ 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;
} }
} }
@ -1321,25 +1255,21 @@ 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";
@ -1460,6 +1390,7 @@ void mavlink_receiveCallback(uint8_t c)
break; break;
} }
} }
} }
break; break;
default: default:
@ -1471,27 +1402,20 @@ 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 (topicPubMsg[i] != oldMsg[i]) if (i == 0) { //心跳包 每每向心跳主题发布信息
{
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];
} }
@ -1504,19 +1428,14 @@ void pubThread()
/** /**
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/ */
void flashThread() void flashThread() {
{ if (digitalRead(23) == HIGH) {
if (digitalRead(23) == HIGH) if (isPush) { //点击之后
{
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;
} }
@ -1525,15 +1444,13 @@ 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);
} }

2
src/moto.h
View File

@ -2,7 +2,7 @@
#include <Arduino.h> #include <Arduino.h>
#include <CAN.h> #include <CAN.h>
#include <ESP32SJA1000.h> #include <ESP32SJA1000.h>
#define MOTO_REVERSED -1 // 正反转1为正转，-1为反转 #define MOTO_REVERSED 1 // 正反转1为正转，-1为反转
#define MOTO_REDUCTION 36 #define MOTO_REDUCTION 36
#define MOTO_MAXANG 8192 // 0-8191 #define MOTO_MAXANG 8192 // 0-8191
#define MOTO_MAXANG_HALF MOTO_MAXANG / 2 #define MOTO_MAXANG_HALF MOTO_MAXANG / 2

9
src/motocontrol.h
View File

@ -5,7 +5,7 @@
#include <ESP32Servo.h> #include <ESP32Servo.h>
#define ROPE_MAXLENGTH 700 // 最多能放700cm---实际绳子应该比这个长750之类的 #define ROPE_MAXLENGTH 700 // 最多能放700cm---实际绳子应该比这个长750之类的
#define WHEEL_DIAMETER 2.3 // 轮子直径cm #define WHEEL_DIAMETER 3.8 // 轮子直径cm
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长 #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
@ -29,9 +29,9 @@
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms #define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
#define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些 #define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
#define SERVO_LOCKPOS 1000 // 1920 // 舵机锁定位置 #define SERVO_LOCKPOS 1920 // 舵机锁定位置
#define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置 #define SERVO_UNLOCKPOS 1800 // 舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 #define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克) #define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
@ -111,7 +111,6 @@ private:
void unlockservo(); void unlockservo();
void set_hook_speed(float hooksspeed); void set_hook_speed(float hooksspeed);
void sethooksstatus(HookStatus hookstatus); void sethooksstatus(HookStatus hookstatus);
public: public:
Motocontrol(); // 构造函数 Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数 ~Motocontrol(); // 析构函数