diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index cdd2b78..0597a77 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -94,7 +94,21 @@ String FoodCube::getMacAdd() { /** * @description: 连接wifi */ +// 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解） +IPAddress local_IP(192, 168, 8, 201); +// 设置静态IP网关 +IPAddress gateway(192, 168, 8, 1); + +IPAddress subnet(255, 255, 255, 0); +IPAddress primaryDNS(8, 8, 8, 8); //optional +IPAddress secondaryDNS(8, 8, 4, 4); //optional + void FoodCube::connectWifi() { + + if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) { + Serial.println("STA Failed to configure"); + } + //设置wifi帐号密码 WiFi.begin(ssid, password); //连接wifi @@ -115,7 +129,7 @@ void FoodCube::connectWifi() { macAdd.replace(":", ""); //板子的物理地址 并且去除冒号 log("macAdd: "); logln(macAdd); - playText("歪佛哎,已连接"); + playText("网络连接成功"); delay(500); } @@ -137,7 +151,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题 subscribeTopic(topicSub[i], 1); } - playText("哎木可优踢踢,已连接"); + playText("指令服务器已连接"); } else { //失败返回状态码 log("MQTT Server Connect Failed. Client State:"); @@ -393,32 +407,33 @@ void FoodCube::mav_request_data() { int len = mavlink_msg_to_send_buffer(buf, &msg); SWrite(buf, len, mavlinkSerial); } + // printf("send_mav\n"); } /** * @description: 解析mavlink数据流 * @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调 */ -void FoodCube::comm_receive(void (*pFun)(mavlink_message_t*, mavlink_status_t*, uint8_t)) { +void FoodCube::comm_receive(void (*pFun)( uint8_t)) { mavlink_message_t msg; mavlink_status_t status; switch (mavlinkSerial) { case 0: while (Serial.available() > 0) { // 判断串口缓存 是否有数据 uint8_t c = Serial.read(); // 从缓存拿到数据 - pFun(&msg, &status, c); + pFun( c); } break; case 1: while (Serial1.available() > 0) { // 判断串口缓存 是否有数据 uint8_t c = Serial1.read(); // 从缓存拿到数据 - pFun(&msg, &status, c); + pFun(c); } break; case 2: while (Serial2.available() > 0) { // 判断串口缓存 是否有数据 uint8_t c = Serial2.read(); // 从缓存拿到数据 - pFun(&msg, &status, c); + pFun(c); } break; } diff --git a/src/FoodDeliveryBase.h b/src/FoodDeliveryBase.h index 93ec278..ac766f9 100644 --- a/src/FoodDeliveryBase.h +++ b/src/FoodDeliveryBase.h @@ -57,7 +57,7 @@ public: /*mavlink*/ String setNBit(String str, uint8_t n, uint8_t i); void mav_request_data(); - void comm_receive(void (*pFun)(mavlink_message_t*, mavlink_status_t*, uint8_t)); + void comm_receive(void (*pFun)(uint8_t)); void mav_mission_count(uint8_t taskcount); void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z); void mav_set_mode(uint8_t SysState); diff --git a/src/main.cpp b/src/main.cpp index b158dd5..900967b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -81,6 +81,8 @@ enum Initstatus } initstatus; unsigned long _tm_waitinit; +const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //自动放线 +const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //暂停 /////////////////////// @@ -88,15 +90,15 @@ unsigned long _tm_waitinit; /*项目对象*/ //char* ssid = "szdot"; //wifi帐号 //char* password = "Ttaj@#*.com"; //wifi密码 -char* ssid = (char*)"flicube";//"fxmf_sc01"; //wifi帐号 -char* password = (char*)"fxmf0622";//"12345678"; //wifi密码 +char* ssid = (char*)"fxmf_sc01"; //wifi帐号 +char* password = (char*)"12345678"; //wifi密码 char* mqttServer = (char*)"szdot.top"; //mqtt地址 int mqttPort = 1883; //mqtt端口 char* mqttName = (char*)"admin"; //mqtt帐号 char* mqttPassword = (char*)"123456"; //mqtt密码 uint8_t mavlinkSerial = 2; //飞控占用的串口号 uint8_t voiceSerial = 1; //声音模块占用串口 -char* udpServerIP = (char*) "192.168.8.100"; //云台相机ip +char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip uint32_t udpServerPort = 37260; //云台相机端口 void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length); @@ -104,7 +106,7 @@ void mavThread(); void pubThread(); void flashThread() ; void writeRoute(String topicStr); -void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) ; +void mavlink_receiveCallback(uint8_t c) ; FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort); //创建项目对象 /*订阅 主题*/ @@ -112,11 +114,11 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" }; //订阅主题 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数 /*有更新主动发送 主题*/ -//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态 -String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus" }; +//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","altitude" }; int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数 -String topicPubMsg[15]; //发送数据存放 对应topicPub -String oldMsg[15]; //记录旧的数据 用来对比有没有更新 +String topicPubMsg[16]; //发送数据存放 对应topicPub +String oldMsg[16]; //记录旧的数据 用来对比有没有更新 /*触发发送 主题*/ //0:对频信息 String topicHandle[] = { "crosFrequency" }; @@ -341,15 +343,17 @@ void loop() pubTicker.update(); //定时 发布主题 mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 - - sercomm.getcommand(); // 得到控制命令 + + // sercomm.getcommand(); // 得到控制命令 button_checktop.tick(); // 按钮 button_down.tick(); // 按钮 button_up.tick(); // 按钮 button_test.tick(); motocontrol.setweight(pullweight); // 告诉电机拉的重量 - motocontrol.update(); // 电机控制 + motocontrol.update(); // 电机控制 + showledidel(); // 显示LED灯光 + showinfo(); // 显示一些调试用信息 // 到顶后延迟关闭动力电和舵机 if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY)) @@ -357,6 +361,7 @@ void loop() _bengstop = false; motocontrol.setlocked(true); } + // 检测执行初始化工作 checkinited(); /////////////////////////////////MQTT_语音_MAVLINK 部分 @@ -366,6 +371,7 @@ void loop() fc.mqttLoop(topicSub, topicSubCount); /////////////////////////////////MQTT_语音_MAVLINK 部分结束 delay(1); + } // 在核心0上执行耗时长的低优先级的 void Task1(void *pvParameters) @@ -621,6 +627,32 @@ void upbtn_pressend() motocontrol.stop(); } +//自动下放 +void Hook_autodown(float length_cm) +{ + if (motocontrol.gethooktatus()==HS_Locked) + { + printf("Hook_autodown %.2f cm \n",length_cm); + motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 + }else + Serial.println("autodown fault, not HS_Locked "); +} +void Hook_stop() +{ + Serial.println("Hook_stop"); + motocontrol.stop(); +} +void Hook_resume() +{ + if (motocontrol.gethooktatus()==HS_Stop) + { + printf("Hook_resume\n"); + motocontrol.moto_goodsdownresume(); + }else + Serial.println("resume fault, not HS_Stop "); +} + + // 测试按钮 void testbtn_click() { @@ -766,15 +798,26 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { { case 0: //收 /* code */ + printf("Mqtt_HOOK_UP \n"); + if (motocontrol.gethooktatus()==HS_Stop) + { + motocontrol.setspeed(SPEED_BTN_FAST); + motocontrol.moto_run(MotoStatus::MS_Up); + } break; case 1: //放 /* code */ break; case 2: //暂停 /* code */ + printf("Mqtt_HOOK_PAUSE \n"); + Hook_stop(); break; case 3: //继续 /* code */ + printf("Mqtt_HOOK_resume \n"); + if (motocontrol.gethooktatus()==HS_Stop) + Hook_resume(); break; } }else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController @@ -790,6 +833,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; len = sizeof(command); fc.udpSendToCamera(command, len); + } else if (item == 1) { //1:变焦 uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 }; command[8] = obj["val"]; @@ -804,6 +848,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { len = sizeof(command); fc.udpSendToCamera(command, len); + // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue); } } } @@ -881,16 +926,22 @@ void writeRoute(String topicStr) { * @param {mavlink_status_t*} pStatus * @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; + // printf("mav:%d\n",c); // 尝试从数据流里 解析数据 - if (mavlink_parse_char(MAVLINK_COMM_0, c, pMsg, pStatus)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 + if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 // 通过msgid来判断 数据流的类别 char buf[10]; - switch (pMsg->msgid) { + + // printf("mav_id:%d\n",msg.msgid); + + switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // #0: 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); //飞控心跳状态 topicPubMsg[0] = buf; //心跳主题 //从心跳里判断 飞机是否解锁 解锁改变飞机状态 @@ -976,7 +1027,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_SYS_STATUS: // #1: 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); if (topicPubMsg[1] != buf) { // 有更新 则更新数据 @@ -998,33 +1049,42 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE { mavlink_param_value_t param_value; - mavlink_msg_param_value_decode(pMsg, ¶m_value); + mavlink_msg_param_value_decode(&msg, ¶m_value); } break; case MAVLINK_MSG_ID_RAW_IMU: // #27: 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; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置 { 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); + + if (topicPubMsg[4] != buf) { topicPubMsg[4] = buf; } + //海拔高度 + sprintf(buf, "%.2f", (double)global_position_int.alt / 1000); + if (topicPubMsg[15] != buf) { + topicPubMsg[15] = buf; + } + + } break; case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘 { mavlink_vfr_hud_t vfr_hud; - mavlink_msg_vfr_hud_decode(pMsg, &vfr_hud); + mavlink_msg_vfr_hud_decode(&msg, &vfr_hud); // 对地速度 ps:飞行速度 sprintf(buf, "%.2f", vfr_hud.groundspeed); if (topicPubMsg[5] != buf) { @@ -1036,7 +1096,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS { 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); if (topicPubMsg[6] != buf) { @@ -1096,10 +1156,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 { 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((char*)"MsgID:"); - fc.logln(pMsg->msgid); + fc.logln(msg.msgid); fc.logln((char*)"No:"); fc.logln(mission_request.seq); //飞控 反馈当前要写入航点的序号 @@ -1110,10 +1170,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈 { 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((char*)"MsgID:"); - fc.logln(pMsg->msgid); + fc.logln(msg.msgid); fc.logln((char*)"re:"); fc.logln(mission_ark.type); /*记录航点的写入状态 */ @@ -1125,6 +1185,39 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, } break; + case MAVLINK_MSG_ID_COMMAND_LONG: // #47: 航点提交成果反馈 + { + uint16_t fc_hook_cmd=mavlink_msg_command_long_get_command(&msg); + uint16_t rngalt_cm= (uint16_t)mavlink_msg_command_long_get_param1(&msg); + printf("COMMAND_LONG ID:%d,rng:%dcm \n",fc_hook_cmd,rngalt_cm); + switch (fc_hook_cmd) + { + //自动放线 + case MAV_CMD_FC_HOOK_AUTODOWN: + { + HookStatus hss=motocontrol.gethooktatus(); + printf("MAV_CMD_FC_HOOK_AUTODOWN %d,rng:%dcm \n",hss,rngalt_cm); + //没有激光高度直接退出 + if (rngalt_cm==0) + break; + if (hss==HS_Locked) + Hook_autodown(rngalt_cm); + else + if (hss==HS_Stop) + Hook_resume(); + break; + } + //暂停 + case MAV_CMD_FC_HOOK_PAUSE: + { + printf("MAV_CMD_FC_HOOK_PAUSE \n"); + Hook_stop(); + break; + } + } + + } + break; default: break; } diff --git a/src/moto.cpp b/src/moto.cpp index a1fd4d7..524e4eb 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -30,6 +30,7 @@ moto::~moto() void moto::update() { + //printf("u1:%d\n",_closed); if (!_closed) { unsigned long dt = millis() - moto_chassis.starttime; // 时间差 @@ -44,6 +45,7 @@ void moto::update() _msoftspeed = _start_speed + mspeed * _ds; } pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed); + // printf("u2\n"); set_moto_current(moto_pid.output); // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n", // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output); diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 41e4f4a..b94162f 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -387,8 +387,13 @@ void Motocontrol::set_hook_speed(float hooksspeed) } void Motocontrol::update() // 更新 { + // printf("3.01 \n"); _moto_dji.update(); + // printf("3.02 \n"); + checkmotostatus(); + // printf("3.03 \n"); + checkhookstatus(); } void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用 @@ -424,6 +429,7 @@ void Motocontrol::moto_goodsdownresume() // _moto_dji.setspeedtarget(_goods_speed); } } +//长度cm void Motocontrol::moto_goodsdown(float length) { if (length < 0.0)