MAVLinkMobilityBase

Description

Functions

~MAVLinkMobilityBase

virtual ~MAVLinkMobilityBase();

initialize

virtual void initialize(int stage) override;

waitUntilReady

virtual void waitUntilReady();

Freezes initialization until the vehicle is pre-arm ready

startSimulator

virtual void startSimulator();

Starts a SITL simulator instance

openSocket

virtual void openSocket();

Opens the socket to the SITL instance

handleMessage

virtual void handleMessage(cMessage *msg) override;

finish

virtual void finish() override;

move

virtual void move() override;

orient

virtual void orient() override;

notify

virtual bool notify(int fd) override;

This function is called by the RealTimeScheduler when a message is received on the open socket. It reads the message and translates it to a MAVLink message that is sent to receibeTelemetry. It should probably not be overriden.

receiveTelemetry

virtual void receiveTelemetry(mavlink_message_t const& message);

Callback function called when a message is received from the simulated SITL instance. The default behaviour is to check if the message completes the condition of the active MAVLinkInstruction (if one exists) and to call the next MAVLinkInstruction if the current one is done.

performInitialSetup

virtual void performInitialSetup();

Performs initial setup on the vehicle. This includes setting update rate and setting home to current position

updatePosition

virtual void updatePosition(const mavlink_message_t& message);

Receives telemetry and updates the vehicle's position

queueMessage

virtual void queueMessage(mavlink_message_t message, Condition condition = {}, simtime_t timeout = -1, int retries = 0, std::string label = "");

Queues instruction

queueInstruction

virtual void queueInstruction(std::shared_ptr<Instruction> instruction);

Queues instruction

queueInstructions

virtual void queueInstructions(std::vector<std::shared_ptr<Instruction>> instructions);

Queues list of instructions

clearQueue

virtual void clearQueue();

nextMessage

virtual void nextMessage();

Starts first instruction in queue

nextMessageIfReady

virtual void nextMessageIfReady();

Gets first instruction from queue if the active instruction is completed

sendActiveMessage

virtual bool sendActiveMessage();

Sends the current active instruction

sendMessage

virtual bool sendMessage(const mavlink_message_t& message, bool shouldRetry, int &currentTries, int maxRetries);

Sends a message and returns if successful

mavlink_message_t getActiveMessage() { return (activeInstruction != nullptr) ? activeInstruction->message : mavlink_message_t{}; };

Condition getActiveCondition() { return (activeInstruction != nullptr) ? activeInstruction->condition : Condition{}; };

simtime_t getActiveTimeout() { return (activeInstruction != nullptr) ? activeInstruction->timeout : 0; };

int getActiveRetries() { return (activeInstruction != nullptr) ? activeInstruction->retries : 0; };

bool getActiveCompleted() { return (activeInstruction != nullptr) ? activeInstruction->completed : true; };

std::string getActiveLabel() { return (activeInstruction != nullptr) ? activeInstruction->label : ""; };

cMessage

cMessage *timeoutMessage = new cMessage("MAVLinkMobilityBaseMessage", CommunicationSelfMessages::TIMEOUT);

cMessage

cMessage *heartbeatMessage = new cMessage("MAVLinkMobilityBaseMessage", CommunicationSelfMessages::HEARTBEAT);

cMessage

cMessage *updateMessage = new cMessage("MAVLinkMobilityBaseMessage", CommunicationSelfMessages::UPDATE);