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
board = esp32doit-devkit-v1
framework = arduino
monitor_speed = 57600
monitor_speed = 115200
upload-port = COM[14]
lib_deps =
bogde/HX711@^0.7.5

6
src/config.h
View File

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

289
src/main.cpp
View File

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

2
src/moto.h
View File

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

9
src/motocontrol.h
View File

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