#ifndef MAVLINKTRANSMITTER_H
|
#define MAVLINKTRANSMITTER_H
|
|
#include <Arduino.h>
|
#include <vector>
|
#include <queue>
|
#include <map>
|
#include <utility>
|
#include <modules/my_mutex/Mutex.h>
|
|
#include <modules/mavlink/v2.0/ardupilotmega/mavlink.h>
|
|
class MavlinkTransmitter
|
{
|
|
public:
|
enum{
|
SEND_MSG_ID_COMMAND_LONG,
|
SEND_MSG_ID_PARAM_VALUE,
|
SEND_MSG_ID_STATUSTEXT,
|
SEND_MSG_ID_REQ_MISSION_COUNT,
|
SEND_MSG_ID_REQ_MISSION_ITEM,
|
SEND_MSG_ID_MISSION_COUNT,
|
SEND_MSG_ID_MISSION_ITEM,
|
SEND_MSG_ID_MANUAL_CONTROL,
|
SEND_MSG_ID_GUIDED_MODE,
|
SEND_MSG_ID_ATT_TARGET,
|
SEND_MSG_ID_MODE_RTL,
|
SEND_MSG_ID_STREAM_RATE,
|
SEND_MSG_ID_MODE_LOITER,
|
SEND_MSG_ID_GUIDED_CHANGE_SPEED,
|
SEND_MSG_ID_DISTANCE_SENSOR,
|
SEND_MSG_ID_RC_CHANNELS_OVERRIDE,
|
SEND_MSG_MAVLINK,
|
SEND_MSG_GUIDED_CHANGE_ALTITUDE,
|
};
|
|
MavlinkTransmitter();
|
~MavlinkTransmitter();
|
static void TaskMavlinkTx(void *pvParameters);
|
void SetSysId(uint8_t id){mSysId=id;}
|
void UnsubscribeMsg(uint32_t msgId);
|
void SubscribeMsg(uint32_t msgId,uint32_t interval);
|
void SendParameter(mavlink_param_value_t par);
|
void SendText(String str,uint16_t ser);
|
void SendMissionReq();
|
void SendRequestMissionItem(uint16_t n);
|
void SetWps(std::map<uint16_t,mavlink_mission_item_t> map);
|
void SendMissionCount();
|
void SendMissionItem(uint16_t num);
|
void SendManualCoontrool(int16_t x,int16_t y,int16_t z,int16_t r);
|
void SendGuidedMode(uint8_t base_mode,mavlink_mission_item_t wpg);
|
void SendGuidedControll(float q0,float q1,float q2,float q3,float Alt,bool sendAlt);
|
void SendRtl();
|
void SendStreamRateAll();
|
void SendLoiter(float alt);
|
void SendGuidedSpeed(float speed);
|
void SendDistanceSensor(float distance, bool isValid, float maxDist);
|
void SendCamPWM(uint8_t n,u_int16_t pwm);
|
void SendFlapPWM(uint8_t n,u_int16_t pwm);
|
void SendMavlink(uint8_t *p,u_int16_t len);
|
void SendLoiterSpeed(float spd);
|
private:
|
void SendHearbit();
|
bool SendFromQueue();
|
void SendCmdLong();
|
void SendParams();
|
void SendStatusText();
|
void SendReqMissionCount();
|
void SendReqMissionItem();
|
void SendCurrentMissionCount();
|
void SendNextMissionItem();
|
void SendManualCoontrool();
|
void SendGuidedMode();
|
void SendLoiterWp();
|
void SendGuidedControll();
|
void SendRtlMode();
|
void SendStreamRate();
|
void SendLoiterMode();
|
void SendGuidedChangeSpeed();
|
void SendDistanceSensorUpdate();
|
void SendFlapOverride();
|
void SendMsgMavlink();
|
void SendGuidedAlt();
|
Mutex mMutex;
|
uint8_t mBufferOut[MAVLINK_MAX_PACKET_LEN];
|
uint8_t mBufferCmd[MAVLINK_MAX_PACKET_LEN];
|
uint16_t mMavMsgLen=0;
|
uint32_t mMsgLen=0;
|
HardwareSerial *mSerial;
|
std::queue<uint32_t> mQueueMsgId;
|
std::queue<mavlink_command_long_t> mQueueCmdLong;
|
std::queue<mavlink_param_value_t> mQueueParams;
|
std::queue<std::pair<String,uint16_t>> mQueueText;
|
std::map<uint16_t,mavlink_mission_item_t> mMissionMap;
|
uint8_t mSysId=0;
|
uint16_t mItemNum=0;
|
mavlink_message_t msg;
|
uint16_t mCurrentItemSend=0;
|
int16_t mX,mY,mZ,mR;
|
mavlink_mission_item_t mWpGuided;
|
uint8_t mBaseMode;
|
mavlink_set_attitude_target_t att_target;
|
uint32_t mCurrentTick=0;
|
uint32_t mLastTick=0;
|
float mLoiterAlt=100;
|
float mGuidedSpeed=30.0;
|
float mLoiterSpeed=30.0;
|
uint32_t mCurrMsgId=0;
|
float mRangefinderDistance=0.0;
|
float mRangefinderMax = 0;
|
bool mRangefinderValid=false;
|
uint8_t mFLAP_RC=0;
|
uint16_t mFLAP_VAL=0;
|
float mGuidedAlt=1;
|
};
|
|
|
|
|
#endif
|
