How to use =========== .. toctree:: :maxdepth: 3 :caption: Contents: Platform support ---------------- Framework: - Arduino This library is made to be used with: - ESP32 family - Teensy 4.X family Generate headers for custom messages ------------------------------------ The mavlink headers are generated using the mavlink generator. A shell script is provided for easy generation. The common messages set is already generated and can be found in the libDM_mavlink/autogen_mavlink/common folder. If you need to add messages to the set, you can modify libDM_mavlink/message_definition/sysrox.xml and add your messages. Then, you can generate the headers using the following command: .. code-block:: bash ./generate_mavlink.sh More informations about the mavlink generator can be found here: https://mavlink.io/en/ Necessary inclusions -------------------- The header of all selected messages selected messages has to be included. The header of libDM_mavlink also .. code-block:: c++ #include "libDM_mavlink.hpp" #include "autogen_mavlink/common/mavlink_msg_attitude.h" #include "autogen_mavlink/sysrox/sysrox.h" Message reception ----------------- 1. Create a MAVLINK_UTILS object ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. important:: API ref: :cpp:class:`MAVLINK_UTILS` .. admonition:: Example .. code-block:: c++ MAVLINK_UTILS mavlink(&streamObj); 2. (optional) Add a MAVLINK_STREAM object ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ If no interface has already been specified, you can configure an interface and add it to the :cpp:class:`MAVLINK_UTILS` object .. important:: API ref: :cpp:func:`MAVLINK_UTILS::addMavlinkStream` .. admonition:: Example .. code-block:: c++ // Initialize serial link Serial2.setRxBufferSize(200); Serial2.setTxBufferSize(200); Serial2.begin(921600, SERIAL_8N1, pinProtocolRx, pinProtocolTx); mavlink.addMavlinkStream(&Serial2); 3. Add messages to monitor ~~~~~~~~~~~~~~~~~~~~~~~~~~ .. important:: API ref: :cpp:func:`MAVLINK_UTILS::addMsgToMonitor` .. admonition:: Example .. code-block:: c++ mavlink.addMsgToMonitor(MAVLINK_MSG_ID_ATTITUDE); mavlink.addMsgToMonitor(MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION); 4. Get your own subset of messages list ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. important:: API ref: :cpp:func:`MAVLINK_UTILS::getMonitoredMsgsList`, :cpp:func:`MAVLINK_MSG_LIST::getSubset` .. admonition:: Example .. code-block:: c++ // Get own subset of message to receive (all messages in this case) std::vector messageToReceiveIdx = { MAVLINK_MSG_ID_ATTITUDE, MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION}; MAVLINK_MSG_LIST myReceiveList = mavlink.getMonitoredMsgsList()->getSubset(messageToReceiveIdx); 5. (optional) Save references for the monitored messages ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ To avoid getting the reference from the list every time, a reference can be saved at init for performance .. important:: API ref: :cpp:func:`MAVLINK_MSG_LIST::getMsg` .. admonition:: Example .. code-block:: c++ // Get references to the serialized messages. Not mandatory as we can use the list but more efficient: MAVLINK_MSG* attitudeMsgRef = myReceiveList.getMsg(MAVLINK_MSG_ID_ATTITUDE); MAVLINK_MSG* calibrationMsgRef = myReceiveList.getMsg(MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION); 6. Initialize the deserialized structs to fill with received messages ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. admonition:: Example .. code-block:: c++ mavlink_attitude_t attitudeReceived; mavlink_sysrox_ins_generic_calibration_baro_sensor_correction_t calibrationReceived; 7. Call updateMonitoredMessages(buffer, bufferLength) at every step ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ The updateMonitoredMessages has to be called periodically and feeded with the received buffer on some hardware interface. It will update the monitored messages list with the new messages received. .. important:: API ref: :cpp:func:`MAVLINK_UTILS::updateMonitoredMessages` .. admonition:: Example .. code-block:: c++ uint8_t receptionSerialBuffer[MAVLINK_MAX_PACKET_LEN]; size_t size = Serial2.available(); OP::UART::readBytesBuffer(&Serial2, receptionSerialBuffer, size); mavlink.updateMonitoredMessages(receptionSerialBuffer, size); 8. Get the last message of a specific ID (or all) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ The messages have been updated automatically with updateMonitoredMessages calls. It is the possible to check if the message is new and get the last message of a specific ID. 8.1 Check if message has been received on some registered interface """""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""""" To check if the message has been updated, you can use the built in method inside your :cpp:class:`MAVLINK_MSG_LIST` object: :cpp:func:`MAVLINK_MSG_LIST::getIsNew` If a reference to the :cpp:class:`MAVLINK_MSG` has been saved during initialization, it is also possible to check the 'mIsNew' attribute of the message. 1. With the reference created during initialization .. admonition:: Example .. code-block:: c++ attitudeMsgRef->mIsNew 2. With the list object .. admonition:: Example .. code-block:: c++ myReceiveList.getIsNew(MAVLINK_MSG_ID_ATTITUDE) 8.2 Method 1: Get the last updated message """""""""""""""""""""""""""""""""""""""""""" .. admonition:: Example .. code-block:: c++ if (myReceiveList.getIsNew(MAVLINK_MSG_ID_ATTITUDE)) // or attitudeMsgRef->mIsNew { Serial.println("Attitude received"); // Decode the message. You can get the serialized message with direct alias (created // during initialization like in this example) or MAVLINK_MSG* attitudeMsgRef = // myReceiveList.getMsg(MAVLINK_MSG_ID_ATTITUDE) mavlink_msg_attitude_decode(&attitudeMsgRef->getMavlinkMsg(), &attitudeReceived); Serial.println(attitudeReceived.yaw); Serial.println(attitudeReceived.pitch); Serial.println(attitudeReceived.roll); Serial.println(attitudeReceived.time_boot_ms); } if (myReceiveList.getIsNew( MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION)) { Serial.println("Calibration received"); mavlink_msg_sysrox_ins_generic_calibration_baro_sensor_correction_decode( &calibrationMsgRef->getMavlinkMsg(), &calibrationReceived); for (size_t i = 0; i < 3; i++) { Serial.print(calibrationReceived.xyzBias[i]); Serial.print(" "); } Serial.println(); Serial.println(calibrationReceived.opcDeltaPressure); for (size_t i = 0; i < 5; i++) { Serial.print(calibrationReceived.gTemperatureDriftValue[i]); Serial.print(" "); } Serial.println(); } 8.3. Method 2: Get only some field of the last received message .. admonition:: Example .. code-block:: c++ if (myReceiveList.getIsNew(MAVLINK_MSG_ID_ATTITUDE)) // or attitudeMsgRef->mIsNew { Serial.println("Attitude received"); Serial.println(mavlink_msg_attitude_get_yaw(&attitudeMsgRef->getMavlinkMsg())); } Message emission ----------------- The parts marked as optional are included if you choose periodic sending of messages. This kind of use is not recommended as it is less efficient than sending messages when they are ready. 1. Create a MAVLINK_UTILS object ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. important:: API ref: :cpp:class:`MAVLINK_UTILS` .. admonition:: Example .. code-block:: c++ MAVLINK_UTILS mavlink(&streamObj); 2. Add a MAVLINK_STREAM object ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ If no interface has already been specified, you can configure an interface and add it to the :cpp:class:`MAVLINK_UTILS` object .. important:: API ref: :cpp:func:`MAVLINK_UTILS::addMavlinkStream` .. admonition:: Example .. code-block:: c++ // Initialize serial link Serial2.setRxBufferSize(200); Serial2.setTxBufferSize(200); Serial2.begin(921600, SERIAL_8N1, pinProtocolRx, pinProtocolTx); mavlink.addMavlinkStream(&Serial2); 3. (optional) Add messages to sending list ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ The messages are added to a sending list with a source list for each message. .. important:: API ref: :cpp:func:`MAVLINK_UTILS::addMsgToSendingList`, :cpp:enum:`MAVLINK_SOURCE` .. admonition:: Example .. code-block:: c++ // We want to send these messages on the serial interface std::vector sendingInterfaces = {MAVLINK_SOURCE::SOURCE_SERIAL}; mavlink.addMsgToSendingList(MAVLINK_MSG_ID_ATTITUDE, sendingInterfaces); mavlink.addMsgToSendingList( MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION, sendingInterfaces); 4. (optional) Get your own subset of messages list ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. important:: API ref: :cpp:func:`MAVLINK_UTILS::getMonitoredMsgsList`, :cpp:func:`MAVLINK_MSG_LIST::getSubset` .. admonition:: Example .. code-block:: c++ // Get own subset of message to send (all messages in this case) std::vector messageToSendIdx = { MAVLINK_MSG_ID_ATTITUDE, MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION}; MAVLINK_MSG_LIST mySendList = mavlink.getMsgsToSendList()->getSubset(messageToSendIdx); // optional 5. (optional) Save references for the monitored messages ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ To avoid getting the reference from the list every time, a reference can be saved at init for performance .. important:: API ref: :cpp:func:`MAVLINK_MSG_LIST::getMsg` .. admonition:: Example .. code-block:: c++ // Get references to the serialized messages: MAVLINK_MSG* attitudeMsgRef = mySendList.getMsg(MAVLINK_MSG_ID_ATTITUDE); MAVLINK_MSG* calibrationMsgRef = mySendList.getMsg(MAVLINK_MSG_ID_SYSROX_INS_GENERIC_CALIBRATION_BARO_SENSOR_CORRECTION); 6. Initialize generic message and the deserialized structs ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. admonition:: Example .. code-block:: c++ // Initialize generic message mavlink_message_t genericMsg = mavlink_message_t(); // Get the deserialized structs for the messages that need to be sent mavlink_attitude_t attitudeToSend; mavlink_sysrox_ins_generic_calibration_baro_sensor_correction_t calibrationToSend; 7. Update messages and send them ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Update the messages you want to send and indicate they are ready to be sent. Once done, the message has to be serialized into the generic message using mavlink autogenerated function. The message can then be sent manually using :cpp:func:`MAVLINK_UTILS::sendMessage` or automatically using :cpp:func:`MAVLINK_MSG::updateMsgForSend` and with calling periodically :cpp:func:`MAVLINK_UTILS::sendMessages` . .. tip:: As the :cpp:func:`MAVLINK_UTILS::sendMessages` is called every loop step, the direct call to :cpp:func:`MAVLINK_UTILS::sendMessage` when the message is filled is recommended. In this case, no need to use the sending list. In this case the steps are just to fill your mavlink messages, serialize it and use :cpp:func:`MAVLINK_UTILS::sendMessage` to send it. .. important:: API ref: :cpp:func:`MAVLINK_MSG::updateMsgForSend`, :cpp:func:`MAVLINK_UTILS::sendMessage`, :cpp:func:`MAVLINK_UTILS::sendMessages` The following examples shows how to send the 2 messages manually and automatically at 200Hz. 1. Manually .. admonition:: Example .. code-block:: c++ long t_now = millis(); while (true) { // Send every 5 ms if ((millis() - t_now) > 5) { t_now = millis() // Update and send the attitude message attitudeToSend.time_boot_ms += 2; attitudeToSend.pitch = 0.666; attitudeToSend.roll = 0.777; attitudeToSend.yaw = 0.888; mavlink_msg_attitude_encode(0, 0, &genericMsg, &attitudeToSend); mavlink.sendMessage(MAVLINK_SOURCE::SOURCE_SERIAL, genericMsg); // Update and send the calibration message calibrationToSend.xyzBias[0] = 0.111; calibrationToSend.xyzBias[1] = 0.222; calibrationToSend.xyzBias[2] = 0.333; calibrationToSend.opcDeltaPressure = 0.444; calibrationToSend.gTemperatureDriftValue[3] = 0.555; mavlink_msg_sysrox_ins_generic_calibration_baro_sensor_correction_encode( 0, 0, &genericMsg, &calibrationToSend); mavlink.sendMessage(MAVLINK_SOURCE::SOURCE_SERIAL, genericMsg); // custom immediate sending } } 2. Automatically .. admonition:: Example .. code-block:: c++ long t_now = millis(); while (true) // main loop { // Send every 5 ms if ((millis() - t_now) > 5) { t_now = millis() // Update and send the attitude message attitudeToSend.time_boot_ms += 2; attitudeToSend.pitch = 0.666; attitudeToSend.roll = 0.777; attitudeToSend.yaw = 0.888; mavlink_msg_attitude_encode(0, 0, &genericMsg, &attitudeToSend); attitudeMsgRef->updateMsgForSend(genericMsg); // Update and send the calibration message calibrationToSend.xyzBias[0] = 0.111; calibrationToSend.xyzBias[1] = 0.222; calibrationToSend.xyzBias[2] = 0.333; calibrationToSend.opcDeltaPressure = 0.444; calibrationToSend.gTemperatureDriftValue[3] = 0.555; mavlink_msg_sysrox_ins_generic_calibration_baro_sensor_correction_encode( 0, 0, &genericMsg, &calibrationToSend); calibrationMsgRef->updateMsgForSend(genericMsg); } mavlink.sendMessages(); // send messages when they are ready }