Mavlink Translator (#58)

Co-authored-by: Roni Kant <[email protected]>

Author: Yaremadzulynsky

Drivers/common/circular_buffer/src/circular_buffer.cpp

@@ -8,7 +8,10 @@ CircularBuffer::CircularBuffer(uint8_t* buf, uint16_t size) {
 }
 
 CircularBuffer::CircularBuffer(const CircularBuffer& other) {
-  
+    buf_ = other.buf_;
+    size_ = other.size_;
+    readPtr_ = other.readPtr_;
+    writePtr_ = other.writePtr_;
 }
 
 bool CircularBuffer::peek(uint8_t& byte, uint16_t index) {

SystemManager/Inc/SystemManager.hpp

@@ -9,6 +9,7 @@
 #include "rcreceiver_datatypes.h"
 #include "independent_watchdog.h"
 #include "ZP_D_PWMChannel.hpp"
+#include "TelemetryManager.hpp"
 
 #define SBUS_THRESHOLD 25
 #define SBUS_MIN 0
@@ -20,6 +21,9 @@ class SystemManager {
     SystemManager();
     ~SystemManager();
 
+    /* Other managers*/
+    TelemetryManager *telemetryManager;
+
     /* Class Functions */
     void flyManually();
 

SystemManager/Src/SystemManager.cpp

@@ -4,78 +4,101 @@
 //
 
 #include "SystemManager.hpp"
-#include "sbus_receiver.hpp"
-#include "sbus_defines.h"
-#include "rcreceiver_datatypes.h"
+
 #include "drivers_config.hpp"
 #include "independent_watchdog.h"
+#include "rcreceiver_datatypes.h"
+#include "sbus_defines.h"
+#include "sbus_receiver.hpp"
 #include "tim.h"
 
-#define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
+#define TIMEOUT_CYCLES 250000  // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
 
 static uint32_t DisconnectionCount = 0;
 float prevthrottle;
 float prevyaw;
 float prevroll;
 float prevpitch;
 
-SystemManager::SystemManager():
-    rcController_(sbus_pointer),
-    throttleMotorChannel_(&htim2, TIM_CHANNEL_1),
-    yawMotorChannel_(&htim2, TIM_CHANNEL_2),
-    rollMotorChannel_(&htim2, TIM_CHANNEL_3),
-    pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
-    invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
-    watchdog_(&hiwdg)
-{}
+SystemManager::SystemManager()
+    : rcController_(sbus_pointer),
+      throttleMotorChannel_(&htim2, TIM_CHANNEL_1),
+      yawMotorChannel_(&htim2, TIM_CHANNEL_2),
+      rollMotorChannel_(&htim2, TIM_CHANNEL_3),
+      pitchMotorChannel_(&htim2, TIM_CHANNEL_4),
+      invertedRollMotorChannel_(&htim3, TIM_CHANNEL_1),
+      watchdog_(&hiwdg) {
+    // VARIABLES FOR TELEMETRY MANAGER TO HAVE AS REFERENCES THEY OBV SHOULD BE PUT SOMEWHERE ELSE,
+    // BUT I FEEL LIKE SM PM WOULD KNOW WHERE. MAYBE IN THE HPP FILE? IDK HOW YOU ARE PLANNING ON
+    // GATHERING THE DATA. I JUST PUT THEM HERE FOR NOW
+    int32_t lat = 0;
+    int32_t lon = 0;
+    int32_t alt = 0;
+    int32_t relative_alt = 0;
+    int16_t vx = 0;
+    int16_t vy = 0;
+    int16_t vz = 0;
+    uint16_t hdg = 0;
+    int32_t time_boot_ms = 0;
+    MAV_STATE state = MAV_STATE::MAV_STATE_STANDBY;
+    MAV_MODE_FLAG mode = MAV_MODE_FLAG::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+    float roll = 0;
+    float pitch = 0;
+    float yaw = 0;
+    float rollspeed = 0;
+    float pitchspeed = 0;
+    float yawspeed = 0;
+
+    this->telemetryManager =
+        new TelemetryManager(lat, lon, alt, relative_alt, vx, vy, vz, hdg, time_boot_ms, state,
+                             mode, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
+    this->telemetryManager->init();
+    // IDK WHERE SM PLANS TO DO THIS, BUT telemetryManager.update() NEEDS TO BE CALLED AT A SEMI
+    // REGULAR INTERVAL AS IT DEALS WITH MESSAGE DECODING AND LOW PRIORITY DATA TRANSMISSION
+}
 
 SystemManager::~SystemManager() {}
 
 void SystemManager::flyManually() {
-    for(;;){
+    for (;;) {
         this->rcInputs_ = rcController_->GetRCControl();
 
-        //TO-DO: need to implement it using is_Data_New;
-        // boolean is true if data has not changed since the last cycle
-        bool is_unchanged{
-        	rcInputs_.throttle == prevthrottle &&
-        	rcInputs_.yaw == prevyaw &&
-			rcInputs_.roll == prevroll &&
-			rcInputs_.pitch == prevpitch
-        };
+        // TO-DO: need to implement it using is_Data_New;
+        //  boolean is true if data has not changed since the last cycle
+        bool is_unchanged{rcInputs_.throttle == prevthrottle && rcInputs_.yaw == prevyaw &&
+                          rcInputs_.roll == prevroll && rcInputs_.pitch == prevpitch};
 
         if (is_unchanged) {
-        	DisconnectionCount += 1; // if its not changed we increment the timeout counter
-        	if (DisconnectionCount > TIMEOUT_CYCLES) { // if timeout has occured
-        		DisconnectionCount = TIMEOUT_CYCLES+1; // avoid overflow but keep value above threshold
-        		this->rcInputs_.arm = 0; // failsafe
-        	}
+            DisconnectionCount += 1;  // if its not changed we increment the timeout counter
+            if (DisconnectionCount > TIMEOUT_CYCLES) {  // if timeout has occured
+                DisconnectionCount =
+                    TIMEOUT_CYCLES + 1;   // avoid overflow but keep value above threshold
+                this->rcInputs_.arm = 0;  // failsafe
+            }
         } else {
-        	DisconnectionCount = 0; //if the data has changed we want to reset out counter
+            DisconnectionCount = 0;  // if the data has changed we want to reset out counter
         }
 
-        watchdog_.refreshWatchdog(); // always hit the dog
+        watchdog_.refreshWatchdog();  // always hit the dog
 
-        if(this->rcInputs_.arm >= (SBUS_MAX/2)) {
-        		this->throttleMotorChannel_.set(rcInputs_.throttle);
-        		this->yawMotorChannel_.set(rcInputs_.yaw);
-        		this->rollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
-        		this->pitchMotorChannel_.set(SBUS_MAX - rcInputs_.pitch);
-        		this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
+        if (this->rcInputs_.arm >= (SBUS_MAX / 2)) {
+            this->throttleMotorChannel_.set(rcInputs_.throttle);
+            this->yawMotorChannel_.set(rcInputs_.yaw);
+            this->rollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
+            this->pitchMotorChannel_.set(SBUS_MAX - rcInputs_.pitch);
+            this->invertedRollMotorChannel_.set(SBUS_MAX - rcInputs_.roll);
 
-        		prevthrottle = rcInputs_.throttle;
-        		prevyaw = rcInputs_.yaw;
-        		prevroll = rcInputs_.roll;
-        		prevpitch = rcInputs_.pitch;
+            prevthrottle = rcInputs_.throttle;
+            prevyaw = rcInputs_.yaw;
+            prevroll = rcInputs_.roll;
+            prevpitch = rcInputs_.pitch;
 
-
-        }
-        else{
+        } else {
             this->throttleMotorChannel_.set(0);
-            this->yawMotorChannel_.set(SBUS_MAX/2);
-            this->rollMotorChannel_.set(SBUS_MAX/2);
-            this->pitchMotorChannel_.set(SBUS_MAX/2);
-            this->invertedRollMotorChannel_.set(SBUS_MAX/2);
+            this->yawMotorChannel_.set(SBUS_MAX / 2);
+            this->rollMotorChannel_.set(SBUS_MAX / 2);
+            this->pitchMotorChannel_.set(SBUS_MAX / 2);
+            this->invertedRollMotorChannel_.set(SBUS_MAX / 2);
         }
     }
 }

TelemetryManager/Inc/MavlinkDecoder.hpp

@@ -0,0 +1,83 @@
+/**
+ * @file MavlinkEncoder.hpp
+ * @brief This file is responsible for decoding MAVLink messages received from the ground station.
+ *
+ * @note Anything future maintainers should know about this file?
+ *
+ * @version Milestone 2
+ * @date 2023-08-24
+ * @author Yarema Dzulynsky: initial structure & implementation
+ *
+ * @warning Any issues you think are important/foresee in the future?
+ */
+
+#ifndef MAVLINKDECODER_H
+#define MAVLINKDECODER_H
+
+#include <functional>
+// #include <iostream>
+// #include <unordered_map>
+
+#include "Official_Mavlink_2_Library/common/mavlink.h"
+
+/**
+ * Macro for registering a decoder function for a specific message type.
+ * @param msgId (int) - the message id of the message type to be decoded.
+ * @param baseName (string) - the base name of the message type to be decoded. Ex. for
+ * mavlink_attitude_t, the base name is attitude. Essentially, whatever is in between mavlink_ and
+ * _t form the message you would like to register. The base name is used to generate the decoder
+ * function name. Ex. for mavlink_attitude_t, the decoder function name is
+ * mavlink_msg_attitude_decode.
+ * @param postProcessor ([](mavlink_**baseName**_t &message)) - the post processor function to be
+ * called after the message. This function must take in a reference to the message type that is
+ * being decoded. You can then use this reference to access the message fields. Ex. message.roll,
+ * message.pitch, etc. Now do what you want with them! :)
+ * @example REGISTER_DECODER(MAVLINK_MSG_ID_ATTITUDE, attitude, [](mavlink_attitude_t &message) {
+ *     //print out the message
+ *     std::cout << "ATTITUDE" << std::endl;
+ *     std::cout << "roll: " << message.roll << std::endl;
+ *     std::cout << "pitch: " << message.pitch << std::endl;
+ *     std::cout << "yaw: " << message.yaw << std::endl;
+ *     std::cout << "rollspeed: " << message.rollspeed << std::endl;
+ *     std::cout << "pitchspeed: " << message.pitchspeed << std::endl;
+ *     };
+ */
+// #define REGISTER_DECODER(msgId, baseName, postProcessor)    \
+//     decodingFunctions[msgId] = [](mavlink_message_t &msg) { \
+//         mavlink_##baseName##_t msgType;                     \
+//         mavlink_msg_##baseName##_decode(&msg, &msgType);    \
+//         postProcessor(msgType);                             \
+//     };
+
+class MavlinkDecoder {
+   public:
+    // The number of messages that have been decoded - used for testing purposes.
+    long messagesHandledSuccessfully = 0;
+
+    // std::unordered_map<int, std::function<void(mavlink_message_t &)>> decodingFunctions;
+    /**
+     * Default constructor.
+     */
+    MavlinkDecoder();
+
+    /**
+     * Default destructor.
+     */
+    ~MavlinkDecoder();
+
+    /**
+     * Unpacks the message, triggering the respective callback set in the switch statement.
+     * @param msg - The preformatted mavlink message, ready for decoding.
+     */
+
+    bool decodeMsg(mavlink_message_t &msg);
+
+    /**
+     * Transforms a byte sequence into mavlink messages, then activates the respective callbacks.
+     * @param buffer - Byte sequence for parsing.
+     * @param bufferSize - Length of the byte sequence.
+     */
+    void parseBytesToMavlinkMsgs(uint8_t *buffer, std::size_t bufferSize);
+};
+
+#endif

TelemetryManager/Inc/MavlinkEncoder.hpp

@@ -0,0 +1,48 @@
+/**
+ * @file MavlinkEncoder.hpp
+ * @brief This file is responsible for encoding MAVLink messages to send to the ground station.
+ *
+ * @note Anything future maintainers should know about this file?
+ *
+ * @version Milestone 2
+ * @date 2023-08-24
+ * @author Yarema Dzulynsky: initial structure & implementation
+ *
+ * @warning Any issues you think are important/foresee in the future?
+ */
+
+#ifndef MAVLINKENCODER_H
+#define MAVLINKENCODER_H
+
+#include <iostream>
+
+#include "Official_Mavlink_2_Library/common/mavlink.h"
+#include "TMCircularBuffer.hpp"
+
+/**
+ * @brief Class dedicated to MAVLink message encoding.
+ *
+ * The 'MavlinkEncoder' class provides an interface and underlying functionality for MAVLink message
+ * encoding, using associated parameters and defined message types.
+ */
+class MavlinkEncoder {
+   public:
+    // The message object that's currently targeted for encoding.
+    mavlink_message_t currentMessage;
+
+    // Default constructor for the MavlinkEncoder class.
+    MavlinkEncoder();
+
+    // Default destructor for the MavlinkEncoder class.
+    ~MavlinkEncoder();
+
+    /**
+     * @brief Helper function to pack a mavlink message into a buffer.
+     *
+     * @param msg The mavlink message to be packed into the buffer.
+     * @param txToGroundByteQueue The buffer to pack the message into.
+     */
+    bool encodeMsgIntoBuffer(mavlink_message_t& msg, TMCircularBuffer& txToGroundByteQueue);
+};
+
+#endif

TelemetryManager/Inc/MavlinkTranslator.hpp

@@ -1,23 +1,30 @@
 /**
  * @file MavlinkTranslator.hpp
- * @brief What does this file do?
+ * @brief This file is responsible for decoding/encoding MAVLink messages received_from/sent_to the ground station.
  *
  * @note Anything future maintainers should know about this file?
  *
- * @version 1.0
+ * @version Milestone 2
  * @date 2023-08-24
  * @author Yarema Dzulynsky: initial structure & implementation
  *
  * @warning Any issues you think are important/foresee in the future?
  */
 
-#include "TMCircularBuffer.hpp"
+#include "MavlinkDecoder.hpp"
+#include "MavlinkEncoder.hpp"
 #include "Official_Mavlink_2_Library/common/mavlink.h"
+#include "TMCircularBuffer.hpp"
 #ifndef MAVLINKTRANSLATOR_H
 #define MAVLINKTRANSLATOR_H
 
 class MavlinkTranslator {
-   public:
+      public:
+    // The decoder and encoder objects.
+    MavlinkDecoder decoder;
+    MavlinkEncoder encoder;
+    // The number of messages that have been decoded - used for testing purposes.
+    long decodedMessages = 0;
     /**
      * @brief Construct a new MavlinkTranslator object. Do whatever needs to be done here.
      *
@@ -47,8 +54,7 @@ class MavlinkTranslator {
      * @param txToGroundByteQueue The CircularBuffer containing the bytes to be sent to the ground
      * station.
      */
-    void addMavlinkMsgToByteQueue(mavlink_message_t &msg,
-                                          TMCircularBuffer &txToGroundByteQueue);
+    void addMavlinkMsgToByteQueue(mavlink_message_t &msg, TMCircularBuffer &txToGroundByteQueue);
 };
 
 #endif  // MAVLINKTRANSLATOR_H