#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();
}