Merge branch 'main' into refactor/watchdog

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,13 +4,15 @@
 //
 
 #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 TIMOUT_MS 10000 // 10 sec
 
@@ -20,63 +22,86 @@ 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_(TIMOUT_MS)
-{}
+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_(TIMOUT_MS) {
+ // 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