mavModule.h

Go to the documentation of this file.

/*****************************************************************************
 * @Brief     Base class of dji2mav module. ROS-free and mav-depended
 * @Version   0.3.0
 * @Author    Chris Liu
 * @Created   2015/12/06
 * @Modified  2015/12/24
 *****************************************************************************/

#ifndef _MAV2DJI_MAVMODULE_H_
#define _MAV2DJI_MAVMODULE_H_


#include <mavlink/common/mavlink.h>
#include <new>
#include <string>
#include <pthread.h>

#include "moduleBuf.h"
#include "dji_sdk_dji2mav/mavHandler.h"
#include "dji_sdk_dji2mav/log.h"

namespace dji2mav{

    class MavModule {

        public:
            MavModule(MavHandler &handler, std::string name, uint16_t bufSize) {
                m_hdlr = &handler;
                m_masterGcsIdx = -1;
                m_name = name;

                try {
                    m_senderRecord = new int[m_hdlr->getMaxListSize()];
                    //set all sender record to -1
                    memset( m_senderRecord, 0xFF, 
                            m_hdlr->getMaxListSize() * sizeof(int) );
                    m_moduleBuf = new ModuleBuf(bufSize);
                } catch(std::bad_alloc& m) {
                    DJI2MAV_FATAL( "Failed to allocate memory for mavModule! " 
                            "Exception: %s!", m.what() );
                    exit(EXIT_FAILURE);
                }

                DJI2MAV_DEBUG("Construct base MavModule.");
            }


            virtual ~MavModule() {
                DJI2MAV_DEBUG("Going to destruct base MavModule...");
                if(NULL != m_senderRecord) {
                    delete []m_senderRecord;
                    m_senderRecord = NULL;
                }
                if(NULL != m_moduleBuf)
                    delete m_moduleBuf;
                m_hdlr = NULL;
                DJI2MAV_DEBUG("...finish destructing base Mavmodule.");
            }


            inline std::string getName() {
                return m_name;
            }


            inline uint8_t getMySysid() {
                return m_hdlr->getMySysid();
            }


            inline int getMasterGcsIdx() {
                return m_masterGcsIdx;
            }


            int getMasterGcsSenderIdx() {
                if(-1 == m_masterGcsIdx)
                    return -2;
                else
                    return m_senderRecord[m_masterGcsIdx];
            }


            bool setMasterGcsIdx(uint16_t gcsIdx) {
                if( !activateSender(gcsIdx) ) {
                    DJI2MAV_ERROR("Fail to set master GCS to #%u because it " 
                            "is not activated", gcsIdx);
                    return false;
                }
                if(-1 != m_masterGcsIdx) {
                    DJI2MAV_INFO("Switch master GCS from #%u to #%u.", 
                            m_masterGcsIdx, gcsIdx);
                }
                //at last, turn on master mode
                m_masterGcsIdx = gcsIdx;
                return true;
            }


            inline const int* getSenderRecord() {
                return m_senderRecord;
            }


            int getGcsSenderIdx(uint16_t gcsIdx) {
                if( !m_hdlr->isValidMngIdx(gcsIdx) )
                    return -2;
                return m_senderRecord[gcsIdx];
            }


            bool employGcsSender(uint16_t gcsIdx) {
                if( !activateSender(gcsIdx) ) {
                    DJI2MAV_ERROR("Fail to employ a sender for GCS #%u!", gcsIdx);
                    return false;
                }
                //turn off master mode
                m_masterGcsIdx = -1;
                DJI2MAV_DEBUG("Succeed in employing sender for GCS #%u.", gcsIdx);
                return true;
            }


            bool activateSender(uint16_t gcsIdx) {
                if( !m_hdlr->isValidMngIdx(gcsIdx) ) {
                    DJI2MAV_ERROR("Fail to activate sender for invalid GCS " 
                            "index %u!", gcsIdx);
                    return false;
                }
                int sender = m_senderRecord[gcsIdx];
                if( -1 == sender ) {
                    sender = m_hdlr->registerSender(gcsIdx);
                    if(0 > sender) {
                        DJI2MAV_ERROR("Fail to register sender for GCS #%u!", 
                                gcsIdx);
                        return false;
                    }
                    m_senderRecord[gcsIdx] = sender;
                }
                DJI2MAV_DEBUG("Succeed in activating sender with index %u for " 
                        "GCS #%u.", m_senderRecord[gcsIdx], gcsIdx);
                return true;
            }


            bool sendMsgToGcs(uint16_t gcsIdx, mavlink_message_t& msg) {
                if(0 > m_senderRecord[gcsIdx]) {
                    DJI2MAV_ERROR("Fail to send message to GCS #%u because no " 
                            "sender is employed for it!", gcsIdx);
                    return false;
                }
                bool ret = m_hdlr->sendEncodedMsgToMng(gcsIdx, 
                        (uint16_t)m_senderRecord[gcsIdx], msg);
                if(ret) {
                    DJI2MAV_TRACE("Succeed in sending message to GCS #%u " 
                            "using sender with index %u.", gcsIdx, 
                            m_senderRecord[gcsIdx]);
                } else {
                    DJI2MAV_ERROR("Fail to send message to GCS #%u!", gcsIdx);
                }
                return ret;
            }


            bool sendMsgToSys(uint8_t sysid, mavlink_message_t& msg) {
                int idx = m_hdlr->findMngIdx(sysid);
                if( idx <= 0 || idx > m_hdlr->getMaxListSize() ) {
                    DJI2MAV_ERROR("Fail to send message to GCS because no " 
                            "valid index is matched sysid %u! Return value: " 
                            "%d!", sysid, idx);
                    return false;
                }
                //TODO: Cannot call sendEncodedMsgToSys for passing senderIdx
                return sendMsgToGcs( (uint8_t)idx, msg);
            }


            inline bool sendMsgToMaster(mavlink_message_t& msg) {
                return sendMsgToGcs(m_masterGcsIdx, msg);
            }


            bool sendMsgToAll(mavlink_message_t& msg) {
                bool ret = true;
                if(-1 != m_masterGcsIdx) {
                    return sendMsgToMaster(msg);
                } else {
                    bool ret = true;
                    for(uint16_t i = 0; i < m_hdlr->getMaxListSize(); ++i) {
                        if( -1 != m_senderRecord[i] && !sendMsgToGcs(i, msg) ) {
                            DJI2MAV_ERROR("Fail to send message to GCS #%u!", i);
                            ret = false;
                        }
                    }
                    if(!ret)
                        DJI2MAV_ERROR("Fail to send the message to some GCS!");
                    return ret;
                }
            }


            inline bool pushMsg(uint16_t gcsIdx, 
                    const mavlink_message_t &srcMsg) {

                return m_moduleBuf->writeBuf( (uint8_t*)&srcMsg, 
                        sizeof(mavlink_message_t) );

            }


            inline bool pullMsg(mavlink_message_t &destMsg) {

                return m_moduleBuf->readBuf( (uint8_t*)&destMsg, 
                        sizeof(mavlink_message_t) );

            }


            inline void clearBuf() {
                m_moduleBuf->clear();
            }


            bool run() {
                int ret = pthread_create( &m_tid, NULL, thread_call, 
                        (void*)this );
                if(0 != ret) {
                    printf("Fail to create thread for the module.\n");
                    return false;
                }
                return true;
            }


            static void* thread_call(void* param) {
                mavlink_message_t msg;
                while(true) {
                    //call passivelyReceive(msg) inside helper
                    ( (MavModule*)param )->receiveHelper(msg);

                    ( (MavModule*)param )->activelySend();
                }
            }


            void receiveHelper(mavlink_message_t &msg) {
                if( pullMsg(msg) ) {
                    if( -1 != m_masterGcsIdx && msg.sysid != 
                            m_hdlr->findMngSysid(m_masterGcsIdx) ) {
                        return;
                    }
                    passivelyReceive(msg);
                }
            }


            virtual void passivelyReceive(mavlink_message_t &msg) = 0;


            virtual void activelySend() = 0;


        private:
            int* m_senderRecord;
            ModuleBuf* m_moduleBuf;
            int m_masterGcsIdx;
            uint8_t m_masterSysid;

            MavHandler* m_hdlr;

            pthread_t m_tid;
            std::string m_name;

    };

} //namespace dji2mav


#endif