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

【类 型】：style

Browse Source 
【主	题】：代码格式化 较上版本 无任何变动
【描	述】：
	[原因]：
	[过程]：
	[影响]：
【结	束】

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
This commit is contained in:
tk 2024-07-02 18:24:51 +08:00
parent f3583e17e5
commit 811a89f8b2
9 changed files with 1453 additions and 1317 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

537
src/FoodDeliveryBase.cpp
View File

@ -1,25 +1,27 @@
#include "FoodDeliveryBase.h" #include "FoodDeliveryBase.h"
/** /**
* @description: 初始化 * @description: 初始化
*/ */
static const char* MOUDLENAME = "FC"; static const char *MOUDLENAME = "FC";
FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char* userUdpServerIP, uint32_t userUdpServerPort) { FoodCube::FoodCube(char *userSsid, char *userPassword, char *userMqttServer, int userMqttPort, char *userMqttName, char *userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char *userUdpServerIP, uint32_t userUdpServerPort)
{
/*初始化*/ /*初始化*/
ssid = userSsid; //wifi帐号 ssid = userSsid; // wifi帐号
password = userPassword; //wifi密码 password = userPassword; // wifi密码
mqttServer = userMqttServer; //mqtt服务器地址 mqttServer = userMqttServer; // mqtt服务器地址
mqttPort = userMqttPort; //mqtt服务器端口 mqttPort = userMqttPort; // mqtt服务器端口
mqttName = userMqttName; //mqtt帐号 mqttName = userMqttName; // mqtt帐号
mqttPassword = userMqttPassword; //mqtt密码 mqttPassword = userMqttPassword; // mqtt密码
mavlinkSerial = userMavlinkSerial; //mavlink用的串口 mavlinkSerial = userMavlinkSerial; // mavlink用的串口
voiceSerial = userVoiceSerial; //声音模块用的串口 voiceSerial = userVoiceSerial; // 声音模块用的串口
udpServerIP = userUdpServerIP; //云台相机ip udpServerIP = userUdpServerIP; // 云台相机ip
udpServerPort = userUdpServerPort; //云台相机端口 udpServerPort = userUdpServerPort; // 云台相机端口
wificonnected=false; wificonnected = false;
_tm_mqttconnect=0; _tm_mqttconnect = 0;
//初始化飞控通讯串口 波特率 // 初始化飞控通讯串口 波特率
switch (mavlinkSerial) { //初始化指定 串口号 switch (mavlinkSerial)
{ // 初始化指定 串口号
case 0: case 0:
Serial.begin(57600); Serial.begin(57600);
break; break;
@ -30,8 +32,9 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
Serial2.begin(57600); Serial2.begin(57600);
break; break;
} }
//初始化声音模块串口 波特率 // 初始化声音模块串口 波特率
switch (voiceSerial) { //初始化指定 串口号 switch (voiceSerial)
{ // 初始化指定 串口号
case 0: case 0:
Serial.begin(115200); Serial.begin(115200);
break; break;
@ -47,74 +50,88 @@ FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int
SetplayvolMax(); SetplayvolMax();
} }
/** /**
* @description: 日志打印 * @description: 日志打印
* @param {*} val 输出的信息 * @param {*} val 输出的信息
*/ */
void FoodCube::log(String val) { void FoodCube::log(String val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(char* val) { void FoodCube::log(char *val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(int val) { void FoodCube::log(int val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(IPAddress val) { void FoodCube::log(IPAddress val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::log(bool val) { void FoodCube::log(bool val)
{
Serial.print(val); Serial.print(val);
} }
void FoodCube::logln(String val) { void FoodCube::logln(String val)
ESP_LOGD(MOUDLENAME,"%s",val); {
ESP_LOGD(MOUDLENAME, "%s", val);
} }
void FoodCube::logln(char* val) { void FoodCube::logln(char *val)
ESP_LOGD(MOUDLENAME,"%s",val); {
ESP_LOGD(MOUDLENAME, "%s", val);
} }
void FoodCube::logln(int val) { void FoodCube::logln(int val)
ESP_LOGD(MOUDLENAME,"%d",val); {
ESP_LOGD(MOUDLENAME, "%d", val);
} }
void FoodCube::logln(IPAddress val) { void FoodCube::logln(IPAddress val)
ESP_LOGD(MOUDLENAME,"%s",val.toString()); {
ESP_LOGD(MOUDLENAME, "%s", val.toString());
} }
void FoodCube::logln(bool val) { void FoodCube::logln(bool val)
{
Serial.print(val); Serial.print(val);
} }
/** /**
*@description: 取值 设置值 *@description: 取值 设置值
*/ */
bool FoodCube::getIsInit() { bool FoodCube::getIsInit()
{
return isInit; return isInit;
} }
void FoodCube::setIsInit(bool b) { void FoodCube::setIsInit(bool b)
{
isInit = b; isInit = b;
} }
String FoodCube::getMacAdd() { String FoodCube::getMacAdd()
{
return macAdd; return macAdd;
} }
/** /**
* @description: 连接wifi * @description: 连接wifi
*/ */
// 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解） // 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解）
IPAddress local_IP(192, 168, 8, 201); IPAddress local_IP(192, 168, 8, 201);
// 设置静态IP网关 // 设置静态IP网关
IPAddress gateway(192, 168, 8, 1); IPAddress gateway(192, 168, 8, 1);
IPAddress subnet(255, 255, 255, 0); IPAddress subnet(255, 255, 255, 0);
IPAddress primaryDNS(8, 8, 8, 8); //optional IPAddress primaryDNS(8, 8, 8, 8); // optional
IPAddress secondaryDNS(8, 8, 4, 4); //optional IPAddress secondaryDNS(8, 8, 4, 4); // optional
void FoodCube::connectWifi() { void FoodCube::connectWifi()
{
//if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) { // if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) {
// ESP_LOGD(MOUDLENAME,"STA Failed to configure"); // ESP_LOGD(MOUDLENAME,"STA Failed to configure");
//} // }
//设置wifi帐号密码 // 设置wifi帐号密码
WiFi.begin(ssid, password); WiFi.begin(ssid, password);
//连接wifi // 连接wifi
logln("Connecting Wifi..."); logln("Connecting Wifi...");
/* /*
while (WiFi.status() != WL_CONNECTED) { while (WiFi.status() != WL_CONNECTED) {
@ -140,19 +157,19 @@ void FoodCube::connectWifi() {
bool FoodCube::checkWiFiStatus() bool FoodCube::checkWiFiStatus()
{ {
if(!wificonnected&&(WiFi.status() == WL_CONNECTED)) if (!wificonnected && (WiFi.status() == WL_CONNECTED))
{ {
wificonnected=true; wificonnected = true;
//获取局域网ip // 获取局域网ip
logln(""); logln("");
logln("WiFi connected"); logln("WiFi connected");
log("IP address: "); log("IP address: ");
logln(WiFi.localIP()); logln(WiFi.localIP());
localIp = WiFi.localIP(); localIp = WiFi.localIP();
//设置开发板为无线终端 获取物理mac地址 // 设置开发板为无线终端 获取物理mac地址
WiFi.mode(WIFI_STA); WiFi.mode(WIFI_STA);
macAdd = WiFi.macAddress(); macAdd = WiFi.macAddress();
macAdd.replace(":", ""); //板子的物理地址 并且去除冒号 macAdd.replace(":", ""); // 板子的物理地址 并且去除冒号
log("macAdd: "); log("macAdd: ");
logln(macAdd); logln(macAdd);
playText("网络连接成功"); playText("网络连接成功");
@ -160,91 +177,104 @@ bool FoodCube::checkWiFiStatus()
return wificonnected; return wificonnected;
} }
/** /**
* @description: 连接mqtt * @description: 连接mqtt
* @param {String[]} topicSub 主题数组 * @param {String[]} topicSub 主题数组
* @param {int} topicSubCount 数组总数 * @param {int} topicSubCount 数组总数
*/ */
void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { void FoodCube::connectMqtt(String topicSub[], int topicSubCount)
if (mqttClient->connected()) return; {
if (mqttClient->connected())
return;
/*尝试连接mqtt*/ /*尝试连接mqtt*/
if ((millis() - _tm_mqttconnect)>3000) if ((millis() - _tm_mqttconnect) > 3000)
{ {
logln("connect_mqtt"); logln("connect_mqtt");
if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) { if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword))
{
logln("MQTT Server Connected."); logln("MQTT Server Connected.");
log("Server Address: "); log("Server Address: ");
logln(mqttServer); logln(mqttServer);
log("ClientId :"); log("ClientId :");
logln(macAdd); logln(macAdd);
/*连接成功 订阅主题*/ /*连接成功 订阅主题*/
//订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback // 订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback
subscribeTopic("cmd", 1); subscribeTopic("cmd", 1);
delay(500); delay(500);
playText("服务器已连接"); playText("服务器已连接");
} else { }
//失败返回状态码 else
{
// 失败返回状态码
log("MQTT Server Connect Failed. Client State:"); log("MQTT Server Connect Failed. Client State:");
logln(mqttClient->state()); logln(mqttClient->state());
_tm_mqttconnect=millis(); _tm_mqttconnect = millis();
//delay(3000); // delay(3000);
} }
} }
} }
/** /**
* @description: 写在loop函数里 检测mqtt连接情况 并保持心跳 * @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
* @param {String[]} topicSub 主题数组 * @param {String[]} topicSub 主题数组
* @param {int} topicSubCount 数组总数 * @param {int} topicSubCount 数组总数
*/ */
void FoodCube::mqttLoop(String topicSub[], int topicSubCount) { void FoodCube::mqttLoop(String topicSub[], int topicSubCount)
if (mqttClient->connected()) { //检测 如果开发板成功连接服务器 {
if (mqttClient->connected())
{ // 检测 如果开发板成功连接服务器
mqttClient->loop(); // 保持心跳 mqttClient->loop(); // 保持心跳
} else { // 如果开发板未能成功连接服务器 }
else
{ // 如果开发板未能成功连接服务器
connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器 connectMqtt(topicSub, topicSubCount); // 则尝试连接服务器
} }
} }
/** /**
* @description: 订阅指定主题 * @description: 订阅指定主题
* @param {String} topicString 主题名称 * @param {String} topicString 主题名称
* @param {int} Qos 1：重要 ps:响应时间快 * @param {int} Qos 1：重要 ps:响应时间快
*/ */
void FoodCube::subscribeTopic(String topicString, int Qos = 1) { void FoodCube::subscribeTopic(String topicString, int Qos = 1)
//处理主题字符串 {
// 处理主题字符串
topicString = topicString + "/" + macAdd; topicString = topicString + "/" + macAdd;
char subTopic[topicString.length() + 1]; char subTopic[topicString.length() + 1];
strcpy(subTopic, topicString.c_str()); strcpy(subTopic, topicString.c_str());
//订阅主题 // 订阅主题
mqttClient->subscribe(subTopic, Qos); mqttClient->subscribe(subTopic, Qos);
} }
/** /**
* @description: 向指定主题 发布信息 * @description: 向指定主题 发布信息
* @param {String} topicString 主题名称 * @param {String} topicString 主题名称
* @param {String} messageString 发布的内容 * @param {String} messageString 发布的内容
*/ */
void FoodCube::pubMQTTmsg(String topicString, String messageString) { void FoodCube::pubMQTTmsg(String topicString, String messageString)
//处理主题字符串 {
// 处理主题字符串
topicString = topicString + "/" + macAdd; topicString = topicString + "/" + macAdd;
char publishTopic[topicString.length() + 1]; char publishTopic[topicString.length() + 1];
strcpy(publishTopic, topicString.c_str()); strcpy(publishTopic, topicString.c_str());
//处理发布内容字符串 // 处理发布内容字符串
char publishMsg[messageString.length() + 1]; char publishMsg[messageString.length() + 1];
strcpy(publishMsg, messageString.c_str()); strcpy(publishMsg, messageString.c_str());
//向指定主题 发布信息 // 向指定主题 发布信息
mqttClient->publish(publishTopic, publishMsg); mqttClient->publish(publishTopic, publishMsg);
} }
/** /**
* @description: 判断并向指定串口 发送命令帧 * @description: 判断并向指定串口 发送命令帧
* @param {uint8_t[]} buf[] 命令帧 字节数组 * @param {uint8_t[]} buf[] 命令帧 字节数组
* @param {int} len 命令帧 的长度 * @param {int} len 命令帧 的长度
* @param {uint8_t} swSerial 串口选择 * @param {uint8_t} swSerial 串口选择
*/ */
void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) { void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial)
//通过串口发送 {
switch (swSerial) { //初始化指定 串口号 // 通过串口发送
switch (swSerial)
{ // 初始化指定 串口号
case 0: case 0:
Serial.write(buf, len); Serial.write(buf, len);
break; break;
@ -259,30 +289,31 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
void FoodCube::SetplayvolMax() void FoodCube::SetplayvolMax()
{ {
//输出音量开到最大 // 输出音量开到最大
uint8_t volcommand[10]={0xFD,0x00,0x07,0x01,0x01,0x5B,0x76,0x31,0x30,0x5D}; uint8_t volcommand[10] = {0xFD, 0x00, 0x07, 0x01, 0x01, 0x5B, 0x76, 0x31, 0x30, 0x5D};
SWrite(volcommand, sizeof(volcommand), voiceSerial); SWrite(volcommand, sizeof(volcommand), voiceSerial);
} }
/** /**
* @description: 声音播放 * @description: 声音播放
* @param {String} str 播放内容 * @param {String} str 播放内容
*/ */
void FoodCube::playText(String str,bool flying) { void FoodCube::playText(String str, bool flying)
{
String vstr; String vstr;
//输出音量开到最大 // 输出音量开到最大
// uint8_t volcommand[10]={0xFD,0x00,0x07,0x01,0x01,0x5B,0x76,0x31,0x30,0x5D}; // uint8_t volcommand[10]={0xFD,0x00,0x07,0x01,0x01,0x5B,0x76,0x31,0x30,0x5D};
// SWrite(volcommand, sizeof(volcommand), voiceSerial); // SWrite(volcommand, sizeof(volcommand), voiceSerial);
//字符串里面的音量 // 字符串里面的音量
//空中音量开到最大，地面防止刺耳开到小 // 空中音量开到最大，地面防止刺耳开到小
if (flying) if (flying)
vstr="[v9]"+str; vstr = "[v9]" + str;
else else
vstr="[v1]"+str; vstr = "[v1]" + str;
//return ; // return ;
/*消息长度*/ /*消息长度*/
int len = vstr.length(); //修改信息长度 消息长度 int len = vstr.length(); // 修改信息长度 消息长度
// if (len >= 3996) { //限制信息长度 超长的不播放 // if (len >= 3996) { //限制信息长度 超长的不播放
// return; // return;
// } // }
@ -292,7 +323,7 @@ void FoodCube::playText(String str,bool flying) {
byte lowByte = frameLength & 0xFF; // extract low byte of length byte lowByte = frameLength & 0xFF; // extract low byte of length
/*帧命令头*/ /*帧命令头*/
uint8_t command[len + 5]; uint8_t command[len + 5];
//已知 帧头0xFD z信息长度高位 x信息长度低位 0x01命令字1是开始命令 0x04utf8编码 先给值 // 已知 帧头0xFD z信息长度高位 x信息长度低位 0x01命令字1是开始命令 0x04utf8编码 先给值
int index = 0; int index = 0;
command[index++] = 0xFD; command[index++] = 0xFD;
command[index++] = highByte; command[index++] = highByte;
@ -300,7 +331,8 @@ void FoodCube::playText(String str,bool flying) {
command[index++] = 0x01; command[index++] = 0x01;
command[index++] = 0x04; command[index++] = 0x04;
/*帧命令 消息段*/ /*帧命令 消息段*/
for (int i = 0; i < vstr.length(); i++) { for (int i = 0; i < vstr.length(); i++)
{
command[index++] = (int)vstr[i]; command[index++] = (int)vstr[i];
} }
logln("playText"); logln("playText");
@ -312,36 +344,41 @@ void FoodCube::playText(String str,bool flying) {
} }
logln(""); logln("");
*/ */
//串口发送 播放声音 // 串口发送 播放声音
SWrite(command, sizeof(command), voiceSerial); SWrite(command, sizeof(command), voiceSerial);
} }
/** /**
* @description: 检查声音模块是否空闲 * @description: 检查声音模块是否空闲
* @return {uint8_t} 0x4E 78:系统忙 0x4F 79:系统空闲 * @return {uint8_t} 0x4E 78:系统忙 0x4F 79:系统空闲
*/ */
uint8_t FoodCube::chekVoiceMcu() { uint8_t FoodCube::chekVoiceMcu()
{
uint8_t serialData; uint8_t serialData;
uint8_t check[] = { 0xFD, 0x00, 0x01, 0x21 }; //检查命令帧 uint8_t check[] = {0xFD, 0x00, 0x01, 0x21}; // 检查命令帧
SWrite(check, sizeof(check), voiceSerial); //发送检查指令 SWrite(check, sizeof(check), voiceSerial); // 发送检查指令
switch (voiceSerial) { switch (voiceSerial)
{
case 0: case 0:
{ {
while (Serial.available()) { // 当串口接收到信息后 while (Serial.available())
{ // 当串口接收到信息后
serialData = Serial.read(); // 将接收到的信息使用read读取 serialData = Serial.read(); // 将接收到的信息使用read读取
} }
} }
break; break;
case 1: case 1:
{ {
while (Serial1.available()) { // 当串口接收到信息后 while (Serial1.available())
{ // 当串口接收到信息后
serialData = Serial1.read(); // 将接收到的信息使用read读取 serialData = Serial1.read(); // 将接收到的信息使用read读取
} }
} }
break; break;
case 2: case 2:
{ {
while (Serial2.available()) { // 当串口接收到信息后 while (Serial2.available())
{ // 当串口接收到信息后
serialData = Serial2.read(); // 将接收到的信息使用read读取 serialData = Serial2.read(); // 将接收到的信息使用read读取
} }
} }
@ -352,23 +389,26 @@ uint8_t FoodCube::chekVoiceMcu() {
} }
/** /**
* @description: 声音模块停止合成 * @description: 声音模块停止合成
*/ */
void FoodCube::stopVoice() { void FoodCube::stopVoice()
uint8_t stop[] = { 0xFD, 0x00, 0x01, 0x22 }; //停止合成命令帧 {
SWrite(stop, sizeof(stop), voiceSerial); //发送检查指令 uint8_t stop[] = {0xFD, 0x00, 0x01, 0x22}; // 停止合成命令帧
SWrite(stop, sizeof(stop), voiceSerial); // 发送检查指令
} }
/** /**
* @description: 相机控制命令帧 校检码 * @description: 相机控制命令帧 校检码
* @param {uint8_t[]} 命令帧 数组 * @param {uint8_t[]} 命令帧 数组
* @param {int} len 命令帧 的长度 * @param {int} len 命令帧 的长度
*/ */
uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) { uint16_t FoodCube::CRC16_cal(uint8_t *ptr, uint32_t len, uint16_t crc_init)
{
uint16_t crc, oldcrc16; uint16_t crc, oldcrc16;
uint8_t temp; uint8_t temp;
crc = crc_init; crc = crc_init;
while (len-- != 0) { while (len-- != 0)
{
temp = (crc >> 8) & 0xff; temp = (crc >> 8) & 0xff;
oldcrc16 = crc16_tab[*ptr ^ temp]; oldcrc16 = crc16_tab[*ptr ^ temp];
crc = (crc << 8) ^ oldcrc16; crc = (crc << 8) ^ oldcrc16;
@ -377,61 +417,67 @@ uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) {
return (crc); return (crc);
} }
void FoodCube::crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result) { void FoodCube::crc_check_16bites(uint8_t *pbuf, uint32_t len, uint16_t *p_result)
{
uint16_t crc_result = 0; uint16_t crc_result = 0;
crc_result = CRC16_cal(pbuf, len, 0); crc_result = CRC16_cal(pbuf, len, 0);
*p_result = crc_result; *p_result = crc_result;
} }
/** /**
* @description: 相机 向下 * @description: 相机 向下
*/ */
void FoodCube::Camera_action_down() { void FoodCube::Camera_action_down()
Camera_action_move(-100,0);//俯仰 向下 {
Camera_action_move(-100, 0); // 俯仰 向下
} }
/** /**
* @description: 相机 角度控制 * @description: 相机 角度控制
*/ */
void FoodCube::Camera_action_move(uint8_t vpitch,uint8_t vyaw ) { void FoodCube::Camera_action_move(uint8_t vpitch, uint8_t vyaw)
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; {
command[8] =vyaw; //旋转 uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
command[9] =vpitch; //俯仰 command[8] = vyaw; // 旋转
command[9] = vpitch; // 俯仰
udpSendToCamera(command, sizeof(command)); udpSendToCamera(command, sizeof(command));
} }
/** /**
* @description: 相机 角度控制 * @description: 相机 角度控制
*/ */
void FoodCube::Camera_action_zoom(uint8_t vzoom ) { void FoodCube::Camera_action_zoom(uint8_t vzoom)
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 }; {
command[8] =vzoom; //变焦 uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
command[8] = vzoom; // 变焦
udpSendToCamera(command, sizeof(command)); udpSendToCamera(command, sizeof(command));
} }
/** /**
* @description: 相机 回中 * @description: 相机 回中
*/ */
void FoodCube::Camera_action_ret() { void FoodCube::Camera_action_ret()
uint8_t command1[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; {
uint8_t command1[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
udpSendToCamera(command1, sizeof(command1)); udpSendToCamera(command1, sizeof(command1));
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
udpSendToCamera(command, sizeof(command)); udpSendToCamera(command, sizeof(command));
} }
/** /**
* @description: 相机 * @description: 相机
* @param {uint8_t[]} 命令帧 数组 * @param {uint8_t[]} 命令帧 数组
* @param {int} len 命令帧 的长度 * @param {int} len 命令帧 的长度
*/ */
void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) { void FoodCube::udpSendToCamera(uint8_t *p_command, uint32_t len)
{
uint16_t result = 0; uint16_t result = 0;
uint16_t* p_result = &result; uint16_t *p_result = &result;
//计算校检码 // 计算校检码
crc_check_16bites(p_command, len, p_result); crc_check_16bites(p_command, len, p_result);
//加上校检码 // 加上校检码
uint8_t bytes[2 + len]; uint8_t bytes[2 + len];
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++)
{
bytes[i] = p_command[i]; bytes[i] = p_command[i];
} }
bytes[len] = static_cast<uint8_t>(result); // 低位 互换 bytes[len] = static_cast<uint8_t>(result); // 低位 互换
@ -443,36 +489,39 @@ void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) {
printf("0x%02X,", bytes[i]); printf("0x%02X,", bytes[i]);
printf("\n"); printf("\n");
*/ */
//udp发送命令帧 // udp发送命令帧
udp.beginPacket(udpServerIP, udpServerPort); udp.beginPacket(udpServerIP, udpServerPort);
udp.write(bytes, len + 2); udp.write(bytes, len + 2);
udp.endPacket(); udp.endPacket();
} }
/** /**
* @description: 按位设置 1 or 0 * @description: 按位设置 1 or 0
* @param {String} str 要设置的值 * @param {String} str 要设置的值
* @param {uint8_t} n 第几位 从0开始 * @param {uint8_t} n 第几位 从0开始
* @param {uint8_t} isOne 1:设置成1 0:设置成0 * @param {uint8_t} isOne 1:设置成1 0:设置成0
*/ */
String FoodCube::setNBit(String str, uint8_t n, uint8_t i) { String FoodCube::setNBit(String str, uint8_t n, uint8_t i)
{
char buf[10]; char buf[10];
uint16_t val = (uint8_t)str.toInt(); uint16_t val = (uint8_t)str.toInt();
if (i) { if (i)
val |= (1u << n); //按位设置成1 {
} else { val |= (1u << n); // 按位设置成1
val &= ~(1u << n); //按位设置成0 }
else
{
val &= ~(1u << n); // 按位设置成0
} }
sprintf(buf, "%d", val); sprintf(buf, "%d", val);
return buf; return buf;
} }
/** /**
* @description: 向飞控请求 指定数据 * @description: 向飞控请求 指定数据
*/ */
void FoodCube::mav_request_data() { void FoodCube::mav_request_data()
{
mavlink_message_t msg; mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// 以下注释 把数据流组合到一起 // 以下注释 把数据流组合到一起
@ -490,9 +539,10 @@ void FoodCube::mav_request_data() {
*/ */
// 根据从Pixhawk请求的所需信息进行设置 // 根据从Pixhawk请求的所需信息进行设置
const int maxStreams = 1; // 遍历次数 （下面组合的长度） const int maxStreams = 1; // 遍历次数 （下面组合的长度）
const uint8_t MAVStreams[maxStreams] = { MAV_DATA_STREAM_ALL }; // 请求的数据流组合 放到一个对象 后面进行遍历 const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_ALL}; // 请求的数据流组合 放到一个对象 后面进行遍历
const uint16_t MAVRates[maxStreams] = { 0x01 }; // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹 既每秒发送一次 const uint16_t MAVRates[maxStreams] = {0x01}; // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹 既每秒发送一次
for (int i = 0; i < maxStreams; i++) { for (int i = 0; i < maxStreams; i++)
{
// 向飞控发送请求 // 向飞控发送请求
mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1); mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
@ -505,24 +555,29 @@ void FoodCube::mav_request_data() {
* @description: 解析mavlink数据流 * @description: 解析mavlink数据流
* @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调 * @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
*/ */
void FoodCube::comm_receive(void (*pFun)( uint8_t)) { void FoodCube::comm_receive(void (*pFun)(uint8_t))
{
mavlink_message_t msg; mavlink_message_t msg;
mavlink_status_t status; mavlink_status_t status;
switch (mavlinkSerial) { switch (mavlinkSerial)
{
case 0: case 0:
while (Serial.available() > 0) { // 判断串口缓存 是否有数据 while (Serial.available() > 0)
{ // 判断串口缓存 是否有数据
uint8_t c = Serial.read(); // 从缓存拿到数据 uint8_t c = Serial.read(); // 从缓存拿到数据
pFun( c); pFun(c);
} }
break; break;
case 1: case 1:
while (Serial1.available() > 0) { // 判断串口缓存 是否有数据 while (Serial1.available() > 0)
{ // 判断串口缓存 是否有数据
uint8_t c = Serial1.read(); // 从缓存拿到数据 uint8_t c = Serial1.read(); // 从缓存拿到数据
pFun(c); pFun(c);
} }
break; break;
case 2: case 2:
while (Serial2.available() > 0) { // 判断串口缓存 是否有数据 while (Serial2.available() > 0)
{ // 判断串口缓存 是否有数据
uint8_t c = Serial2.read(); // 从缓存拿到数据 uint8_t c = Serial2.read(); // 从缓存拿到数据
pFun(c); pFun(c);
} }
@ -534,13 +589,14 @@ void FoodCube::comm_receive(void (*pFun)( uint8_t)) {
* @description: 写入航点 第一步 “向飞控发送航点数量” * @description: 写入航点 第一步 “向飞控发送航点数量”
* @param {uint8_t} taskcount 航点数量 * @param {uint8_t} taskcount 航点数量
*/ */
void FoodCube::mav_mission_count(uint8_t taskcount) { void FoodCube::mav_mission_count(uint8_t taskcount)
mavlink_message_t msg; //mavlink协议信息(msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 mavlink_message_t msg; // mavlink协议信息(msg)
//设置飞行模式的数据包 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包
mavlink_msg_mission_count_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, taskcount); mavlink_msg_mission_count_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, taskcount);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
@ -559,74 +615,80 @@ void FoodCube::mav_mission_count(uint8_t taskcount) {
* @param {double} y * @param {double} y
* @param {double} z * @param {double} z
*/ */
void FoodCube::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 FoodCube::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)
mavlink_message_t msg; //mavlink协议信息(msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 mavlink_message_t msg; // mavlink协议信息(msg)
//写入航点数据包 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 写入航点数据包
mavlink_msg_mission_item_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); mavlink_msg_mission_item_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
/** /**
* @description: 向飞控发送 设置模式指令 * @description: 向飞控发送 设置模式指令
*/ */
void FoodCube::mav_set_mode(uint8_t SysState) { void FoodCube::mav_set_mode(uint8_t SysState)
mavlink_message_t msg; //mavlink协议信息(msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 mavlink_message_t msg; // mavlink协议信息(msg)
//设置飞行模式的数据包 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包
mavlink_msg_set_mode_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 209, SysState); mavlink_msg_set_mode_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 209, SysState);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) { void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd)
mavlink_message_t msg; //mavlink协议信息(msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd); mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
void FoodCube::mav_send_text(uint8_t severity,const char *text) { void FoodCube::mav_send_text(uint8_t severity, const char *text)
{
return; return;
mavlink_message_t msg; //mavlink协议信息(msg) mavlink_message_t msg; // mavlink协议信息(msg)
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
memset(&msg, 0,sizeof(mavlink_message_t)); memset(&msg, 0, sizeof(mavlink_message_t));
mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID,&msg,severity,text); mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, severity, text);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
printf("Send:"); printf("Send:");
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++)
printf("0x%02X ",buf[i]); ; {
printf("0x%02X ", buf[i]);
;
} }
printf(" \n"); printf(" \n");
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
/** /**
* @description: 飞控 控制 * @description: 飞控 控制
* @param {uint8_t} controlType 3:加解锁模式控制 * @param {uint8_t} controlType 3:加解锁模式控制
* @param {uint16_t} param[] * @param {uint16_t} param[]
*/ */
void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) { void FoodCube::mav_command(uint8_t controlType, uint16_t param[])
mavlink_message_t msg; //mavlink协议信息(msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 mavlink_message_t msg; // mavlink协议信息(msg)
//设置飞行模式的数据包 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 设置飞行模式的数据包
mavlink_command_long_t cmd; mavlink_command_long_t cmd;
cmd.target_system = 1; cmd.target_system = 1;
cmd.target_component = 1; cmd.target_component = 1;
cmd.confirmation = 0; cmd.confirmation = 0;
if (controlType == 3) { //飞机加解锁 if (controlType == 3)
{ // 飞机加解锁
cmd.command = MAV_CMD_COMPONENT_ARM_DISARM; cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
cmd.param1 = param[0]; // 0:加锁 1：解锁 cmd.param1 = param[0]; // 0:加锁 1：解锁
cmd.param2 = param[1]; //0:符合条件加解锁 21196:强制加解锁 cmd.param2 = param[1]; // 0:符合条件加解锁 21196:强制加解锁
} }
if (controlType == 6) { //测试起飞 if (controlType == 6)
{ // 测试起飞
float p = (float)param[0]; float p = (float)param[0];
cmd.command = MAV_CMD_NAV_TAKEOFF; cmd.command = MAV_CMD_NAV_TAKEOFF;
cmd.param1 = 0; cmd.param1 = 0;
@ -635,12 +697,14 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
cmd.param4 = 0; cmd.param4 = 0;
cmd.param5 = 0; cmd.param5 = 0;
cmd.param6 = 0; cmd.param6 = 0;
cmd.param7 = p; //起飞高度 cmd.param7 = p; // 起飞高度
} }
if (controlType == 8) { //降落 if (controlType == 8)
{ // 降落
cmd.command = MAV_CMD_NAV_LAND; cmd.command = MAV_CMD_NAV_LAND;
} }
if (controlType == 11) { //磁罗盘校准 if (controlType == 11)
{ // 磁罗盘校准
cmd.command = MAV_CMD_DO_START_MAG_CAL; cmd.command = MAV_CMD_DO_START_MAG_CAL;
cmd.param1 = 0; cmd.param1 = 0;
cmd.param2 = 1; cmd.param2 = 1;
@ -648,19 +712,20 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
} }
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd); mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }
/** /**
* @description: 油门控制 * @description: 油门控制
* @param {uint16_t} chan[] 四个通道 * @param {uint16_t} chan[] 四个通道
*/ */
void FoodCube::mav_channels_override(uint16_t chan[]) { void FoodCube::mav_channels_override(uint16_t chan[])
mavlink_message_t msg; //mavlink协议信息(msg) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 mavlink_message_t msg; // mavlink协议信息(msg)
//控制油门 uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // 发送的缓存
// 控制油门
mavlink_msg_rc_channels_override_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff); mavlink_msg_rc_channels_override_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
int len = mavlink_msg_to_send_buffer(buf, &msg); int len = mavlink_msg_to_send_buffer(buf, &msg);
//通过串口发送 // 通过串口发送
SWrite(buf, len, mavlinkSerial); SWrite(buf, len, mavlinkSerial);
} }

78
src/FoodDeliveryBase.h
View File

@ -16,27 +16,28 @@
#define MAVLINK_SYSTEM_ID 0xFF #define MAVLINK_SYSTEM_ID 0xFF
#define MAVLINK_COMPONENT_ID 0xBE #define MAVLINK_COMPONENT_ID 0xBE
class FoodCube { class FoodCube
{
public: public:
/*飞行航点任务相关属性*/ /*飞行航点任务相关属性*/
bool writeState = false; //是否是写入状态 bool writeState = false; // 是否是写入状态
int8_t writeSeq = -1; //飞控反馈 需写入航点序列号 int8_t writeSeq = -1; // 飞控反馈 需写入航点序列号
int8_t futureSeq = 0; //记录将来要写入 航点序列号 int8_t futureSeq = 0; // 记录将来要写入 航点序列号
int8_t missionArkType = -1; //航点写入是否成功 int8_t missionArkType = -1; // 航点写入是否成功
/*航点任务 送餐信息喊话*/ /*航点任务 送餐信息喊话*/
String questVoiceStr; String questVoiceStr;
/*前端模拟遥控的油门通道*/ /*前端模拟遥控的油门通道*/
uint16_t channels[4] = { 1500, 1500, 1500, 1500 }; uint16_t channels[4] = {1500, 1500, 1500, 1500};
/*初始化*/ /*初始化*/
FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial,char* userUdpServerIP, uint32_t userUdpServerPort); FoodCube(char *userSsid, char *userPassword, char *userMqttServer, int userMqttPort, char *userMqttName, char *userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char *userUdpServerIP, uint32_t userUdpServerPort);
/*日志打印*/ /*日志打印*/
void log(String val); void log(String val);
void log(char* val); void log(char *val);
void log(int val); void log(int val);
void log(IPAddress val); void log(IPAddress val);
void log(bool val); void log(bool val);
void logln(String val); void logln(String val);
void logln(char* val); void logln(char *val);
void logln(int val); void logln(int val);
void logln(IPAddress val); void logln(IPAddress val);
void logln(bool val); void logln(bool val);
@ -47,7 +48,7 @@ public:
/*wifi*/ /*wifi*/
void connectWifi(); void connectWifi();
/*mqtt*/ /*mqtt*/
PubSubClient* mqttClient; //指向 mqtt服务器连接 对象 PubSubClient *mqttClient; // 指向 mqtt服务器连接 对象
void connectMqtt(String topicSub[], int topicSubCount); void connectMqtt(String topicSub[], int topicSubCount);
void mqttLoop(String topicSub[], int topicSubCount); void mqttLoop(String topicSub[], int topicSubCount);
void subscribeTopic(String topicString, int Qos); void subscribeTopic(String topicString, int Qos);
@ -55,7 +56,7 @@ public:
/*串口输出*/ /*串口输出*/
void SWrite(uint8_t buf[], int len, uint8_t swSerial); void SWrite(uint8_t buf[], int len, uint8_t swSerial);
/*声音模块控制*/ /*声音模块控制*/
void playText(String str,bool flying=false); void playText(String str, bool flying = false);
void SetplayvolMax(); void SetplayvolMax();
uint8_t chekVoiceMcu(); uint8_t chekVoiceMcu();
void stopVoice(); void stopVoice();
@ -69,40 +70,38 @@ public:
void mav_command(uint8_t controlType, uint16_t param[]); void mav_command(uint8_t controlType, uint16_t param[]);
void mav_send_command(mavlink_command_long_t msg_cmd); void mav_send_command(mavlink_command_long_t msg_cmd);
void mav_channels_override(uint16_t chan[]); void mav_channels_override(uint16_t chan[]);
void mav_send_text(uint8_t severity,const char *text); void mav_send_text(uint8_t severity, const char *text);
/*云台相机控制*/ /*云台相机控制*/
void udpSendToCamera(uint8_t* p_command, uint32_t len); void udpSendToCamera(uint8_t *p_command, uint32_t len);
void Camera_action_down(); void Camera_action_down();
void Camera_action_ret(); void Camera_action_ret();
void Camera_action_move(uint8_t vpitch,uint8_t vyaw ); void Camera_action_move(uint8_t vpitch, uint8_t vyaw);
void Camera_action_zoom(uint8_t vzoom ) ; void Camera_action_zoom(uint8_t vzoom);
bool checkWiFiStatus(); bool checkWiFiStatus();
private: private:
char* ssid; //wifi帐号 char *ssid; // wifi帐号
char* password; //wifi密码 char *password; // wifi密码
char* mqttServer; //mqtt服务器地址 char *mqttServer; // mqtt服务器地址
int mqttPort; //mqtt服务器端口 int mqttPort; // mqtt服务器端口
char* mqttName; //mqtt帐号 char *mqttName; // mqtt帐号
char* mqttPassword; //mqtt密码 char *mqttPassword; // mqtt密码
uint8_t mavlinkSerial; //飞控占用的串口号 uint8_t mavlinkSerial; // 飞控占用的串口号
uint8_t voiceSerial; //飞控占用的串口号 uint8_t voiceSerial; // 飞控占用的串口号
bool isInit = true; //用来判断接收到第一次心跳时 重新向飞控 发送一个请求数据类型 bool isInit = true; // 用来判断接收到第一次心跳时 重新向飞控 发送一个请求数据类型
WiFiClient wifiClient; //网络客户端 WiFiClient wifiClient; // 网络客户端
IPAddress localIp; //板子的IP地址 IPAddress localIp; // 板子的IP地址
String macAdd; //板子的物理地址(已去掉冒号分隔符) String macAdd; // 板子的物理地址(已去掉冒号分隔符)
bool wificonnected; //网络是否连接 bool wificonnected; // 网络是否连接
/*云台相机控制*/ /*云台相机控制*/
WiFiUDP udp; //udp信息操作对象 WiFiUDP udp; // udp信息操作对象
char* udpServerIP; //云台相机ip地址 char *udpServerIP; // 云台相机ip地址
uint32_t udpServerPort; //云台相机端口 uint32_t udpServerPort; // 云台相机端口
unsigned long _tm_mqttconnect; //mqtt上次连接时间 unsigned long _tm_mqttconnect; // mqtt上次连接时间
// 摄像头控制 校验代码
const uint16_t crc16_tab[256] = {0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
//摄像头控制 校验代码
const uint16_t crc16_tab[256] = { 0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
@ -133,10 +132,9 @@ private:
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0 }; 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0};
uint16_t CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init); uint16_t CRC16_cal(uint8_t *ptr, uint32_t len, uint16_t crc_init);
void crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result); void crc_check_16bites(uint8_t *pbuf, uint32_t len, uint16_t *p_result);
}; };
#endif #endif

523
src/commser.cpp
View File

@ -11,53 +11,54 @@ extern uint8_t curr_mode;
extern Initstatus initstatus; extern Initstatus initstatus;
extern void Hook_autodown(float length_cm); extern void Hook_autodown(float length_cm);
extern void Hook_recovery(); extern void Hook_recovery();
static const char* MOUDLENAME = "COMMSER"; static const char *MOUDLENAME = "COMMSER";
/*项目对象*/ /*项目对象*/
//char* ssid = "szdot"; //wifi帐号 // char* ssid = "szdot"; //wifi帐号
//char* password = "Ttaj@#*.com"; //wifi密码 // char* password = "Ttaj@#*.com"; //wifi密码
char* ssid = "flicube"; //wifi帐号 char *ssid = "flicube"; // wifi帐号
char* password = "fxmf0622"; //wifi密码 char *password = "fxmf0622"; // wifi密码
char* mqttServer = "szdot.top"; //mqtt地址 char *mqttServer = "szdot.top"; // mqtt地址
int mqttPort = 1883; //mqtt端口 int mqttPort = 1883; // mqtt端口
char* mqttName = "admin"; //mqtt帐号 char *mqttName = "admin"; // mqtt帐号
char* mqttPassword = "123456"; //mqtt密码 char *mqttPassword = "123456"; // mqtt密码
uint8_t mavlinkSerial = 2; //飞控占用的串口号 uint8_t mavlinkSerial = 2; // 飞控占用的串口号
uint8_t voiceSerial = 1; //声音模块占用串口 uint8_t voiceSerial = 1; // 声音模块占用串口
char* udpServerIP = "192.168.3.92"; //云台相机ip char *udpServerIP = "192.168.3.92"; // 云台相机ip
uint32_t udpServerPort = 37260; //云台相机端口 uint32_t udpServerPort = 37260; // 云台相机端口
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); //创建项目对象 FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort); // 创建项目对象
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */ /* 发布 主题 ps:以下是登记发布json内容的组成元素 */
//登记 json成员名字 // 登记 json成员名字
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度} // 0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度}
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position" }; String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position"};
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //登记 json成员总数 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
String topicPubMsg[16]; //登记 json成员的值 对应topicPub String topicPubMsg[16]; // 登记 json成员的值 对应topicPub
String oldMsg[16]; //记录旧的值 用来对比有没有更新 String oldMsg[16]; // 记录旧的值 用来对比有没有更新
/*触发发送 主题*/ /*触发发送 主题*/
//0:对频信息 // 0:对频信息
String topicHandle[] = { "crosFrequency" }; String topicHandle[] = {"crosFrequency"};
boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微动开关 boolean isPush = false; // 记得删除 板子按钮状态 ps:D3引脚下拉微动开关
/*异步线程对象*/ /*异步线程对象*/
Ticker pubTicker(pubThread,1000); //定时发布主题 线程 Ticker pubTicker(pubThread, 1000); // 定时发布主题 线程
Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 Ticker mavTicker(mavThread, 30000); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程 Ticker flashTicker(flashThread, 50); // 单片机主动 按钮主动发布主题 线程
//Ticker chanTicker; //定时向飞控 发送油门指定
// Ticker chanTicker; //定时向飞控 发送油门指定
/** /**
* @description: mqtt订阅主题 收到信息 的回调函数 * @description: mqtt订阅主题 收到信息 的回调函数
* @param {char*} topic 主题名称 msg/macadd * @param {char*} topic 主题名称 msg/macadd
* @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)
{
/*解构mqtt发过来的内容*/ /*解构mqtt发过来的内容*/
String jsonStr = ""; String jsonStr = "";
for (int i = 0; i < length; i++) { for (int i = 0; i < length; i++)
{
jsonStr += (char)payload[i]; jsonStr += (char)payload[i];
} }
/*解构内容*/ /*解构内容*/
@ -66,15 +67,19 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
// 遍历 JSON 对象 // 遍历 JSON 对象
String key; // 键 String key; // 键
JsonVariant value; // 值 JsonVariant value; // 值
for (JsonPair kv : doc.as<JsonObject>()) { for (JsonPair kv : doc.as<JsonObject>())
{
key = kv.key().c_str(); // 获取键 key = kv.key().c_str(); // 获取键
value = kv.value(); // 获取值 value = kv.value(); // 获取值
} }
/*根据订阅内容 键值 做具体操作*/ /*根据订阅内容 键值 做具体操作*/
if (key == "questAss") { //飞行航点任务 questAss if (key == "questAss")
String todo = value; //转换值 { // 飞行航点任务 questAss
writeRoute(todo); //写入航点 String todo = value; // 转换值
} else if (key == "setPlaneState") { //设置飞机状态 writeRoute(todo); // 写入航点
}
else if (key == "setPlaneState")
{ // 设置飞机状态
/* /*
*其中topicPubMsg[10]既飞机状态的值 *其中topicPubMsg[10]既飞机状态的值
*二进制0000 0000 0000 0000 *二进制0000 0000 0000 0000
@ -91,97 +96,129 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
*第10位：航点继续 *第10位：航点继续
*第11位：磁罗盘校准 *第11位：磁罗盘校准
*/ */
String todoJson = value; //转换值 String todoJson = value; // 转换值
/* json 反序列化 */ /* json 反序列化 */
DynamicJsonDocument doc(500); DynamicJsonDocument doc(500);
deserializeJson(doc, todoJson); deserializeJson(doc, todoJson);
JsonObject obj = doc.as<JsonObject>(); JsonObject obj = doc.as<JsonObject>();
uint8_t n = obj["bit"]; //状态位数 uint8_t n = obj["bit"]; // 状态位数
uint8_t state = obj["state"]; //标记飞机状态 0 or 1 uint8_t state = obj["state"]; // 标记飞机状态 0 or 1
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)
uint16_t chan[] = { 1500, 1500, 1100, 1500 }; //加解锁 油门到底 { // 3操作飞机加解锁
fc.mav_channels_override(chan); //控制油门 uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
fc.mav_set_mode(2); //飞控设置成AltHold定高 fc.mav_channels_override(chan); // 控制油门
fc.mav_command(n, param); fc.mav_set_mode(2); // 飞控设置成AltHold定高
} else {
uint16_t chan[] = { 1500, 1500, 1500, 1500 }; //除了加解锁模式 油门全部控制居中
fc.mav_channels_override(chan); //控制油门
}
if (n == 6) { //6测试起飞
fc.mav_set_mode(4); //飞控设置成Guided引导模式
fc.mav_command(n, param); //起飞
} else if (n == 7) { //7 悬停
fc.mav_set_mode(5); //飞控设置成Loiter留待模式
} else if (n == 5) { //5 航点执行
fc.mav_set_mode(3); //飞控设置成auto自动模式
} else if (n == 8) { //8降落*
fc.mav_command(n, param);
} else if (n == 9) { //9返航
fc.mav_set_mode(6); //飞控设置成RTL返航
} else if (n == 11) { //11磁罗盘校准
fc.mav_command(n, param); fc.mav_command(n, param);
} }
} else if (key == "getPlaneState") { //获取飞机状态 else
fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); //终端主动get飞机状态 {
} else if (key == "resetState") { //恢复飞机为初始状态 uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
String todo = value; //转换值 fc.mav_channels_override(chan); // 控制油门
topicPubMsg[10] = todo; //恢复初始状态 }
} else if (key == "chan1") { if (n == 6)
uint16_t todo = value; //转换值 { // 6测试起飞
fc.channels[0] = todo; //恢复初始状态 fc.mav_set_mode(4); // 飞控设置成Guided引导模式
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_command(n, param); // 起飞
} else if (key == "chan2") { }
uint16_t todo = value; //转换值 else if (n == 7)
fc.channels[1] = todo; //恢复初始状态 { // 7 悬停
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_set_mode(5); // 飞控设置成Loiter留待模式
} else if (key == "chan3") { }
uint16_t todo = value; //转换值 else if (n == 5)
fc.channels[2] = todo; //恢复初始状态 { // 5 航点执行
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_set_mode(3); // 飞控设置成auto自动模式
} else if (key == "chan4") { }
uint16_t todo = value; //转换值 else if (n == 8)
fc.channels[3] = todo; //恢复初始状态 { // 8降落*
fc.mav_channels_override(fc.channels); //油门控制 fc.mav_command(n, param);
} else if (key == "hookConteroller") { //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置 }
else if (n == 9)
{ // 9返航
fc.mav_set_mode(6); // 飞控设置成RTL返航
}
else if (n == 11)
{ // 11磁罗盘校准
fc.mav_command(n, param);
}
}
else if (key == "getPlaneState")
{ // 获取飞机状态
fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); // 终端主动get飞机状态
}
else if (key == "resetState")
{ // 恢复飞机为初始状态
String todo = value; // 转换值
topicPubMsg[10] = todo; // 恢复初始状态
}
else if (key == "chan1")
{
uint16_t todo = value; // 转换值
fc.channels[0] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制
}
else if (key == "chan2")
{
uint16_t todo = value; // 转换值
fc.channels[1] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制
}
else if (key == "chan3")
{
uint16_t todo = value; // 转换值
fc.channels[2] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制
}
else if (key == "chan4")
{
uint16_t todo = value; // 转换值
fc.channels[3] = todo; // 恢复初始状态
fc.mav_channels_override(fc.channels); // 油门控制
}
else if (key == "hookConteroller")
{ // 钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
uint16_t todo = value; uint16_t todo = value;
switch(todo){ switch (todo)
{
case 0: case 0:
{ {
//收钩 // 收钩
} }
break; break;
case 1: case 1:
{ {
//放钩 // 放钩
} }
break; break;
case 2: case 2:
{ {
//暂停 // 暂停
} }
break; break;
case 3: case 3:
{ {
//继续 // 继续
} }
break; break;
case 4: case 4:
{ {
//重置重量 // 重置重量
} }
break; break;
} }
} else if (key == "cameraController") { //云台相机控制 }
String todoJson = value; //转换值 else if (key == "cameraController")
{ // 云台相机控制
String todoJson = value; // 转换值
/* json 反序列化 */ /* json 反序列化 */
DynamicJsonDocument doc(500); DynamicJsonDocument doc(500);
deserializeJson(doc, todoJson); deserializeJson(doc, todoJson);
@ -190,21 +227,26 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
int8_t val = obj["val"]; int8_t val = obj["val"];
int8_t pitch = obj["pitch"]; int8_t pitch = obj["pitch"];
int8_t yaw = obj["yaw"]; int8_t yaw = obj["yaw"];
//相机控制 // 相机控制
if (item == 0) { //0:一键回中 if (item == 0)
uint8_t stopCommand[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; //回中前 强制 俯仰旋转 停止 { // 0:一键回中
uint8_t stopCommand[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}; // 回中前 强制 俯仰旋转 停止
size_t stopLen = sizeof(stopCommand); size_t stopLen = sizeof(stopCommand);
fc.udpSendToCamera(stopCommand, stopLen); fc.udpSendToCamera(stopCommand, stopLen);
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
size_t len = sizeof(command); size_t len = sizeof(command);
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
} else if (item == 1) { //1:变焦 }
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 }; else if (item == 1)
{ // 1:变焦
uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
command[8] = val; command[8] = val;
size_t len = sizeof(command); size_t len = sizeof(command);
fc.udpSendToCamera(command, len); fc.udpSendToCamera(command, len);
} else if (item == 2) { //2:俯仰 旋转 }
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; else if (item == 2)
{ // 2:俯仰 旋转
uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
command[8] = yaw; command[8] = yaw;
command[9] = pitch; command[9] = pitch;
size_t len = sizeof(command); size_t len = sizeof(command);
@ -212,52 +254,58 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
} }
} }
} }
/** /**
* @description: 写入航点 * @description: 写入航点
* @param {String} todo mqtt订阅执行任务的Json字符串 * @param {String} todo mqtt订阅执行任务的Json字符串
*/ */
void writeRoute(String todoJson) { void writeRoute(String todoJson)
{
if (fc.writeState) // 如果正在写入状态 跳出 if (fc.writeState) // 如果正在写入状态 跳出
{ {
fc.logln("正在写航点"); // 提示正在写入中 fc.logln("正在写航点"); // 提示正在写入中
return; return;
} }
//改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 // 改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); //正在写入航点 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); //结束初始状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}");//发送正在写入的飞机状态 fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); // 发送正在写入的飞机状态
// json 反序列化 // json 反序列化
DynamicJsonDocument doc(0x2FFF); DynamicJsonDocument doc(0x2FFF);
deserializeJson(doc, todoJson); deserializeJson(doc, todoJson);
JsonObject obj = doc.as<JsonObject>(); JsonObject obj = doc.as<JsonObject>();
// 写入航点 // 写入航点
uint8_t taskcount = obj["taskcount"]; //获取航点总数 uint8_t taskcount = obj["taskcount"]; // 获取航点总数
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("misson_bingo..."); fc.logln("misson_bingo...");
//改变飞机状态 // 改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 航点写入成功状态
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) { //写入失败
fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态
fc.logln("misson_error...");
//改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
//当有成果反馈之后 初始化下列数据
return; //写入失败 中断函数
} }
//飞控返回 新的写入航点序号 else if (fc.missionArkType > 0)
if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) { { // 写入失败
//从订阅信息里拿航点参数 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态
fc.logln("misson_error...");
// 改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); // 航点写入失败状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
// 当有成果反馈之后 初始化下列数据
return; // 写入失败 中断函数
}
// 飞控返回 新的写入航点序号
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"];
uint8_t current = obj["tasks"][fc.writeSeq]["current"]; uint8_t current = obj["tasks"][fc.writeSeq]["current"];
@ -270,52 +318,63 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
double y = obj["tasks"][fc.writeSeq]["y"]; double y = obj["tasks"][fc.writeSeq]["y"];
double z = obj["tasks"][fc.writeSeq]["z"]; double z = obj["tasks"][fc.writeSeq]["z"];
String str = obj["tasks"][fc.writeSeq]["sound"]; String str = obj["tasks"][fc.writeSeq]["sound"];
if (str != "") { if (str != "")
{
fc.questVoiceStr = str; fc.questVoiceStr = str;
} }
fc.logln("frame--"); fc.logln("frame--");
fc.logln(frame); fc.logln(frame);
//写入航点 // 写入航点
fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
fc.futureSeq++; //下一个航点序号 fc.futureSeq++; // 下一个航点序号
} }
delay(200); delay(200);
} }
} }
/** /**
* @description: 从串口拿到mavlink缓存数据之后 解析数据执行的回调 * @description: 从串口拿到mavlink缓存数据之后 解析数据执行的回调
* @param {mavlink_message_t*} pMsg mavlink数据信息指针 * @param {mavlink_message_t*} pMsg mavlink数据信息指针
* @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;
// 尝试从数据流里 解析数据 // 尝试从数据流里 解析数据
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];
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; // 解构的数据放到这个对象
mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据 mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据
sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态 sprintf(buf, "%d", heartbeat.base_mode); // 飞控心跳状态
topicPubMsg[0] = buf; //心跳主题 topicPubMsg[0] = buf; // 心跳主题
//从心跳里判断 飞机是否解锁 解锁改变飞机状态 // 从心跳里判断 飞机是否解锁 解锁改变飞机状态
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (heartbeat.base_mode & 128)
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 { // 从心跳里面 判断已经解锁
} else { //心跳里面 判断没有解锁 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁 }
if ((uint8_t)topicPubMsg[10].toInt() & 4) { //如果已经写入了航点 else
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //设置为航点写入成功状态 { // 心跳里面 判断没有解锁
} else { topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
topicPubMsg[10] = "1"; //没写航点 设置为初始化状态 if ((uint8_t)topicPubMsg[10].toInt() & 4)
{ // 如果已经写入了航点
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态
}
else
{
topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态
} }
} }
//从心跳里面 判断飞机当前的模式 // 从心跳里面 判断飞机当前的模式
switch (heartbeat.custom_mode) { switch (heartbeat.custom_mode)
{
case 0: case 0:
{ {
topicPubMsg[12] = "Stabilize自稳"; topicPubMsg[12] = "Stabilize自稳";
@ -371,17 +430,20 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
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;
} }
} }
@ -407,7 +469,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
mavlink_msg_global_position_int_decode(&msg, &global_position_int); mavlink_msg_global_position_int_decode(&msg, &global_position_int);
// 高度 // 高度
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000); sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
if (topicPubMsg[4] != buf) { if (topicPubMsg[4] != buf)
{
topicPubMsg[4] = buf; topicPubMsg[4] = buf;
} }
//{经度，维度，海拔高度} //{经度，维度，海拔高度}
@ -415,7 +478,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
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;
} }
} }
@ -427,7 +491,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
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;
} }
} }
@ -439,21 +504,25 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
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";
@ -497,12 +566,12 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
{ {
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
//日志 // 日志
fc.logln("MsgID:"); fc.logln("MsgID:");
fc.logln(msg.msgid); fc.logln(msg.msgid);
fc.logln("No:"); fc.logln("No:");
fc.logln(mission_request.seq); fc.logln(mission_request.seq);
//飞控 反馈当前要写入航点的序号 // 飞控 反馈当前要写入航点的序号
fc.writeSeq = mission_request.seq; fc.writeSeq = mission_request.seq;
} }
break; break;
@ -517,11 +586,11 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
fc.logln("re:"); fc.logln("re:");
fc.logln(mission_ark.type); fc.logln(mission_ark.type);
/*记录航点的写入状态 */ /*记录航点的写入状态 */
fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败 fc.missionArkType = mission_ark.type; // 0：写入成功 非0表示失败
/*当有成果反馈之后 初始化下列数据*/ /*当有成果反馈之后 初始化下列数据*/
fc.writeState = false; //是否是写入状态 fc.writeState = false; // 是否是写入状态
fc.writeSeq = -1; //飞控反馈 需写入航点序列号 fc.writeSeq = -1; // 飞控反馈 需写入航点序列号
fc.futureSeq = 0; //记录将来要写入 航点序列号 fc.futureSeq = 0; // 记录将来要写入 航点序列号
} }
break; break;
@ -529,27 +598,34 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
break; break;
} }
} }
} }
/** /**
* @description: 发送主题线程 * @description: 发送主题线程
*/ */
void pubThread() { void pubThread()
{
/*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */ /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
// 创建一个JSON对象 // 创建一个JSON对象
DynamicJsonDocument doc(2000); // 缓冲区 DynamicJsonDocument doc(2000); // 缓冲区
//遍历 有更新的数据 组成一个json对象 // 遍历 有更新的数据 组成一个json对象
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])
//启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 {
if (fc.getIsInit()) { if (i == 0)
{ // 心跳包 每每向心跳主题发布信息
// 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
if (fc.getIsInit())
{
fc.setIsInit(false); fc.setIsInit(false);
fc.mav_request_data(); //再向飞控请求一次 设定飞控输出数据流内容 fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容
} }
//设置对象成员 ps:心跳 // 设置对象成员 ps:心跳
doc[topicPub[0]] = topicPubMsg[0]; doc[topicPub[0]] = topicPubMsg[0];
} else { //非心跳 有更新 录入成员 }
else
{ // 非心跳 有更新 录入成员
doc[topicPub[i]] = topicPubMsg[i]; doc[topicPub[i]] = topicPubMsg[i];
oldMsg[i] = topicPubMsg[i]; oldMsg[i] = topicPubMsg[i];
} }
@ -560,39 +636,46 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
serializeJson(doc, jsonString); serializeJson(doc, jsonString);
fc.pubMQTTmsg("planeState", jsonString); fc.pubMQTTmsg("planeState", jsonString);
/*更新4G网络测速ping值*/ /*更新4G网络测速ping值*/
//pingNetTest(); // pingNetTest();
//八达岭需求 过后删除 // 八达岭需求 过后删除
fc.pubMQTTmsg("heartBeat", topicPubMsg[0]); fc.pubMQTTmsg("heartBeat", topicPubMsg[0]);
fc.pubMQTTmsg("position", topicPubMsg[15]); fc.pubMQTTmsg("position", topicPubMsg[15]);
} }
/** /**
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/ */
void flashThread() { void flashThread()
if (digitalRead(23) == LOW) { {
if (isPush) { //点击之后 if (digitalRead(23) == LOW)
//请求注册 ps:发送esp8266的物理地址 到对频主题 {
if (isPush)
{ // 点击之后
// 请求注册 ps:发送esp8266的物理地址 到对频主题
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
} }
isPush = false; //复位按钮 isPush = false; // 复位按钮
} else { }
//FLASH按下状态 else
{
// FLASH按下状态
isPush = true; isPush = true;
} }
} }
/** /**
* @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);
}

14
src/commser.h
View File

@ -6,16 +6,16 @@
#include "FoodDeliveryBase.h" #include "FoodDeliveryBase.h"
#include "config.h" #include "config.h"
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length); void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
void mavThread(); void mavThread();
void pubThread(); void pubThread();
void flashThread() ; void flashThread();
void writeRoute(String topicStr); void writeRoute(String topicStr);
void mavlink_receiveCallback(uint8_t c) ; void mavlink_receiveCallback(uint8_t c);
extern String topicPub[]; extern String topicPub[];
extern int topicPubCount; extern int topicPubCount;
extern FoodCube fc; //创建项目对象 extern FoodCube fc; // 创建项目对象
extern Ticker pubTicker; //定时发布主题 线程 extern Ticker pubTicker; // 定时发布主题 线程
extern Ticker mavTicker; //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 extern Ticker mavTicker; // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
extern Ticker flashTicker; //单片机主动 按钮主动发布主题 线程 extern Ticker flashTicker; // 单片机主动 按钮主动发布主题 线程
#endif #endif

47
src/config.h
View File

@ -3,8 +3,8 @@
// 定义公共结构，变量，硬件接口等 // 定义公共结构，变量，硬件接口等
/// ///
// //
#define VERSION "0.90" //软件版本 #define VERSION "0.90" // 软件版本
#define VERSION_HW 1 //硬件版本1:第一块硬件 2:目前版本 #define VERSION_HW 1 // 硬件版本1:第一块硬件 2:目前版本
// 硬件接口定义//////////////////////////// // 硬件接口定义////////////////////////////
// 按钮 // 按钮
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND #define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
@ -12,37 +12,36 @@
#define BTN_CT 21 // 到顶检测开关 #define BTN_CT 21 // 到顶检测开关
#define BTN_TEST 18 // 测试开关 #define BTN_TEST 18 // 测试开关
// 称重传感器- HX711 // 称重传感器- HX711
#define LOADCELL_DOUT_PIN 13 //16 #define LOADCELL_DOUT_PIN 13 // 16
#define LOADCELL_SCK_PIN 33 //17 #define LOADCELL_SCK_PIN 33 // 17
/////////////////////////////////////////// ///////////////////////////////////////////
#define SERVO_PIN 14 // 锁定舵机PWM控制脚 #define SERVO_PIN 14 // 锁定舵机PWM控制脚
////LED ////LED
#define LED_DATA_PIN 25 #define LED_DATA_PIN 25
#if (VERSION_HW == 1) #if (VERSION_HW == 1)
// Moto-CAN //第一版本硬件参数---1号机使用 // Moto-CAN //第一版本硬件参数---1号机使用
#define MOTO_CAN_RX 26 #define MOTO_CAN_RX 26
#define MOTO_CAN_TX 27 #define MOTO_CAN_TX 27
#define WEIGHT_SCALE 165 // //A通道是165,B通道是41 #define WEIGHT_SCALE 165 // //A通道是165,B通道是41
#define HX711_GAIN 128 #define HX711_GAIN 128
#else #else
// Moto-CAN //第二版本硬件参数---2号机使用 // Moto-CAN //第二版本硬件参数---2号机使用
#define MOTO_CAN_RX 27 //PCB画板需要，做了调整 #define MOTO_CAN_RX 27 // PCB画板需要，做了调整
#define MOTO_CAN_TX 26 //PCB画板需要，做了调整 #define MOTO_CAN_TX 26 // PCB画板需要，做了调整
#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41 #define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41
#define HX711_GAIN 32 //减少零点漂移用B通道的感度 #define HX711_GAIN 32 // 减少零点漂移用B通道的感度
#endif #endif
///serial1 /// serial1
#define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_TX 5
#define SERIAL_REPORT_RX 18 #define SERIAL_REPORT_RX 18
///// /////
//#define WEIGHT_SCALE 41 // //A通道是165,B通道是41 // #define WEIGHT_SCALE 41 // //A通道是165,B通道是41
#define TM_INSTORE_WEIGHT_DELAY 200 //200 // 入仓动力延时关闭时间 ms #define TM_INSTORE_WEIGHT_DELAY 200 // 200 // 入仓动力延时关闭时间 ms
#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, // 还未初始化
@ -67,9 +66,9 @@ enum Initstatus
IS_CheckWeightZero, // 检查称重传感器是否归0 IS_CheckWeightZero, // 检查称重传感器是否归0
IS_OK, // 4.所有初始化已经OK，可以正常操作 IS_OK, // 4.所有初始化已经OK，可以正常操作
IS_Error // 5.初始化遇到错误 IS_Error // 5.初始化遇到错误
} ; };
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线 const uint16_t MAV_CMD_FC_HOOK_AUTODOWN = 40; // 飞控发的---自动放线
const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停 const uint16_t MAV_CMD_FC_HOOK_PAUSE = 41; // 飞控发的---暂停
const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态 const uint16_t MAV_CMD_FC_HOOK_STATUS = 42; // 发给飞控的状态
const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 const uint16_t MAV_CMD_FC_HOOK_RECOVERY = 43; // 飞控发的---收线
const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下 const uint16_t MAV_CMD_FC_DESCENTSTART = 44; // 飞开始下降，需要调整相机向下

326
src/main.cpp
View File

@ -57,23 +57,23 @@ void btn_presssatonce();
int get_pullweight(); int get_pullweight();
void Task1(void *pvParameters); void Task1(void *pvParameters);
//void led_show(CRGB ledcolor); // void led_show(CRGB ledcolor);
// void Task1code( void *pvParameters ); // void Task1code( void *pvParameters );
void showledidel(); void showledidel();
int pullweight = 0; //检测重量-克 int pullweight = 0; // 检测重量-克
int pullweightoktimes=0; //校准成功次数 int pullweightoktimes = 0; // 校准成功次数
int8_t btn_pressatonce=0; //是否同时按下 int8_t btn_pressatonce = 0; // 是否同时按下
unsigned long _tm_bengstop; unsigned long _tm_bengstop;
bool _bengstop = false; bool _bengstop = false;
bool _needweightalign = false; bool _needweightalign = false;
HookStatus _lasthooktatus=HS_UnInit; //前一个钩子状态 HookStatus _lasthooktatus = HS_UnInit; // 前一个钩子状态
bool curr_armed=false; bool curr_armed = false;
uint8_t curr_mode=0; uint8_t curr_mode = 0;
bool _checkweightcal=false; //检测是否要检测称重传感器是否需要校准 bool _checkweightcal = false; // 检测是否要检测称重传感器是否需要校准
uint8_t _checkweighttimes=0; // uint8_t _checkweighttimes = 0; //
unsigned long _tm_checkweigh; unsigned long _tm_checkweigh;
static const char* MOUDLENAME = "MAIN"; static const char *MOUDLENAME = "MAIN";
// 称重校准状态 // 称重校准状态
enum Weightalign_status enum Weightalign_status
{ {
@ -82,18 +82,12 @@ enum Weightalign_status
WAS_AlignOK, WAS_AlignOK,
WAS_Alignfault WAS_Alignfault
} _weightalign_status; } _weightalign_status;
unsigned long _weightalign_begintm; //校准开始时间 unsigned long _weightalign_begintm; // 校准开始时间
// 需要做的初始化工作 // 需要做的初始化工作
Initstatus initstatus; Initstatus initstatus;
unsigned long _tm_waitinit; unsigned long _tm_waitinit;
void setup() void setup()
{ {
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启 WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
@ -102,13 +96,10 @@ void setup()
// 调试串口 // 调试串口
Serial.begin(57600); Serial.begin(57600);
ESP_LOGI(MOUDLENAME,"Starting PullupDevice... Ver:%s",VERSION); ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION);
preferences.begin("PullupDev", false); preferences.begin("PullupDev", false);
wei_offset = preferences.getLong("wei_offset", 0); wei_offset = preferences.getLong("wei_offset", 0);
ESP_LOGD(MOUDLENAME,"wei_offset:%d",wei_offset); ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset);
// 初始化按钮 // 初始化按钮
button_up.attachClick(upbtn_click); button_up.attachClick(upbtn_click);
@ -137,40 +128,37 @@ void setup()
// 初始化RGB LED 显示黄色灯表示需要注意 LED // 初始化RGB LED 显示黄色灯表示需要注意 LED
FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
if (!motocontrol.init(&myservo)) // 初始化电机控制 if (!motocontrol.init(&myservo)) // 初始化电机控制
ESP_LOGE(MOUDLENAME,"motocontrol init fault"); ESP_LOGE(MOUDLENAME, "motocontrol init fault");
tksendinfo.start(); tksendinfo.start();
initstatus = IS_WaitStart; initstatus = IS_WaitStart;
_tm_waitinit = millis(); _tm_waitinit = millis();
_needweightalign = (wei_offset==0); _needweightalign = (wei_offset == 0);
led_show(255,255,255);// 连接wifi中 led_show(255, 255, 255); // 连接wifi中
/////////////////////////////////MQTT_语音_MAVLINK 部分 /////////////////////////////////MQTT_语音_MAVLINK 部分
/*初始化*/ /*初始化*/
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射 Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); // 声音模块引串口脚映射
fc.playText("开始启动"); fc.playText("开始启动");
fc.connectWifi(); //连接wifi fc.connectWifi(); // 连接wifi
fc.playText("正在连接服务器"); fc.playText("正在连接服务器");
fc.connectMqtt(topicPub, topicPubCount); //连接mqtt fc.connectMqtt(topicPub, topicPubCount); // 连接mqtt
fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调 fc.mqttClient->setCallback(mqtt_receiveCallback); // 设置订阅成功 回调
fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义) fc.mav_request_data(); // 指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
/*异步线程*/ /*异步线程*/
pubTicker.start(); //定时 发布主题 pubTicker.start(); // 定时 发布主题
mavTicker.start(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 mavTicker.start(); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
flashTicker.start(); //监听 按flash键时 主动发布对频主题 flashTicker.start(); // 监听 按flash键时 主动发布对频主题
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
// if (motocontrol.getstatus()==MS_Stop) // if (motocontrol.getstatus()==MS_Stop)
// { // {
// //slowup(); // //slowup();
// } // }
ESP_LOGI(MOUDLENAME,"PullupDevice is ready!"); ESP_LOGI(MOUDLENAME, "PullupDevice is ready!");
// fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!"); // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
} }
void slowup() void slowup()
@ -180,43 +168,43 @@ void slowup()
} }
void checkstatus() void checkstatus()
{ {
HookStatus vhooktatus= motocontrol.gethooktatus(); HookStatus vhooktatus = motocontrol.gethooktatus();
//入仓把云台回中 // 入仓把云台回中
if ((_lasthooktatus!=vhooktatus)&&(vhooktatus==HS_InStore)) if ((_lasthooktatus != vhooktatus) && (vhooktatus == HS_InStore))
fc.Camera_action_ret(); fc.Camera_action_ret();
if (_checkweightcal && (vhooktatus==HS_Stop)) if (_checkweightcal && (vhooktatus == HS_Stop))
{ {
//第一次进来 // 第一次进来
if (_lasthooktatus!=vhooktatus) if (_lasthooktatus != vhooktatus)
_tm_checkweigh=millis(); _tm_checkweigh = millis();
else else
{ //1秒内检测连续大于5次就认为重了 { // 1秒内检测连续大于5次就认为重了
if ((millis()-_tm_checkweigh)<1000) if ((millis() - _tm_checkweigh) < 1000)
{ {
if (abs(pullweight) > 100) if (abs(pullweight) > 100)
_checkweighttimes++; _checkweighttimes++;
else _checkweighttimes=0; else
//ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight); _checkweighttimes = 0;
if (_checkweighttimes>10) // ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
if (_checkweighttimes > 10)
{ {
_checkweightcal=false; _checkweightcal = false;
//ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight); // ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量"); fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量");
} }
}else }
else
{ {
_checkweightcal=false; _checkweightcal = false;
} }
} }
} }
_lasthooktatus=vhooktatus; _lasthooktatus = vhooktatus;
// 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);
} }
//1秒调用一次了发mqtt到地面 // 1秒调用一次了发mqtt到地面
void showinfo() void showinfo()
{ {
// moto_measure_t mf=motocontrol.getmotoinfo() ; // moto_measure_t mf=motocontrol.getmotoinfo() ;
@ -224,62 +212,64 @@ void showinfo()
// if (pullweight > 10) // if (pullweight > 10)
// printf("PullWeight:%d\n", pullweight); //发送重量到mqtt // printf("PullWeight:%d\n", pullweight); //发送重量到mqtt
//topicPubMsg[14]=motocontrol.gethooktatus_str() ; // topicPubMsg[14]=motocontrol.gethooktatus_str() ;
//topicPubMsg[13]=pullweight; // topicPubMsg[13]=pullweight;
// control_status_t cs=motocontrol.getcontrolstatus() ; // control_status_t cs=motocontrol.getcontrolstatus() ;
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus); // printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
HookStatus vhooktatus= motocontrol.gethooktatus(); HookStatus vhooktatus = motocontrol.gethooktatus();
mavlink_command_long_t msg_cmd; mavlink_command_long_t msg_cmd;
msg_cmd.command=MAV_CMD_FC_HOOK_STATUS; msg_cmd.command = MAV_CMD_FC_HOOK_STATUS;
msg_cmd.param1=vhooktatus; msg_cmd.param1 = vhooktatus;
msg_cmd.param2=pullweight; msg_cmd.param2 = pullweight;
msg_cmd.target_system = 1; msg_cmd.target_system = 1;
msg_cmd.target_component = 1; msg_cmd.target_component = 1;
msg_cmd.confirmation = 0; msg_cmd.confirmation = 0;
fc.mav_send_command(msg_cmd); fc.mav_send_command(msg_cmd);
} }
//校准称重 // 校准称重
void begin_tare() void begin_tare()
{ {
ESP_LOGD(MOUDLENAME,"begin_tare"); ESP_LOGD(MOUDLENAME, "begin_tare");
_weightalign_status=WAS_Aligning; _weightalign_status = WAS_Aligning;
_needweightalign = true; _needweightalign = true;
_weightalign_begintm=millis(); _weightalign_begintm = millis();
} }
//检查校准结果 // 检查校准结果
bool check_tare() bool check_tare()
{ {
if (_weightalign_status!=WAS_Aligning) if (_weightalign_status != WAS_Aligning)
return false; return false;
//超时失败 // 超时失败
if ((millis()-_weightalign_begintm)>2000) if ((millis() - _weightalign_begintm) > 2000)
{ {
_weightalign_status=WAS_Alignfault; _weightalign_status = WAS_Alignfault;
return false; return false;
} }
if ((pullweight < 10)&&((pullweight >-10))) if ((pullweight < 10) && ((pullweight > -10)))
{ {
pullweightoktimes++; pullweightoktimes++;
if (pullweightoktimes>20) if (pullweightoktimes > 20)
{ {
_needweightalign = false; _needweightalign = false;
_weightalign_status=WAS_AlignOK; _weightalign_status = WAS_AlignOK;
wei_offset=scale.get_offset(); wei_offset = scale.get_offset();
preferences.putLong("wei_offset", wei_offset); preferences.putLong("wei_offset", wei_offset);
motocontrol.weightalign(true); motocontrol.weightalign(true);
ESP_LOGD(MOUDLENAME,"check_tare ok: %d,offset:%d", pullweight,wei_offset); ESP_LOGD(MOUDLENAME, "check_tare ok: %d,offset:%d", pullweight, wei_offset);
return true; return true;
}else _needweightalign = true; }
else
_needweightalign = true;
} }
else else
{ {
// 没成功继续校准 // 没成功继续校准
ESP_LOGD(MOUDLENAME,"weight not zero: %d", pullweight); ESP_LOGD(MOUDLENAME, "weight not zero: %d", pullweight);
_needweightalign = true; _needweightalign = true;
pullweightoktimes=0; pullweightoktimes = 0;
} }
return false; return false;
} }
@ -299,16 +289,16 @@ void checkinited()
case Initstatus::IS_Start: case Initstatus::IS_Start:
{ {
//一开始没有锁定状态 // 一开始没有锁定状态
if (motocontrol.gethooktatus() != HS_Locked) if (motocontrol.gethooktatus() != HS_Locked)
{ {
// 开始自动慢速上升，直到顶部按钮按下 // 开始自动慢速上升，直到顶部按钮按下
ESP_LOGD(MOUDLENAME,"IS_Start: startup wait locked"); ESP_LOGD(MOUDLENAME, "IS_Start: startup wait locked");
motocontrol.setspeed(SPEED_BTN_SLOW); motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up); motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_Locked; initstatus = IS_Wait_Locked;
} }
else //开机就是锁定状态--开始校准称重传感器 else // 开机就是锁定状态--开始校准称重传感器
{ {
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
} }
@ -318,47 +308,47 @@ void checkinited()
{ {
if (motocontrol.gethooktatus() == HS_Locked) if (motocontrol.gethooktatus() == HS_Locked)
{ {
ESP_LOGD(MOUDLENAME,"IS_Wait_LengthZero: is locked"); ESP_LOGD(MOUDLENAME, "IS_Wait_LengthZero: is locked");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
} }
break; break;
} }
case Initstatus::IS_begin_WeightZero : case Initstatus::IS_begin_WeightZero:
{ {
//如果没有保存的校准数据就需要校准 // 如果没有保存的校准数据就需要校准
if (wei_offset!=0) if (wei_offset != 0)
{ {
ESP_LOGD(MOUDLENAME,"offset is loaded weight: %d,offset:%d", pullweight,wei_offset); ESP_LOGD(MOUDLENAME, "offset is loaded weight: %d,offset:%d", pullweight, wei_offset);
scale.set_offset(wei_offset); scale.set_offset(wei_offset);
motocontrol.weightalign(true); motocontrol.weightalign(true);
_needweightalign = false; _needweightalign = false;
// fc.playText("挂钩已锁定"); // fc.playText("挂钩已锁定");
initstatus = IS_OK; initstatus = IS_OK;
}else }
else
{ {
ESP_LOGD(MOUDLENAME,"begin_tare"); ESP_LOGD(MOUDLENAME, "begin_tare");
begin_tare(); begin_tare();
initstatus = IS_CheckWeightZero; initstatus = IS_CheckWeightZero;
} }
break; break;
} }
case Initstatus::IS_CheckWeightZero : case Initstatus::IS_CheckWeightZero:
{ {
if (_weightalign_status==WAS_AlignOK) if (_weightalign_status == WAS_AlignOK)
{ {
// fc.playText("挂钩已锁定"); // fc.playText("挂钩已锁定");
initstatus = IS_OK; initstatus = IS_OK;
}else }
if (_weightalign_status==WAS_Alignfault) else if (_weightalign_status == WAS_Alignfault)
{ {
ESP_LOGD(MOUDLENAME,"weightalign fault! again"); ESP_LOGD(MOUDLENAME, "weightalign fault! again");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
} }
break; break;
} }
/* /*
case Initstatus::IS_LengthZero: case Initstatus::IS_LengthZero:
{ {
@ -411,9 +401,8 @@ void set_locked(bool locked)
void loop() void loop()
{ {
tksendinfo.update(); // 定时发送信息任务 tksendinfo.update(); // 定时发送信息任务
//pubTicker.update(); //定时 发布主题 // pubTicker.update(); //定时 发布主题
//mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 // mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
// sercomm.getcommand(); // 得到控制命令 // sercomm.getcommand(); // 得到控制命令
button_checktop.tick(); // 按钮 button_checktop.tick(); // 按钮
@ -424,12 +413,12 @@ void loop()
motocontrol.update(); // 电机控制 motocontrol.update(); // 电机控制
showledidel(); // 显示LED灯光 showledidel(); // 显示LED灯光
checkstatus(); //检测状态，执行一些和状态有关的功能 checkstatus(); // 检测状态，执行一些和状态有关的功能
//showinfo(); // 显示一些调试用信息- // showinfo(); // 显示一些调试用信息-
// 到顶后延迟关闭动力电和舵机 // 到顶后延迟关闭动力电和舵机
if (_bengstop) if (_bengstop)
{ {
if ((initstatus==IS_OK)&&(pullweight>TM_INSTORE_DELAY_WEIGHT) ) if ((initstatus == IS_OK) && (pullweight > TM_INSTORE_DELAY_WEIGHT))
{ {
if ((millis() - _tm_bengstop) > TM_INSTORE_WEIGHT_DELAY) if ((millis() - _tm_bengstop) > TM_INSTORE_WEIGHT_DELAY)
{ {
@ -446,7 +435,7 @@ void loop()
} }
} }
} }
check_tare(); //检查看是否需要校准称重 check_tare(); // 检查看是否需要校准称重
// 检测执行初始化工作 // 检测执行初始化工作
checkinited(); checkinited();
/////////////////////////////////MQTT_语音_MAVLINK 部分 /////////////////////////////////MQTT_语音_MAVLINK 部分
@ -456,13 +445,12 @@ void loop()
fc.mqttLoop(topicPub, topicPubCount); fc.mqttLoop(topicPub, topicPubCount);
/////////////////////////////////MQTT_语音_MAVLINK 部分结束 /////////////////////////////////MQTT_语音_MAVLINK 部分结束
delay(1); delay(1);
} }
// 在核心0上执行耗时长的低优先级的 // 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters) void Task1(void *pvParameters)
{ {
// 初始化称重传感器 // 初始化称重传感器
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN,HX711_GAIN); //2号机用的B通道,1号用的A通道 scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN, HX711_GAIN); // 2号机用的B通道,1号用的A通道
scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
scale.tare(); // 重置为0 scale.tare(); // 重置为0
@ -475,13 +463,13 @@ void Task1(void *pvParameters)
scale.tare(); scale.tare();
} }
pullweight = get_pullweight(); pullweight = get_pullweight();
//显示重量 // 显示重量
//printf("pullweight: %d \n", pullweight); // printf("pullweight: %d \n", pullweight);
/*保持mqtt心跳*/ /*保持mqtt心跳*/
// fc.mqttLoop(topicSub, topicSubCount); // fc.mqttLoop(topicSub, topicSubCount);
///px1 /// px1
// if (fc.checkWiFiStatus()) // if (fc.checkWiFiStatus())
/*保持mqtt心跳,如果Mqtt没有连接会自动连接*/ /*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
// fc.mqttLoop(topicSub, topicSubCount); // fc.mqttLoop(topicSub, topicSubCount);
@ -490,7 +478,7 @@ void Task1(void *pvParameters)
} }
void sendinfo() // 每500ms发送状态信息 void sendinfo() // 每500ms发送状态信息
{ {
//sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight); // sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
showinfo(); showinfo();
} }
@ -530,7 +518,7 @@ void showledidel()
led_show(255, 255, 0); // 黄色 led_show(255, 255, 0); // 黄色
break; break;
} }
case HookStatus::HS_Down : case HookStatus::HS_Down:
case HookStatus::HS_DownSlow: case HookStatus::HS_DownSlow:
case HookStatus::HS_WaitUnhook: case HookStatus::HS_WaitUnhook:
{ {
@ -558,10 +546,8 @@ void showledidel()
led_show(255, 255, 255); // 白色 led_show(255, 255, 255); // 白色
break; break;
} }
} }
/* /*
switch (motocontrol.getcontrolstatus().motostatus) switch (motocontrol.getcontrolstatus().motostatus)
{ {
@ -612,9 +598,9 @@ void showledidel()
// 顶部按钮，检测是否到顶部 // 顶部按钮，检测是否到顶部
void ctbtn_pressstart() void ctbtn_pressstart()
{ {
ESP_LOGD(MOUDLENAME,"Top_pressstart"); ESP_LOGD(MOUDLENAME, "Top_pressstart");
//只在上升时停止 // 只在上升时停止
if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore)) if ((motocontrol.gethooktatus() == HS_UnInit) || (motocontrol.gethooktatus() == HS_Up) || (motocontrol.gethooktatus() == HS_InStore))
{ {
_tm_bengstop = millis(); _tm_bengstop = millis();
_bengstop = true; _bengstop = true;
@ -623,14 +609,14 @@ void ctbtn_pressstart()
// 顶部按钮，抬起 // 顶部按钮，抬起
void ctbtn_pressend() void ctbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME,"Top_pressend"); ESP_LOGD(MOUDLENAME, "Top_pressend");
set_locked(false); set_locked(false);
_bengstop = false; _bengstop = false;
} }
void down_action(float motospeed,float length = 0.0f) void down_action(float motospeed, float length = 0.0f)
{ {
ESP_LOGD(MOUDLENAME,"Down_action sp:%.2f len:%.2f cm",motospeed,length); ESP_LOGD(MOUDLENAME, "Down_action sp:%.2f len:%.2f cm", motospeed, length);
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{ {
@ -638,14 +624,15 @@ void down_action(float motospeed,float length = 0.0f)
} }
else else
{ {
//如果在自动放线状态，只是恢复自动放线 // 如果在自动放线状态，只是恢复自动放线
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);
fc.playText("放线"); fc.playText("放线");
} }
} }
@ -653,39 +640,41 @@ void down_action(float motospeed,float length = 0.0f)
void up_action(float motospeed) void up_action(float motospeed)
{ {
ESP_LOGD(MOUDLENAME,"Up_action sp:%.2f",motospeed); ESP_LOGD(MOUDLENAME, "Up_action sp:%.2f", motospeed);
if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop) if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
{ {
motocontrol.stopautodown(); //终止自动放线 motocontrol.stopautodown(); // 终止自动放线
motocontrol.setspeed(motospeed); motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Up); motocontrol.moto_run(MotoStatus::MS_Up);
fc.playText("收线"); fc.playText("收线");
}else }
else
motocontrol.stoprun(); motocontrol.stoprun();
} }
// 放线按钮--单击 // 放线按钮--单击
void downbtn_click() void downbtn_click()
{ {
ESP_LOGD(MOUDLENAME,"Down_click"); ESP_LOGD(MOUDLENAME, "Down_click");
_checkweightcal=true;//检测是否需要校准称重传感器--在下放停止时检测 _checkweightcal = true; // 检测是否需要校准称重传感器--在下放停止时检测
_checkweighttimes=0; _checkweighttimes = 0;
down_action(SPEED_BTN_SLOW,10); down_action(SPEED_BTN_SLOW, 10);
} }
// 放线按钮--双击 // 放线按钮--双击
void downbtn_dbclick() void downbtn_dbclick()
{ {
ESP_LOGD(MOUDLENAME,"Down_dbclick"); ESP_LOGD(MOUDLENAME, "Down_dbclick");
down_action(SPEED_BTN_MID); down_action(SPEED_BTN_MID);
} }
// 放线按钮--长按 // 放线按钮--长按
void downbtn_pressstart() void downbtn_pressstart()
{ {
ESP_LOGD(MOUDLENAME,"Down_pressstart"); ESP_LOGD(MOUDLENAME, "Down_pressstart");
//两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce > 2)
if (btn_pressatonce==2) btn_pressatonce = 2;
if (btn_pressatonce == 2)
{ {
btn_presssatonce(); btn_presssatonce();
return; return;
@ -695,10 +684,11 @@ void downbtn_pressstart()
// 放线按钮--长按抬起 // 放线按钮--长按抬起
void downbtn_pressend() void downbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME,"Down_pressend"); ESP_LOGD(MOUDLENAME, "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();
} }
@ -707,32 +697,33 @@ void downbtn_pressend()
void upbtn_click() void upbtn_click()
{ {
// fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click"); // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
ESP_LOGD(MOUDLENAME,"UP_click"); ESP_LOGD(MOUDLENAME, "UP_click");
up_action(SPEED_BTN_SLOW); up_action(SPEED_BTN_SLOW);
} }
// 收线按钮-双击 // 收线按钮-双击
void upbtn_dbclick() void upbtn_dbclick()
{ {
ESP_LOGD(MOUDLENAME,"UP_dbclick"); ESP_LOGD(MOUDLENAME, "UP_dbclick");
up_action(SPEED_BTN_MID); up_action(SPEED_BTN_MID);
} }
// 两个按钮同时按下 // 两个按钮同时按下
void btn_presssatonce() void btn_presssatonce()
{ {
ESP_LOGD(MOUDLENAME,"UP_presssatonce"); ESP_LOGD(MOUDLENAME, "UP_presssatonce");
led_show(255,255, 255); // 高亮一下 led_show(255, 255, 255); // 高亮一下
fc.playText("发送对频信息"); fc.playText("发送对频信息");
///px1 /// px1
//fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); // fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
} }
// 收线按钮-长按 // 收线按钮-长按
void upbtn_pressstart() void upbtn_pressstart()
{ {
ESP_LOGD(MOUDLENAME,"UP_pressstart"); ESP_LOGD(MOUDLENAME, "UP_pressstart");
//两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce > 2)
if (btn_pressatonce==2) btn_pressatonce = 2;
if (btn_pressatonce == 2)
{ {
btn_presssatonce(); btn_presssatonce();
return; return;
@ -742,42 +733,45 @@ void upbtn_pressstart()
// 收线按钮-长按抬起 // 收线按钮-长按抬起
void upbtn_pressend() void upbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME,"UP_pressend"); ESP_LOGD(MOUDLENAME, "UP_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce < 0)
btn_pressatonce = 0;
motocontrol.stoprun(); motocontrol.stoprun();
} }
//自动下放 // 自动下放
void Hook_autodown(float length_cm) void Hook_autodown(float length_cm)
{ {
if (motocontrol.gethooktatus()==HS_Locked) if (motocontrol.gethooktatus() == HS_Locked)
{ {
ESP_LOGD(MOUDLENAME,"Hook_autodown %.2f cm",length_cm); ESP_LOGD(MOUDLENAME, "Hook_autodown %.2f cm", length_cm);
fc.Camera_action_down(); fc.Camera_action_down();
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
}else }
ESP_LOGE(MOUDLENAME,"autodown fault, not HS_Locked "); else
ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked ");
} }
void Hook_stop() void Hook_stop()
{ {
ESP_LOGD(MOUDLENAME,"Hook_stop"); ESP_LOGD(MOUDLENAME, "Hook_stop");
motocontrol.stoprun(); motocontrol.stoprun();
} }
void Hook_resume() void Hook_resume()
{ {
if (motocontrol.gethooktatus()==HS_Stop) if (motocontrol.gethooktatus() == HS_Stop)
{ {
ESP_LOGD(MOUDLENAME,"Hook_resume"); ESP_LOGD(MOUDLENAME, "Hook_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
}else }
ESP_LOGE(MOUDLENAME,"resume fault, not HS_Stop "); else
ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop ");
} }
void Hook_recovery() void Hook_recovery()
{ {
ESP_LOGD(MOUDLENAME,"Hook_recovery"); ESP_LOGD(MOUDLENAME, "Hook_recovery");
if (motocontrol.gethooktatus()!=HS_Locked) if (motocontrol.gethooktatus() != HS_Locked)
{ {
motocontrol.stoprun(); motocontrol.stoprun();
up_action(SPEED_HOOK_UP); up_action(SPEED_HOOK_UP);
@ -787,30 +781,28 @@ void Hook_recovery()
// 测试按钮 // 测试按钮
void testbtn_click() void testbtn_click()
{ {
ESP_LOGD(MOUDLENAME,"testbtn_click"); ESP_LOGD(MOUDLENAME, "testbtn_click");
switch (motocontrol.gethooktatus()) switch (motocontrol.gethooktatus())
{ {
case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
{ {
ESP_LOGD(MOUDLENAME,"moto_goodsdown"); ESP_LOGD(MOUDLENAME, "moto_goodsdown");
motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 motocontrol.moto_goodsdown(22); // 二楼340 //桌子40
break; break;
} }
case HS_Stop: case HS_Stop:
{ {
ESP_LOGD(MOUDLENAME,"moto_resume"); ESP_LOGD(MOUDLENAME, "moto_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
break; break;
} }
default: default:
{ {
ESP_LOGD(MOUDLENAME,"stop"); ESP_LOGD(MOUDLENAME, "stop");
motocontrol.stoprun(); motocontrol.stoprun();
} }
} }
} }
//////////////////////////////MQTT_语音_MAVLINK 部分 //////////////////////////////MQTT_语音_MAVLINK 部分

8
src/moto.cpp
View File

@ -5,20 +5,20 @@
uint8_t CaninBuffer[8]; // 接收指令缓冲区 uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis; moto_measure_t moto_chassis;
PID_TypeDef moto_pid; PID_TypeDef moto_pid;
static const char* MOUDLENAME = "MOTO"; static const char *MOUDLENAME = "MOTO";
moto::moto() moto::moto()
{ {
_closed = true; _closed = true;
} }
bool moto::init() bool moto::init()
{ {
ESP_LOGD(MOUDLENAME,"init moto"); ESP_LOGD(MOUDLENAME, "init moto");
pid_init(); pid_init();
CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
// start the CAN bus at 500 kbps // start the CAN bus at 500 kbps
if (!CAN.begin(1000E3)) if (!CAN.begin(1000E3))
{ {
ESP_LOGE(MOUDLENAME,"Starting CAN failed!"); ESP_LOGE(MOUDLENAME, "Starting CAN failed!");
return false; return false;
} }
CAN.onReceive(onReceive); CAN.onReceive(onReceive);
@ -30,7 +30,7 @@ moto::~moto()
void moto::update() void moto::update()
{ {
//printf("u1:%d\n",_closed); // printf("u1:%d\n",_closed);
if (!_closed) if (!_closed)
{ {
unsigned long dt = millis() - moto_chassis.starttime; // 时间差 unsigned long dt = millis() - moto_chassis.starttime; // 时间差

110
src/motocontrol.cpp
View File

@ -6,7 +6,7 @@
#include "config.h" #include "config.h"
#define DEBUG_OUT false #define DEBUG_OUT false
static const char* MOUDLENAME = "MOTO_C"; static const char *MOUDLENAME = "MOTO_C";
Motocontrol::Motocontrol() Motocontrol::Motocontrol()
{ {
@ -23,7 +23,7 @@ Motocontrol::Motocontrol()
_weightalign = false; _weightalign = false;
_overweightcount = 0; _overweightcount = 0;
_notweightcount = 0; _notweightcount = 0;
_controlstatus.is_autogoodsdown=false; _controlstatus.is_autogoodsdown = false;
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机 _servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
_unblocktimes = 0; _unblocktimes = 0;
} }
@ -53,53 +53,51 @@ void Motocontrol::setweight(int pullweight) // 设置重量
} }
String Motocontrol::gethooktatus_str(bool chstr) String Motocontrol::gethooktatus_str(bool chstr)
{ {
String hookstatusstr="未知"; String hookstatusstr = "未知";
String hookstatusstr_en="unknown"; String hookstatusstr_en = "unknown";
switch (_hooksstatus) switch (_hooksstatus)
{ {
case HS_UnInit: case HS_UnInit:
hookstatusstr="未初始化"; hookstatusstr = "未初始化";
hookstatusstr_en="HS_UnInit"; hookstatusstr_en = "HS_UnInit";
break; break;
case HS_Down: case HS_Down:
hookstatusstr="放钩"; hookstatusstr = "放钩";
hookstatusstr_en="HS_Down"; hookstatusstr_en = "HS_Down";
break; break;
case HS_DownSlow: case HS_DownSlow:
hookstatusstr="慢速放钩"; hookstatusstr = "慢速放钩";
hookstatusstr_en="HS_DownSlow"; hookstatusstr_en = "HS_DownSlow";
break; break;
case HS_WaitUnhook: case HS_WaitUnhook:
hookstatusstr="等待脱钩"; hookstatusstr = "等待脱钩";
hookstatusstr_en="HS_WaitUnhook"; hookstatusstr_en = "HS_WaitUnhook";
break; break;
case HS_Up: case HS_Up:
hookstatusstr="回收"; hookstatusstr = "回收";
hookstatusstr_en="HS_Up"; hookstatusstr_en = "HS_Up";
break; break;
case HS_InStore: case HS_InStore:
hookstatusstr="入仓中"; hookstatusstr = "入仓中";
hookstatusstr_en="HS_InStore"; hookstatusstr_en = "HS_InStore";
break; break;
case HS_Locked: case HS_Locked:
hookstatusstr="已锁定"; hookstatusstr = "已锁定";
hookstatusstr_en="HS_Locked"; hookstatusstr_en = "HS_Locked";
break; break;
case HS_Stop: case HS_Stop:
hookstatusstr="停止"; hookstatusstr = "停止";
hookstatusstr_en="HS_Stop"; hookstatusstr_en = "HS_Stop";
break; break;
default: default:
hookstatusstr="未知"; hookstatusstr = "未知";
hookstatusstr_en="HS_UnInit"; hookstatusstr_en = "HS_UnInit";
break; break;
} }
if (chstr) if (chstr)
return hookstatusstr; return hookstatusstr;
else else
return hookstatusstr_en; return hookstatusstr_en;
} }
void Motocontrol::checkgoods() // 检测是否超重 void Motocontrol::checkgoods() // 检测是否超重
@ -141,13 +139,13 @@ void Motocontrol::checkgoods() // 检测是否超重
} }
void Motocontrol::lockservo() // 锁定舵机 void Motocontrol::lockservo() // 锁定舵机
{ {
ESP_LOGD(MOUDLENAME,"start_lockservo"); ESP_LOGD(MOUDLENAME, "start_lockservo");
_servotatus = SS_WaitMotoStop; _servotatus = SS_WaitMotoStop;
_tm_servotatus = millis(); _tm_servotatus = millis();
} }
void Motocontrol::unlockservo() // 解锁舵机 void Motocontrol::unlockservo() // 解锁舵机
{ {
ESP_LOGD(MOUDLENAME,"unlockservo"); ESP_LOGD(MOUDLENAME, "unlockservo");
// 解锁操作 // 解锁操作
_lockservo->write(SERVO_UNLOCKPOS); _lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
@ -157,11 +155,11 @@ void Motocontrol::unlockservo() // 解锁舵机
} }
void Motocontrol::stoprun(float acctime) // 停止 void Motocontrol::stoprun(float acctime) // 停止
{ {
ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime); ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", acctime);
_moto_dji.settime_acc(acctime); _moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f); _moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop; _controlstatus.motostatus = MS_Stop;
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) ) if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) && (_hooksstatus != HS_Locked))
{ {
_hooksstatus_prv = _hooksstatus; _hooksstatus_prv = _hooksstatus;
sethooksstatus(HS_Stop); sethooksstatus(HS_Stop);
@ -174,8 +172,8 @@ void Motocontrol::stopautodown()
} }
void Motocontrol::sethooksstatus(HookStatus hookstatus) void Motocontrol::sethooksstatus(HookStatus hookstatus)
{ {
_hooksstatus=hookstatus; _hooksstatus = hookstatus;
ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false)); ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false));
} }
void Motocontrol::setlocked(bool locked) void Motocontrol::setlocked(bool locked)
{ {
@ -199,7 +197,6 @@ int16_t Motocontrol::getlength() // 得到长度
return 0; return 0;
} }
// 重量传感器已经校准 // 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign) void Motocontrol::weightalign(bool weightalign)
{ {
@ -220,14 +217,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 如果没有货物--开始回收 // 如果没有货物--开始回收
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME, "Not goods wait %d ms for unhook", HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
break; break;
} }
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt)) if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
{ {
ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt); ESP_LOGD(MOUDLENAME, "HS_Down target is arrived curr: %.2f", mf.output_round_cnt);
sethooksstatus(HS_DownSlow); sethooksstatus(HS_DownSlow);
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(SPEED_HOOK_SLOW); set_hook_speed(SPEED_HOOK_SLOW);
@ -239,7 +236,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME, "Not havegoods wait %d ms unhook", HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
} }
@ -250,7 +247,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{ {
ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup"); ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup");
_moto_dji.settime_acc(1000); _moto_dji.settime_acc(1000);
set_hook_speed(-SPEED_HOOK_UP); set_hook_speed(-SPEED_HOOK_UP);
sethooksstatus(HS_Up); sethooksstatus(HS_Up);
@ -316,14 +313,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
if (!checkservoblock() || (_unblocktimes > 2)) if (!checkservoblock() || (_unblocktimes > 2))
{ {
ESP_LOGD(MOUDLENAME,"Close moto"); ESP_LOGD(MOUDLENAME, "Close moto");
_moto_dji.close(); _moto_dji.close();
_servotatus = SS_ServoLocked; _servotatus = SS_ServoLocked;
} }
else else
// 堵转 // 堵转
{ {
ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto"); ESP_LOGD(MOUDLENAME, "SS servoblock unlock servo and turn moto");
_tm_servotatus = millis(); _tm_servotatus = millis();
_servotatus = SS_WaitUnBlock; _servotatus = SS_WaitUnBlock;
// 写一个不会堵转的位置 // 写一个不会堵转的位置
@ -339,7 +336,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if ((millis() - _tm_servotatus) > TM_UNBLOCK) if ((millis() - _tm_servotatus) > TM_UNBLOCK)
{ {
ESP_LOGD(MOUDLENAME,"SS lock servo"); ESP_LOGD(MOUDLENAME, "SS lock servo");
// 继续锁定等待舵机检测 // 继续锁定等待舵机检测
_lockservo->write(SERVO_LOCKPOS); _lockservo->write(SERVO_LOCKPOS);
_servotatus = SS_WaitServo; _servotatus = SS_WaitServo;
@ -373,12 +370,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(-SPEED_INSTORE); set_hook_speed(-SPEED_INSTORE);
_controlstatus.is_instore = true; _controlstatus.is_instore = true;
ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm); ESP_LOGD(MOUDLENAME, "Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
} }
} }
else else
{ {
//已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支 // 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
if (_hooksstatus != HS_InStore) if (_hooksstatus != HS_InStore)
_controlstatus.is_instore = false; _controlstatus.is_instore = false;
} }
@ -389,7 +386,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt > _target_cnt) if (mf.output_round_cnt > _target_cnt)
{ {
ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME, "stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -397,7 +394,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt < _target_cnt) if (mf.output_round_cnt < _target_cnt)
{ {
ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME, "stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -432,7 +429,8 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdownresume() void Motocontrol::moto_goodsdownresume()
{ {
if (!_controlstatus.is_autogoodsdown) return; if (!_controlstatus.is_autogoodsdown)
return;
if (_hooksstatus == HS_Stop) if (_hooksstatus == HS_Stop)
{ {
_runspeed = _goods_speed; _runspeed = _goods_speed;
@ -449,29 +447,29 @@ void Motocontrol::moto_goodsdownresume()
// _moto_dji.setspeedtarget(_goods_speed); // _moto_dji.setspeedtarget(_goods_speed);
} }
} }
//长度cm // 长度cm
void Motocontrol::moto_goodsdown(float length) void Motocontrol::moto_goodsdown(float length)
{ {
if (length < 0.0) if (length < 0.0)
{ {
ESP_LOGD(MOUDLENAME,"length<0 fault"); ESP_LOGD(MOUDLENAME, "length<0 fault");
return; return;
} }
// 没设置零点 // 没设置零点
if (!_controlstatus.is_setzero) if (!_controlstatus.is_setzero)
{ {
ESP_LOGD(MOUDLENAME,"not lengthzero fault"); ESP_LOGD(MOUDLENAME, "not lengthzero fault");
return; return;
} }
if (!_weightalign) if (!_weightalign)
{ {
ESP_LOGD(MOUDLENAME,"weight not align fault"); ESP_LOGD(MOUDLENAME, "weight not align fault");
return; return;
} }
// 没挂东西 // 没挂东西
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight); ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight);
return; return;
} }
@ -487,12 +485,12 @@ void Motocontrol::moto_goodsdown(float length)
} }
else else
{ {
float tarleng=length; float tarleng = length;
if (length>HOOK_SLOWDOWN_LENGTH) if (length > HOOK_SLOWDOWN_LENGTH)
tarleng-=HOOK_SLOWDOWN_LENGTH; tarleng -= HOOK_SLOWDOWN_LENGTH;
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt; _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER; _goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
ESP_LOGD(MOUDLENAME,"start down %.2f cm,tar:%.2f",tarleng,_goods_down_target_cnt); ESP_LOGD(MOUDLENAME, "start down %.2f cm,tar:%.2f", tarleng, _goods_down_target_cnt);
setspeed(SPEED_HOOK_FAST, TM_ACC_HS); setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST; _goods_speed = SPEED_HOOK_FAST;
} }
@ -512,25 +510,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 运动中暂时不管 // 运动中暂时不管
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up)) if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
{ {
ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up"); ESP_LOGD(MOUDLENAME, "motostatus is MS_Down\\MS_Up");
return; return;
} }
// 没设置速度不转 // 没设置速度不转
if (_controlstatus.speed_targe == 0) if (_controlstatus.speed_targe == 0)
{ {
ESP_LOGD(MOUDLENAME,"not set speed_targe"); ESP_LOGD(MOUDLENAME, "not set speed_targe");
return; return;
} }
if (updown == MS_Up) if (updown == MS_Up)
{ {
if (_controlstatus.is_toplocked) if (_controlstatus.is_toplocked)
{ {
ESP_LOGD(MOUDLENAME,"is_toplocked return"); ESP_LOGD(MOUDLENAME, "is_toplocked return");
return; return;
} }
if (_controlstatus.is_overweight) if (_controlstatus.is_overweight)
{ {
ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight); ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight);
return; return;
} }
sethooksstatus(HS_Up); sethooksstatus(HS_Up);

11
src/motocontrol.h
View File

@ -29,8 +29,8 @@
#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 1000 // 1920 // 舵机锁定位置
#define SERVO_UNLOCKPOS 1120 //1800 // 舵机解锁位置 #define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 #define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克) #define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
@ -55,7 +55,7 @@ typedef struct
bool is_toplocked; // 已到顶锁住 bool is_toplocked; // 已到顶锁住
bool is_overweight; // 是否超重 bool is_overweight; // 是否超重
bool is_havegoods; // 是否有货物 bool is_havegoods; // 是否有货物
bool is_autogoodsdown; //正在自动放线中 bool is_autogoodsdown; // 正在自动放线中
float zero_cnt; // 0长度位置 float zero_cnt; // 0长度位置
float speed_targe; // 当前目标速度 float speed_targe; // 当前目标速度
MotoStatus motostatus; // 电机运行状态 MotoStatus motostatus; // 电机运行状态
@ -111,12 +111,13 @@ 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(); // 析构函数
void setspeed(float motospeed, float acctime = -1); // 设置速度 void setspeed(float motospeed, float acctime = -1); // 设置速度
void stoprun(float acctime = 0); // 停止运行 void stoprun(float acctime = 0); // 停止运行
void stopautodown(); //停止自动下放模式 void stopautodown(); // 停止自动下放模式
void setzero(); // 设置0长度位置 void setzero(); // 设置0长度位置
int16_t getlength(); // 得到长度 int16_t getlength(); // 得到长度
void update(); // 更新 void update(); // 更新
@ -130,7 +131,7 @@ public:
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收 void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
void moto_goodsdownresume(); // 恢复自动放线 void moto_goodsdownresume(); // 恢复自动放线
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
String gethooktatus_str(bool chstr=true); // 得到挂钩状态(中英文） String gethooktatus_str(bool chstr = true); // 得到挂钩状态(中英文）
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
}; };