#include "MavlinkTransmitter.h"
|
#include <modules/my_task_creator/TaskCreator.h>
|
TickType_t mLastHearbitTick=0;
|
MavlinkTransmitter::MavlinkTransmitter()
|
{
|
|
mSerial=&Serial1;
|
/*SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
|
SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
|
SubscribeMsg(MAVLINK_MSG_ID_GPS_RAW_INT,200000);*/
|
mavlink_set_proto_version(MAVLINK_COMM_0,2);
|
}
|
|
MavlinkTransmitter::~MavlinkTransmitter()
|
{
|
}
|
|
void MavlinkTransmitter::TaskMavlinkTx(void *pvParameters)
|
{
|
log_i("Task started");
|
TaskCreator *mCreator = (TaskCreator *)pvParameters;
|
MavlinkTransmitter *obj=&(mCreator->mTransmitter);
|
TrackerRxTx *m_Tracker=&(mCreator->mTracker);
|
mLastHearbitTick=xTaskGetTickCount();
|
while (1)
|
{
|
|
TickType_t mCurrentTick=xTaskGetTickCount();
|
if(mCurrentTick-mLastHearbitTick>=1000)
|
{
|
if(obj->mSysId)
|
obj->SendHearbit();
|
mLastHearbitTick=mCurrentTick;
|
vTaskDelay(1);
|
}
|
if(obj->mSysId)
|
{
|
if(obj->SendFromQueue())
|
{
|
if(obj->mCurrMsgId==SEND_MSG_ID_GUIDED_MODE)
|
{
|
m_Tracker->SetMavlikBuf(obj->mBufferOut,obj->mMsgLen);
|
}
|
vTaskDelay(1);
|
}
|
else
|
vTaskDelay(1);
|
}
|
else
|
vTaskDelay(1);
|
}
|
|
}
|
bool MavlinkTransmitter::SendFromQueue()
|
{
|
bool res=false;
|
uint32_t msgid;
|
mMutex.Lock();
|
if(!mQueueMsgId.empty())
|
{
|
msgid=mQueueMsgId.front();
|
mQueueMsgId.pop();
|
res=true;
|
}
|
mMutex.Unlock();
|
if(!res)
|
return res;
|
mCurrMsgId=msgid;
|
switch (msgid)
|
{
|
case SEND_MSG_ID_DISTANCE_SENSOR:
|
SendDistanceSensorUpdate();
|
break;
|
case SEND_MSG_ID_COMMAND_LONG:
|
SendCmdLong();
|
break;
|
case SEND_MSG_ID_PARAM_VALUE:
|
SendParams();
|
break;
|
case SEND_MSG_ID_STATUSTEXT:
|
SendStatusText();
|
break;
|
case SEND_MSG_ID_REQ_MISSION_COUNT:
|
SendReqMissionCount();
|
break;
|
case SEND_MSG_ID_REQ_MISSION_ITEM:
|
SendReqMissionItem();
|
break;
|
case SEND_MSG_ID_MISSION_COUNT:
|
SendCurrentMissionCount();
|
break;
|
case SEND_MSG_ID_MISSION_ITEM:
|
SendNextMissionItem();
|
break;
|
case SEND_MSG_ID_MANUAL_CONTROL:
|
SendManualCoontrool();
|
break;
|
case SEND_MSG_ID_GUIDED_MODE:
|
SendGuidedMode();
|
break;
|
case SEND_MSG_ID_ATT_TARGET:
|
{
|
static uint16_t cnt=0;
|
mCurrentTick=xTaskGetTickCount();
|
uint32_t dt=mCurrentTick-mLastTick;
|
mLastTick=mCurrentTick;
|
SendGuidedControll();
|
cnt++;
|
if(cnt==500)
|
{
|
log_i("Send att time=%i",dt);
|
cnt=0;
|
}
|
}
|
break;
|
case SEND_MSG_ID_MODE_RTL:
|
SendRtlMode();
|
break;
|
case SEND_MSG_ID_STREAM_RATE:
|
SendStreamRate();
|
break;
|
case SEND_MSG_ID_MODE_LOITER:
|
{
|
|
SendLoiterWp();
|
vTaskDelay(2);
|
SendLoiterMode();
|
vTaskDelay(2);
|
SendLoiterWp();
|
}
|
break;
|
case SEND_MSG_ID_GUIDED_CHANGE_SPEED:
|
SendGuidedChangeSpeed();
|
break;
|
case SEND_MSG_ID_RC_CHANNELS_OVERRIDE:
|
SendFlapOverride();
|
break;
|
case SEND_MSG_MAVLINK:
|
SendMsgMavlink();
|
break;
|
case SEND_MSG_GUIDED_CHANGE_ALTITUDE:
|
SendGuidedAlt();
|
break;
|
|
default:
|
break;
|
}
|
return res;
|
}
|
|
void MavlinkTransmitter::SendRequestMissionItem(uint16_t n)
|
{
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_ID_REQ_MISSION_ITEM);
|
mItemNum=n;
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendReqMissionItem()
|
{
|
|
mavlink_mission_request_t req={0};
|
req.seq=mItemNum;
|
req.target_component=MAV_COMP_ID_MISSIONPLANNER;
|
req.mission_type=0;
|
req.target_system=mSysId;
|
|
mavlink_msg_mission_request_encode(mSysId,MY_COMP_ID,&msg,&req);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
//log_i("Write req wp num=%d writetd=%d from=%d",mItemNum,len1,len);
|
}
|
void MavlinkTransmitter::SendMissionReq()
|
{
|
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_ID_REQ_MISSION_COUNT);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendReqMissionCount()
|
{
|
|
|
mavlink_mission_request_list_t req={0};
|
req.target_component=MAV_COMP_ID_MISSIONPLANNER;
|
req.mission_type=0;
|
req.target_system=mSysId;
|
mavlink_msg_mission_request_list_encode(mSysId, MY_COMP_ID, &msg,&req);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
//log_i("Write req wpnums writetd=%d from=%d",len1,len);
|
}
|
void MavlinkTransmitter::SendHearbit()
|
{
|
|
mavlink_heartbeat_t packet={0};
|
|
packet.mavlink_version = 3;
|
packet.autopilot = MAV_AUTOPILOT_INVALID;
|
packet.base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
packet.custom_mode = 0;
|
packet.system_status = 0;
|
packet.type = MAV_TYPE_ONBOARD_CONTROLLER;
|
msg.compid = MY_COMP_ID;
|
msg.sysid = 255;
|
mavlink_msg_heartbeat_encode(mSysId, MY_COMP_ID, &msg, &packet);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::UnsubscribeMsg(uint32_t msgId)
|
{
|
|
mavlink_command_long_t cmd={0};
|
cmd.command=MAV_CMD_SET_MESSAGE_INTERVAL;
|
cmd.confirmation=0;
|
cmd.param1=msgId;
|
cmd.param2=1000000;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.target_system=mSysId;
|
mMutex.Lock();
|
mQueueCmdLong.push(cmd);
|
mQueueMsgId.push(SEND_MSG_ID_COMMAND_LONG);
|
mMutex.Unlock();
|
}
|
|
void MavlinkTransmitter::SubscribeMsg(uint32_t msgId,uint32_t interval)
|
{
|
|
mavlink_command_long_t cmd={0};
|
cmd.command=MAV_CMD_SET_MESSAGE_INTERVAL;
|
cmd.confirmation=0;
|
cmd.param1=msgId;
|
cmd.param2=interval;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.target_system=mSysId;
|
mMutex.Lock();
|
mQueueCmdLong.push(cmd);
|
mQueueMsgId.push(SEND_MSG_ID_COMMAND_LONG);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendCamPWM(uint8_t n,u_int16_t pwm)
|
{
|
mavlink_command_long_t cmd={0};
|
cmd.command=MAV_CMD_DO_SET_SERVO;
|
cmd.confirmation=0;
|
cmd.param1=n;
|
cmd.param2=pwm;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.target_system=mSysId;
|
mMutex.Lock();
|
mQueueCmdLong.push(cmd);
|
mQueueMsgId.push(SEND_MSG_ID_COMMAND_LONG);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendFlapPWM(uint8_t n,u_int16_t pwm)
|
{
|
mMutex.Lock();
|
mFLAP_RC=n;
|
mFLAP_VAL=pwm;
|
mQueueMsgId.push(SEND_MSG_ID_RC_CHANNELS_OVERRIDE);
|
mMutex.Unlock();
|
|
}
|
void MavlinkTransmitter::SendFlapOverride()
|
{
|
mavlink_rc_channels_override_t rc;
|
memset(&rc,0,sizeof(mavlink_rc_channels_override_t));
|
mavlink_message_t msg;
|
rc.target_component=MAV_COMP_ID_AUTOPILOT1;
|
rc.target_system=mSysId;
|
uint8_t n;
|
uint16_t pwm;
|
mMutex.Lock();
|
n=mFLAP_RC;
|
pwm=mFLAP_VAL;
|
mMutex.Unlock();
|
switch (n)
|
{
|
case 1:
|
rc.chan1_raw=pwm;
|
break;
|
case 2:
|
rc.chan2_raw=pwm;
|
break;
|
case 3:
|
rc.chan3_raw=pwm;
|
break;
|
case 4:
|
rc.chan4_raw=pwm;
|
break;
|
case 5:
|
rc.chan5_raw=pwm;
|
break;
|
case 6:
|
rc.chan6_raw=pwm;
|
break;
|
case 7:
|
rc.chan7_raw=pwm;
|
break;
|
case 8:
|
rc.chan8_raw=pwm;
|
break;
|
case 9:
|
rc.chan9_raw=pwm;
|
break;
|
case 10:
|
rc.chan10_raw=pwm;
|
break;
|
case 11:
|
rc.chan11_raw=pwm;
|
break;
|
case 12:
|
rc.chan12_raw=pwm;
|
break;
|
case 13:
|
rc.chan13_raw=pwm;
|
break;
|
case 14:
|
rc.chan14_raw=pwm;
|
break;
|
case 15:
|
rc.chan15_raw=pwm;
|
break;
|
case 16:
|
rc.chan16_raw=pwm;
|
break;
|
case 17:
|
rc.chan17_raw=pwm;
|
break;
|
case 18:
|
rc.chan18_raw=pwm;
|
break;
|
default:
|
break;
|
}
|
mavlink_msg_rc_channels_override_encode(255/*mSysId*/,MAV_COMP_ID_AUTOPILOT1/*MY_COMP_ID*/,&msg, &rc);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
|
}
|
void MavlinkTransmitter::SendCmdLong()
|
{
|
|
mavlink_command_long_t cmd={0};
|
|
mMutex.Lock();
|
bool res=false;
|
if(!mQueueCmdLong.empty())
|
{
|
cmd=mQueueCmdLong.front();
|
mQueueCmdLong.pop();
|
res=true;
|
}
|
mMutex.Unlock();
|
if(!res)
|
return;
|
mavlink_msg_command_long_encode(mSysId,MY_COMP_ID,&msg,&cmd);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::SendParameter(mavlink_param_value_t par)
|
{
|
mMutex.Lock();
|
mQueueParams.push(par);
|
mQueueMsgId.push(SEND_MSG_ID_PARAM_VALUE);
|
mMutex.Unlock();
|
|
}
|
void MavlinkTransmitter::SendParams()
|
{
|
mavlink_message_t msg={0};
|
mavlink_param_value_t val;
|
|
bool res=false;
|
mMutex.Lock();
|
if(!mQueueParams.empty())
|
{
|
val=mQueueParams.front();
|
mQueueParams.pop();
|
res=true;
|
}
|
mMutex.Unlock();
|
if(!res)
|
return;
|
msg.sysid=255;
|
msg.compid=MY_COMP_ID;
|
mavlink_msg_param_value_encode(mSysId,MY_COMP_ID,&msg,&val);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
log_v("Param send:%s,%f (%d of %d)\n\r",(const char*)val.param_id,val.param_value, val.param_index, val.param_count);
|
}
|
|
void MavlinkTransmitter::SendText(String str,uint16_t ser)
|
{
|
if(!mSysId)
|
return;
|
std::pair<String,uint16_t> par(str,ser);
|
mMutex.Lock();
|
mQueueText.push(par);
|
mQueueMsgId.push(SEND_MSG_ID_STATUSTEXT);
|
mMutex.Unlock();
|
}
|
|
void MavlinkTransmitter::SendStatusText()
|
{
|
mavlink_message_t msg={0};
|
mavlink_statustext_t val;
|
|
bool res=false;
|
std::pair<String,uint16_t> par;
|
mMutex.Lock();
|
if(!mQueueText.empty())
|
{
|
par=mQueueText.front();
|
mQueueText.pop();
|
res=true;
|
}
|
mMutex.Unlock();
|
if(!res)
|
return;
|
val.severity=par.second;
|
val.chunk_seq=0;
|
val.id=0;
|
for(int i=0;i<50;i++)
|
val.text[i]=0;
|
for(int i=0;i<par.first.length();i++)
|
val.text[i]=par.first[i];
|
|
mavlink_msg_statustext_encode(mSysId,MY_COMP_ID,&msg,&val);
|
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
|
}
|
|
void MavlinkTransmitter::SetWps(std::map<uint16_t,mavlink_mission_item_t> map)
|
{
|
mMutex.Lock();
|
mMissionMap=map;
|
mMutex.Unlock();
|
}
|
|
void MavlinkTransmitter::SendMissionCount()
|
{
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_ID_MISSION_COUNT);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendCurrentMissionCount()
|
{
|
uint16_t num=0;
|
mMutex.Lock();
|
num=mMissionMap.size();
|
mMutex.Unlock();
|
mavlink_mission_count_t mcount;
|
mcount.count=num;
|
mcount.mission_type=0;
|
mcount.target_component=MAV_COMP_ID_MISSIONPLANNER;
|
mcount.target_system=mSysId;
|
mavlink_msg_mission_count_encode(mSysId,1,&msg, &mcount);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::SendMissionItem(uint16_t num)
|
{
|
mMutex.Lock();
|
mCurrentItemSend=num;
|
mQueueMsgId.push(SEND_MSG_ID_MISSION_ITEM);
|
mMutex.Unlock();
|
}
|
|
void MavlinkTransmitter::SendNextMissionItem()
|
{
|
mavlink_mission_item_t item;
|
mMutex.Lock();
|
if(mMissionMap.size()>mCurrentItemSend)
|
{
|
item=mMissionMap[mCurrentItemSend];
|
}
|
else
|
{
|
log_v("Error wp number=%d",mCurrentItemSend);
|
}
|
mMutex.Unlock();
|
item.target_component=MAV_COMP_ID_MISSIONPLANNER;
|
item.target_system=mSysId;
|
mavlink_msg_mission_item_encode(mSysId,1,&msg, &item);
|
int len = mavlink_msg_to_send_buffer(mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
log_i("Sended wp num=%d",mCurrentItemSend);
|
}
|
|
void MavlinkTransmitter::SendManualCoontrool(int16_t x, int16_t y, int16_t z, int16_t r)
|
{
|
mMutex.Lock();
|
mX=x;
|
mY=y;
|
mZ=z;
|
mR=r;
|
mQueueMsgId.push(SEND_MSG_ID_MANUAL_CONTROL);
|
mMutex.Unlock();
|
|
}
|
|
void MavlinkTransmitter::SendManualCoontrool()
|
{
|
mavlink_manual_control_t cntr={0};
|
mMutex.Lock();
|
cntr.x=mX;
|
cntr.y=mY;
|
cntr.z=mZ;
|
cntr.r=mR;
|
mMutex.Unlock();
|
cntr.target=mSysId;
|
mavlink_msg_manual_control_encode(255,1,&msg, &cntr);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::SendGuidedMode(uint8_t base_mode, mavlink_mission_item_t wpg)
|
{
|
mMutex.Lock();
|
mBaseMode=base_mode;
|
mWpGuided=wpg;
|
mQueueMsgId.push(SEND_MSG_ID_GUIDED_MODE);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendGuidedSpeed(float speed)
|
{
|
mMutex.Lock();
|
mGuidedSpeed=speed;
|
mQueueMsgId.push(SEND_MSG_ID_GUIDED_CHANGE_SPEED);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendLoiterSpeed(float spd)
|
{
|
mavlink_command_long_t cmd={0};
|
cmd.command=MAV_CMD_DO_CHANGE_SPEED;
|
cmd.confirmation=0;
|
cmd.param1=SPEED_TYPE_AIRSPEED;
|
cmd.param2=spd;
|
cmd.param3=-1;
|
cmd.param4=0;
|
cmd.param5=0;
|
cmd.param6=0;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.target_system=mSysId;
|
mMutex.Lock();
|
mQueueCmdLong.push(cmd);
|
mQueueMsgId.push(SEND_MSG_ID_COMMAND_LONG);
|
mMutex.Unlock();
|
|
}
|
|
void MavlinkTransmitter::SendGuidedChangeSpeed()
|
{
|
mavlink_command_int_t packet;
|
packet.command=MAV_CMD_GUIDED_CHANGE_SPEED;
|
packet.autocontinue=0;
|
packet.current=0;
|
packet.frame=MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
packet.param1=SPEED_TYPE_AIRSPEED;
|
packet.param2=mGuidedSpeed;
|
packet.param3=0;
|
packet.param4=0;
|
packet.target_component=MAV_COMP_ID_AUTOPILOT1;
|
packet.target_system=mSysId;
|
packet.x=0;
|
packet.y=0;
|
packet.z=0;
|
mavlink_msg_command_int_encode(mSysId,MY_COMP_ID,&msg,&packet);
|
//mavlink_msg_set_mode_pack(mSysId, MY_COMP_ID, &msg, mSysId, mBaseMode, PLANE_MODE_GUIDED);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
void MavlinkTransmitter::SendGuidedMode()
|
{
|
mavlink_command_int_t packet;
|
packet.command=MAV_CMD_DO_REPOSITION;
|
packet.autocontinue=0;
|
packet.current=0;
|
packet.frame=MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
packet.param1=0;
|
packet.param2=MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
|
packet.param3=0;
|
packet.param4=0;
|
packet.target_component=MAV_COMP_ID_AUTOPILOT1;
|
packet.target_system=mSysId;
|
mMutex.Lock();
|
packet.x=mWpGuided.x*1E7;
|
packet.y=mWpGuided.y*1E7;
|
packet.z=mWpGuided.z;
|
mMutex.Unlock();
|
if(fabs(packet.z)<0.01)
|
packet.z=0.01;
|
mavlink_msg_command_int_encode(mSysId,MY_COMP_ID,&msg,&packet);
|
//mavlink_msg_set_mode_pack(mSysId, MY_COMP_ID, &msg, mSysId, mBaseMode, PLANE_MODE_GUIDED);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mMsgLen=len;
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::SendLoiterWp()
|
{
|
mavlink_mission_item_t it;
|
it.seq=0;
|
it.current=3;
|
it.command=MAV_CMD_NAV_WAYPOINT;
|
it.x=0;
|
it.y=0;
|
it.z=mLoiterAlt;
|
it.target_component=MAV_COMP_ID_AUTOPILOT1;
|
it.target_system=mSysId;
|
it.frame=MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
it.mission_type=0;
|
|
mavlink_msg_mission_item_encode(mSysId,MY_COMP_ID,&msg, &it);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
log_i("Send loiter Alt=%i",(int)mLoiterAlt);
|
}
|
|
void MavlinkTransmitter::SendGuidedControll(float q0, float q1, float q2, float q3,float Alt,bool sendAlt)
|
{
|
mMutex.Lock();
|
att_target.body_pitch_rate=0;
|
att_target.body_roll_rate=0;
|
att_target.body_yaw_rate=0;
|
att_target.q[0]=q0;
|
att_target.type_mask=0b11000100;
|
att_target.q[0]=q0;
|
att_target.q[1]=q1;
|
att_target.q[2]=q2;
|
att_target.q[3]=q3;
|
att_target.thrust=0.0;
|
att_target.target_system=mSysId;
|
att_target.target_component=MAV_COMP_ID_AUTOPILOT1;
|
mQueueMsgId.push(SEND_MSG_ID_ATT_TARGET);
|
mMutex.Unlock();
|
if(sendAlt)
|
{
|
|
mGuidedAlt=Alt;
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_GUIDED_CHANGE_ALTITUDE);
|
mMutex.Unlock();
|
}
|
}
|
void MavlinkTransmitter::SendGuidedAlt()
|
{
|
mavlink_command_int_t cmd;
|
cmd.command=MAV_CMD_GUIDED_CHANGE_ALTITUDE;
|
cmd.autocontinue=0;
|
cmd.current=0;
|
cmd.frame=MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
cmd.param1=SPEED_TYPE_AIRSPEED;
|
cmd.param2=0;
|
cmd.param3=0;
|
cmd.param4=0;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.target_system=mSysId;
|
cmd.x=0;
|
cmd.y=0;
|
cmd.z=mGuidedAlt;
|
mavlink_msg_command_int_encode(mSysId,MY_COMP_ID,&msg,&cmd);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::SendGuidedControll()
|
{
|
mMutex.Lock();
|
mavlink_msg_set_attitude_target_encode(mSysId,MY_COMP_ID,&msg,&att_target);
|
mMutex.Unlock();
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
|
}
|
|
void MavlinkTransmitter::SendRtl()
|
{
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_ID_MODE_RTL);
|
mMutex.Unlock();
|
}
|
|
void MavlinkTransmitter::SendRtlMode()
|
{
|
mavlink_command_long_t cmd;
|
cmd.command=MAV_CMD_DO_SET_MODE;
|
cmd.param1=mBaseMode;
|
cmd.param2=PLANE_MODE_RTL;
|
cmd.param3=0;
|
cmd.param4=0;
|
cmd.param5=0;
|
cmd.param6=0;
|
cmd.param7=0;
|
cmd.target_system=mSysId;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.confirmation=1;
|
mavlink_msg_command_long_encode(mSysId,MY_COMP_ID,&msg,&cmd);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
void MavlinkTransmitter::SendLoiter(float alt)
|
{
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_ID_MODE_LOITER);
|
mLoiterAlt=alt;
|
mMutex.Unlock();
|
|
}
|
void MavlinkTransmitter::SendLoiterMode()
|
{
|
mavlink_command_long_t cmd;
|
cmd.command=MAV_CMD_DO_SET_MODE;
|
cmd.param1=mBaseMode;
|
cmd.param2=PLANE_MODE_LOITER;
|
cmd.param3=0;
|
cmd.param4=0;
|
cmd.param5=0;
|
cmd.param6=0;
|
cmd.param7=0;
|
cmd.target_system=mSysId;
|
cmd.target_component=MAV_COMP_ID_AUTOPILOT1;
|
cmd.confirmation=1;
|
mavlink_msg_command_long_encode(mSysId,MY_COMP_ID,&msg,&cmd);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
|
|
|
}
|
void MavlinkTransmitter::SendStreamRateAll()
|
{
|
mMutex.Lock();
|
mQueueMsgId.push(SEND_MSG_ID_STREAM_RATE);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendStreamRate()
|
{
|
mavlink_request_data_stream_t stream={};
|
stream.req_stream_id = MAV_DATA_STREAM_ALL;
|
stream.req_message_rate = 1;
|
stream.start_stop = 1;
|
stream.target_system = mSysId;
|
stream.target_component = 0;
|
mavlink_msg_request_data_stream_encode(mSysId, MY_COMP_ID, &msg, &stream);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut,len);
|
}
|
|
void MavlinkTransmitter::SendDistanceSensor(float distance, bool isValid, float maxDist)
|
{
|
mMutex.Lock();
|
mRangefinderDistance = distance;
|
mRangefinderValid = isValid;
|
mRangefinderMax = maxDist;
|
mQueueMsgId.push(SEND_MSG_ID_DISTANCE_SENSOR);
|
mMutex.Unlock();
|
}
|
|
void MavlinkTransmitter::SendDistanceSensorUpdate()
|
{
|
mavlink_distance_sensor_t _msg = {0};
|
_msg.time_boot_ms = millis();
|
_msg.min_distance = 20;
|
mMutex.Lock();
|
_msg.max_distance = this->mRangefinderMax;
|
_msg.current_distance = static_cast<uint16_t>(round(mRangefinderDistance*100));
|
mMutex.Unlock();
|
_msg.type = MAV_DISTANCE_SENSOR_LASER;
|
_msg.id = 1;
|
_msg.orientation = 25;
|
_msg.covariance = UINT8_MAX;
|
_msg.horizontal_fov = 0;
|
_msg.vertical_fov = 0;
|
_msg.quaternion[0] = 1;
|
_msg.quaternion[1] = 0;
|
_msg.quaternion[2] = 0;
|
_msg.quaternion[3] = 0;
|
_msg.signal_quality = mRangefinderValid ? 100 : 0;
|
|
mavlink_msg_distance_sensor_encode(mSysId,MY_COMP_ID,&msg,&_msg);
|
int len = mavlink_msg_to_send_buffer((uint8_t *)mBufferOut, &msg);
|
mSerial->write(mBufferOut, len);
|
//add logs for test with mission planner
|
//log_i("MAVLink TX: DISTANCE_SENSOR dist=%.2fcm valid=%d sysId=%d", mRangefinderDistance * 100, mRangefinderValid, mSysId);
|
}
|
void MavlinkTransmitter::SendMavlink(uint8_t *p,u_int16_t len)
|
{
|
mMutex.Lock();
|
memcpy(mBufferCmd,p,len);
|
mMavMsgLen=len;
|
mQueueMsgId.push(SEND_MSG_MAVLINK);
|
mMutex.Unlock();
|
}
|
void MavlinkTransmitter::SendMsgMavlink()
|
{
|
mMutex.Lock();
|
mSerial->write(mBufferCmd, mMavMsgLen);
|
mMutex.Unlock();
|
}
|
