#include "MavlinkReciver.h"
|
#include <modules/my_task_creator/TaskCreator.h>
|
MavlinkReciver::MavlinkReciver()
|
{
|
mSerial=&Serial1;
|
}
|
|
MavlinkReciver::~MavlinkReciver()
|
{
|
}
|
|
void MavlinkReciver::Init(uint32_t baud)
|
{
|
mBaudRate=baud;
|
#ifdef ARDUINO_WT32_ETH01
|
mSerial=&Serial1;
|
mSerial->begin(mBaudRate,SERIAL_8N1,HWSerial1Eth_rx_pin,HWSerial1Eth_tx_pin);
|
mSerial->setPins(HWSerial1Eth_rx_pin,HWSerial1Eth_tx_pin);//eth board
|
#elif defined ARDUINO_ESP32_DEV
|
mSerial=&Serial1;
|
mSerial->begin(mBaudRate,SERIAL_8N1,HWSerial1_rx_pin,HWSerial1_tx_pin);
|
#endif
|
mavlink_set_proto_version(MAVLINK_COMM_0,2);
|
}
|
|
void MavlinkReciver::TaskMavlinkRx(void *pvParameters)
|
{
|
log_i("Task started");
|
TaskCreator *mCreator = (TaskCreator *)pvParameters;
|
MavlinkReciver *obj=&(mCreator->mReciver);
|
MavlinkTransmitter *mTransmitter=&(mCreator->mTransmitter);
|
DefaultParametrs *mParams=&(mCreator->mParams);
|
EepromParams *mEeprom=&(mCreator->mEeprom);
|
StateMachine *mStateMachine=&(mCreator->mStateMachine);
|
TrackerRxTx *mTracker=&(mCreator->mTracker);
|
vTaskDelay(100);
|
TickType_t mLastTick=xTaskGetTickCount();
|
TickType_t mLastTick1=mLastTick;
|
while (1)
|
{
|
int len=obj->mSerial->available();
|
if(len>2*MAVLINK_MAX_PACKET_LEN)
|
{
|
obj->mSerial->read(obj->mBufferIn,2*MAVLINK_MAX_PACKET_LEN);
|
log_i("MavlinkReciver buf over");
|
continue;
|
}
|
if(len>0)
|
{
|
mavlink_status_t status;
|
mavlink_message_t msg;
|
len=obj->mSerial->read(obj->mBufferIn,len);
|
for(int i=0;i<len;i++)
|
{
|
if(mavlink_parse_char(MAVLINK_COMM_0, obj->mBufferIn[i], &msg, &status))
|
{
|
if(!obj->mStatus)
|
{
|
switch (msg.msgid)
|
{
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
{
|
|
mavlink_heartbeat_t hearbit={0};
|
mavlink_msg_heartbeat_decode(&msg, &hearbit);
|
|
if ((hearbit.system_status == MAV_STATE_BOOT)
|
||(hearbit.system_status == MAV_STATE_POWEROFF)
|
||(hearbit.system_status == MAV_STATE_CALIBRATING))
|
{
|
obj->mStatus=0;
|
log_i("Auttopilot status cahnged=%d", hearbit.system_status);
|
log_i("Reboot");
|
obj->reboot();
|
}
|
else
|
{
|
if (!obj->mSysId && msg.sysid != 255 && msg.sysid != 83 && msg.compid == MAV_COMP_ID_AUTOPILOT1)
|
{
|
obj->mSysId = msg.sysid;
|
mTransmitter->SetSysId(msg.sysid);
|
mStateMachine->SetSysID(msg.sysid);
|
log_i("Mavlink set sysid=%d", msg.sysid);
|
mTransmitter->SendText("Navigation computer start!",MAV_SEVERITY_NOTICE);
|
mTransmitter->SendText(SOFT_VER,MAV_SEVERITY_NOTICE);
|
|
mTransmitter->SendStreamRateAll();
|
obj->mStatus=1;
|
}
|
|
}
|
|
}
|
break;
|
|
default:
|
break;
|
}
|
}
|
else
|
{
|
switch (msg.msgid)
|
{
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
{
|
if ( msg.sysid != obj->mSysId)
|
{
|
break;
|
}
|
mavlink_heartbeat_t hearbit;
|
mavlink_msg_heartbeat_decode(&msg, &hearbit);
|
if(hearbit.custom_mode!=0)
|
{
|
bool currentlyArmed = hearbit.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY;
|
if(obj->mLockMode)
|
{
|
mStateMachine->SetBaseMode(hearbit.base_mode,PLANE_MODE_GUIDED);
|
obj->mLockMode=false;
|
}
|
else
|
{
|
mStateMachine->SetBaseMode(hearbit.base_mode,hearbit.custom_mode);
|
}
|
obj->mBaseMode=hearbit.base_mode;
|
//log_i("Mavlink arm=%d", currentlyArmed);
|
}
|
if ((hearbit.system_status == MAV_STATE_BOOT)
|
||(hearbit.system_status == MAV_STATE_POWEROFF)
|
||(hearbit.system_status == MAV_STATE_CALIBRATING))
|
{
|
obj->mStatus=0;
|
obj->mSysId=0;
|
mTransmitter->SetSysId(0);
|
log_i("Auttopilot status cahnged=%d", hearbit.system_status);
|
log_i("Reboot");
|
obj->reboot();
|
}
|
|
if (hearbit.custom_mode != 0)
|
{
|
if (obj->mAutopilotMode != hearbit.custom_mode)
|
{
|
|
obj->mAutopilotMode = hearbit.custom_mode;
|
obj->mBaseMode=hearbit.base_mode;
|
log_i("Autopilot mode changed=%d", hearbit.custom_mode);
|
}
|
}
|
}
|
break;
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
|
{
|
uint8_t comp=mavlink_msg_param_request_list_get_target_component(&msg);
|
log_i("PARAM_REQUEST_LIST for %i",comp);
|
if(comp!=MY_COMP_ID)
|
{
|
|
continue;
|
}
|
|
uint16_t nums=mParams->GetParamSize();
|
|
for(int i=0;i<nums;i++)
|
{
|
mavlink_param_value_t val={0};
|
String name=mParams->GetName(i);
|
for(int j=0;j<name.length();j++)
|
if(j<16)
|
val.param_id[j]=name[j];
|
if(name.length()<15)
|
val.param_id[name.length()]=0;
|
val.param_count=nums;
|
val.param_index=i;
|
val.param_type=MAV_PARAM_TYPE_REAL32;
|
val.param_value=mParams->GetValue(i);
|
|
mTransmitter->SendParameter(val);
|
|
}
|
}
|
break;
|
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
|
{
|
uint8_t comp=mavlink_msg_param_request_read_get_target_component(&msg);
|
log_i("PARAM_REQUEST_READ for %i",comp);
|
if(comp!=MY_COMP_ID)
|
{
|
|
continue;
|
}
|
mavlink_param_request_read_t param_to_read = {0};
|
mavlink_msg_param_request_read_decode(&msg,¶m_to_read);
|
log_i("Param request:%s %d\n\r",param_to_read.param_id,param_to_read.param_index);
|
mavlink_param_value_t val={0};
|
uint16_t nums=mParams->GetParamSize();
|
uint16_t i = param_to_read.param_index;
|
if(i>nums-1)
|
{
|
log_i("BAD PARAM_REQUEST_READ NUM for %i",i);
|
continue;
|
}
|
String name = mParams->GetName(i);
|
for (int j = 0; j < name.length(); j++)
|
if (j < 16)
|
val.param_id[j] = name[j];
|
if(name.length()<15)
|
val.param_id[name.length()]=0;
|
val.param_count = nums;
|
val.param_index = i;
|
val.param_type = MAV_PARAM_TYPE_REAL32;
|
val.param_value = mParams->GetValue(i);
|
|
mTransmitter->SendParameter(val);
|
}
|
break;
|
case MAVLINK_MSG_ID_ATTITUDE:
|
{
|
static uint16_t cnt1=0;
|
cnt1++;
|
mavlink_attitude_t attitude;
|
mavlink_msg_attitude_decode(&msg, &attitude);
|
mStateMachine->SetRPY(attitude.roll,attitude.pitch,attitude.yaw);
|
if(cnt1==1200)
|
{
|
uint32_t dt=xTaskGetTickCount()-mLastTick1;
|
dt/=1200;
|
|
mLastTick1=xTaskGetTickCount();
|
cnt1=0;
|
String tmp("attitude dt=");
|
tmp+=dt;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
log_i("attitude dt=%d",dt);
|
}
|
}
|
break;
|
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
|
{
|
static uint16_t cnt=0;
|
cnt++;
|
mavlink_global_position_int_t pos;
|
mavlink_msg_global_position_int_decode(&msg, &pos);
|
mStateMachine->SetPos(pos.lat*1e-7,pos.lon*1e-7,pos.relative_alt/1000.0);
|
if(cnt==1000)
|
{
|
uint32_t dt=xTaskGetTickCount()-mLastTick;
|
dt/=1000;
|
|
mLastTick=xTaskGetTickCount();
|
cnt=0;
|
String tmp("GLOBAL_POSITION_INT dt=");
|
tmp+=dt;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
log_i("GLOBAL_POSITION_INT dt=%d",dt);
|
}
|
|
}
|
break;
|
case MAVLINK_MSG_ID_PARAM_SET:
|
{
|
mavlink_param_set_t param_set = {0};
|
|
mavlink_msg_param_set_decode(&msg, ¶m_set);
|
log_v("Mavlink param set:%s %f\n\r",param_set.param_id,(float)param_set.param_value);
|
String name(param_set.param_id);
|
if(name.length()>16)
|
{
|
name.remove(16,name.length()-16);
|
}
|
uint16_t idx=mParams->SetParam(name,param_set.param_value);
|
uint16_t nums=mParams->GetParamSize();
|
if(idx<nums)
|
{
|
mEeprom->WriteParam(idx);
|
mavlink_param_value_t val={0};
|
|
for (int j = 0; j < name.length(); j++)
|
if (j < 16)
|
val.param_id[j] = name[j];
|
val.param_id[name.length()]=0;
|
val.param_count = nums;
|
val.param_index = idx;
|
val.param_type = MAV_PARAM_TYPE_REAL32;
|
val.param_value = mParams->GetValue(idx);
|
|
mTransmitter->SendParameter(val);
|
if(name=="CTRL_REBOOT")
|
{
|
mTransmitter->SendText("Navigation computer reboot!",MAV_SEVERITY_CRITICAL);
|
}
|
if(name=="CTRL_TEST_ALT")
|
{
|
uint8_t m=mParams->GetValue("CTRL_TEST_MODE");
|
mStateMachine->SetTestMode(m,val.param_value);
|
}
|
if(name=="CTRL_K_YAW")
|
{
|
|
float kp=mParams->GetValue("CTRL_K_YAW");
|
mStateMachine->SetK(kp);
|
}
|
if(name=="CTRL_I_YAW")
|
{
|
|
float kp=mParams->GetValue("CTRL_I_YAW");
|
mStateMachine->SetI(kp);
|
}
|
if(name=="CTRL_D_YAW")
|
{
|
|
float kp=mParams->GetValue("CTRL_D_YAW");
|
mStateMachine->SetD(kp);
|
}
|
if(name=="CTRL_I_MAX")
|
{
|
|
float kp=mParams->GetValue("CTRL_I_MAX");
|
mStateMachine->SetImax(kp);
|
}
|
if(name=="CTRL_K_PITCH")
|
{
|
|
float kp=mParams->GetValue("CTRL_K_PITCH");
|
mStateMachine->SetKpitch(kp);
|
}
|
if(name=="CTRL_USE_KPITCH")
|
{
|
bool m=mParams->GetValue("CTRL_USE_KPITCH");
|
mStateMachine->SetUseKpitch(m);
|
}
|
if(name=="VIDEO_ENABLE")
|
{
|
bool m=mParams->GetValue("VIDEO_ENABLE");
|
mStateMachine->SetTrackerEnable(m);
|
}
|
if(name=="CTRL_ATTACK_SPD")
|
{
|
float m=mParams->GetValue("CTRL_ATTACK_SPD");
|
mStateMachine->SetAttackSpeed(m);
|
}
|
if(name=="CAM_SRV_CH")
|
{
|
float m=mParams->GetValue("CAM_SRV_CH");
|
mStateMachine->SetCamSrvCH(m);
|
}
|
if(name=="CTRL_SRV_CH")
|
{
|
float m=mParams->GetValue("CTRL_SRV_CH");
|
mStateMachine->SetCTRLSrvCH(m);
|
}
|
if(name=="CAM_SRV_MAX")
|
{
|
float m=mParams->GetValue("CAM_SRV_MAX");
|
mStateMachine->SetCamSrvMax(m);
|
}
|
if(name=="CAM_SRV_MIN")
|
{
|
float m=mParams->GetValue("CAM_SRV_MIN");
|
mStateMachine->SetCamSrvMin(m);
|
}
|
if(name=="CAM_ANG_CRUISE")
|
{
|
float m=mParams->GetValue("CAM_ANG_CRUISE");
|
mStateMachine->SetCamAngCruise(m);
|
}
|
if(name=="CAM_ANG_MAX")
|
{
|
float m=mParams->GetValue("CAM_ANG_MAX");
|
mStateMachine->SetCamAngMax(m);
|
}
|
if(name=="CAM_ANG_MIN")
|
{
|
float m=mParams->GetValue("CAM_ANG_MIN");
|
mStateMachine->SetCamAngMin(m);
|
}
|
if(name=="CTRL_FLAP_RC")
|
{
|
float m=mParams->GetValue("CTRL_FLAP_RC");
|
mStateMachine->SetFlapRC(m);
|
}
|
if(name=="CTRL_FLAP_VAL")
|
{
|
float m=mParams->GetValue("CTRL_FLAP_VAL");
|
mStateMachine->SetFlapVAL(m);
|
|
}
|
if(name=="TEST_SRV_CH")
|
{
|
float m=mParams->GetValue("TEST_SRV_CH");
|
mStateMachine->SetTestSrvCH(m);
|
|
}
|
if(name=="TEST_SRV_AMP")
|
{
|
float m=mParams->GetValue("TEST_SRV_AMP");
|
mStateMachine->SetTestSrvAmp(m);
|
|
}
|
if(name=="TEST_SRV_T")
|
{
|
float m=mParams->GetValue("TEST_SRV_T");
|
mStateMachine->SetTestSrvT(m);
|
|
}
|
if(name=="TEST_SRV_TRIM")
|
{
|
float m=mParams->GetValue("TEST_SRV_TRIM");
|
mStateMachine->SetTestSrvTrim(m);
|
|
}
|
if(name=="CTRL_LOITER_ALT")
|
{
|
float m=mParams->GetValue("CTRL_LOITER_ALT");
|
mStateMachine->SetLoiterAlt(m);
|
|
}
|
if(name=="CTRL_SAFE_SPD")
|
{
|
float m=mParams->GetValue("CTRL_SAFE_SPD");
|
if(m<20)
|
m=20;
|
mParams->SetParam("CTRL_SAFE_SPD",m);
|
mStateMachine->SetSafeSpeed(m);
|
|
}
|
if(name=="CTRL_SAFE_PITCH")
|
{
|
float m=mParams->GetValue("CTRL_SAFE_PITCH");
|
if(m<0)
|
m=0;
|
mParams->SetParam("CTRL_SAFE_PITCH",m);
|
mStateMachine->SetSafePitch(m);
|
|
}
|
if(name=="CTRL_SAFE_TIME")
|
{
|
float m=mParams->GetValue("CTRL_SAFE_TIME");
|
if(m<10)
|
m=10;
|
mParams->SetParam("CTRL_SAFE_TIME",m);
|
mStateMachine->SetSafeTime(m);
|
|
}
|
if(name=="CAM_SRV_REVERSE")
|
{
|
float m=mParams->GetValue("CAM_SRV_REVERSE");
|
mStateMachine->SetCamReverse(m);
|
|
}
|
if(name=="CAM_SRV_KI")
|
{
|
float m=mParams->GetValue("CAM_SRV_KI");
|
if(m<0)
|
{
|
m=0.1;
|
mParams->SetParam("CAM_SRV_KI",m);
|
}
|
mStateMachine->SetCamSrvKI(m);
|
|
}
|
if(name=="CAM_SRV_KP")
|
{
|
float m=mParams->GetValue("CAM_SRV_KP");
|
if(m<0)
|
{
|
m=0.0;
|
mParams->SetParam("CAM_SRV_KP",m);
|
}
|
mStateMachine->SetCamSrvKP(m);
|
|
}
|
if(name=="CAM_PITCH_DELTA")
|
{
|
float m=mParams->GetValue("CAM_PITCH_DELTA");
|
if(m<2.0)
|
{
|
m=2.0;
|
mParams->SetParam("CAM_PITCH_DELTA",m);
|
}
|
mStateMachine->SetCamPitchDelta(m);
|
|
}
|
if(name=="CAM_RATE_MAX")
|
{
|
float m=mParams->GetValue("CAM_RATE_MAX");
|
if(m<1.0)
|
{
|
m=1.0;
|
mParams->SetParam("CAM_RATE_MAX",m);
|
}
|
mStateMachine->SetCamRateMax(m);
|
|
}
|
if(name=="CTRL_TECS_DALT")
|
{
|
float m=mParams->GetValue("CTRL_TECS_DALT");
|
|
mStateMachine->SetTecsDalt(m);
|
|
}
|
|
}
|
else
|
{
|
log_v("Error set:%s %i ret idx=%i\n\r",name.c_str(),name.length(),idx);
|
}
|
}
|
break;
|
case MAVLINK_MSG_ID_PARAM_VALUE:
|
{
|
//mavlink_param_value_t val={0};
|
//mavlink_msg_param_value_decode(&msg,&val);
|
//mTransmitter->SendParameter(val);
|
}
|
break;
|
case MAVLINK_MSG_ID_MISSION_COUNT:
|
{
|
|
mavlink_mission_count_t req;
|
mavlink_msg_mission_count_decode(&msg,&req);
|
mStateMachine->SetWpCount(req.count-1);
|
log_i("Mavlink readed wpnums=%d",req.count);
|
}
|
break;
|
case MAVLINK_MSG_ID_MISSION_ITEM:
|
{
|
mavlink_mission_item_t item;
|
mavlink_msg_mission_item_decode(&msg,&item);
|
mStateMachine->SetWp(item);
|
log_i("Mavlink readed wp num=%d",item.seq);
|
}
|
break;
|
case MAVLINK_MSG_ID_MISSION_REQUEST:
|
{
|
mavlink_mission_request_t req;
|
mavlink_msg_mission_request_decode(&msg,&req);
|
mTransmitter->SendMissionItem(req.seq);
|
log_i("Mavlink requesr wp num=%d",req.seq);
|
}
|
break;
|
case MAVLINK_MSG_ID_MISSION_ACK:
|
{
|
mavlink_mission_ack_t ack;
|
mavlink_msg_mission_ack_decode(&msg,&ack);
|
if(ack.type==0)
|
mStateMachine->SetWpsSended(true);
|
log_i("Mavlink mission ack=%d",ack.type);
|
}
|
break;
|
case MAVLINK_MSG_ID_MISSION_CURRENT:
|
{
|
mavlink_mission_current_t wpc;
|
mavlink_msg_mission_current_decode(&msg, &wpc);
|
mStateMachine->SetCurrWpNum(wpc.seq);
|
}
|
break;
|
case MAVLINK_MSG_ID_COMMAND_ACK:
|
{
|
mavlink_command_ack_t ack;
|
mavlink_msg_command_ack_decode(&msg,&ack);
|
//qDebug()<<"MAVLINK_MSG_ID_COMMAND_ACK "<<ack.command<<" res="<<ack.result<<" param2="<<ack.result_param2<<endl;
|
if(ack.command==MAV_CMD_GUIDED_CHANGE_SPEED)
|
{
|
String str("Set ATTACK_SPD=");
|
float m=mParams->GetValue("CTRL_ATTACK_SPD");
|
str+=String(m,1);
|
str+=String(" res=");
|
str+=String(ack.result);
|
mTransmitter->SendText(str,MAV_SEVERITY_INFO);
|
}
|
if(ack.command==MAV_CMD_DO_REPOSITION)
|
{
|
obj->mLockMode=true;
|
mStateMachine->SetBaseMode(obj->mBaseMode,PLANE_MODE_GUIDED);
|
log_i("Fast GUIDED %i",ack.result);
|
}
|
}
|
break;
|
case MAVLINK_MSG_ID_NAMED_VALUE_INT:
|
{
|
log_i("Recived cmd to tracker!!!!!!");
|
}
|
break;
|
default:
|
break;
|
}
|
int len = mavlink_msg_to_send_buffer(obj->mBufferOut, &msg);
|
mTracker->SetMavlikBuf(obj->mBufferOut,len);
|
}
|
}
|
}
|
}
|
else
|
vTaskDelay(1);
|
}
|
|
}
|
|
void MavlinkReciver::reboot()
|
{
|
vTaskDelay(2000);
|
ESP.restart();
|
}
|
