diff --git a/SystemManager/Src/SystemManager.cpp b/SystemManager/Src/SystemManager.cpp index 98c2036..86e766f 100644 --- a/SystemManager/Src/SystemManager.cpp +++ b/SystemManager/Src/SystemManager.cpp @@ -12,7 +12,7 @@ #include "sbus_receiver.hpp" #include "tim.h" #include "GroundStationCommunication.hpp" - +#include "TelemetryManager.hpp" #define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec #define TIMOUT_MS 10000 // 10 sec @@ -34,28 +34,32 @@ SystemManager::SystemManager() // 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; + + // Struct containing the state of the drone + StateData stateData; + stateData.lat = 0; + stateData.lon = 0; + stateData.alt = 0; + stateData.relative_alt = 0; + stateData.vx = 0; + stateData.vy = 0; + stateData.vz = 0; + stateData.hdg = 0; + stateData.time_boot_ms = 0; + stateData.roll = 0; + stateData.pitch = 0; + stateData.yaw = 0; + stateData.rollspeed = 0; + stateData.pitchspeed = 0; + stateData.yawspeed = 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; // Creating parameters for the GroundStationCommunication that will be passed to telemetryManager TMCircularBuffer* DMAReceiveBuffer = &(new TMCircularBuffer(rfd900_circular_buffer)); - // the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the + // the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the // ground station. uint8_t* lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE]; @@ -66,9 +70,10 @@ SystemManager::SystemManager() GroundStationCommunication GSC = new GroundStationCommunication(DMAReceiveBuffer, lowPriorityTransmitBuffer, highPriorityTransmitBuffer, RFD900_BUF_SIZE); - this->telemetryManager = - new TelemetryManager(lat, lon, alt, relative_alt, vx, vy, vz, hdg, time_boot_ms, state, - mode, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, GSC); + // the buffer that stores the bytes received from the ground station. + MavlinkTranslator MT; + + this->telemetryManager = new TelemetryManager(stateData, state, mode, GSC, MT); 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 diff --git a/TelemetryManager/Inc/TelemetryManager.hpp b/TelemetryManager/Inc/TelemetryManager.hpp index 52a2568..334590a 100644 --- a/TelemetryManager/Inc/TelemetryManager.hpp +++ b/TelemetryManager/Inc/TelemetryManager.hpp @@ -21,68 +21,89 @@ #include "MavlinkTranslator.hpp" #include "task.h" -class TelemetryManager { - public: - - MavlinkTranslator MT; - // the buffer that stores the bytes received from the ground station. - - // The object that facilitates communication with the ground station (?) - // I'm not 100% sure what GroundStationCommunication does - GroundStationCommunication& GSC; - +struct StateData { /*References to variables that contain the state of the drone (lat, lng, yaw, pitch, etc..). * They are updated by System Manager*/ // Altitude (MSL) (unit: // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int32_t& alt; + // The latitude of the drone (unit: // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int32_t& lat; + // The longitude of the drone (unit: // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int32_t& lon; + // Altitude above home (unit: // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int32_t& relative_alt; + // Ground X Speed (Latitude, positive north) (unit: // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int16_t& vx; + // Ground Y Speed (Longitude, positive east) (unit: // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int16_t& vy; + // Ground Z Speed (Altitude, positive down) (unit: // cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int16_t& vz; + // Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX // (unit: cdeg)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). uint16_t& hdg; // Timestamp (time since system boot) (unit: ms) // (https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). int32_t& time_boot_ms; - // System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE). - MAV_STATE& state; - // System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG). - MAV_MODE_FLAG& mode; + // Roll angle (-pi..+pi) (unit: rad) // (https://mavlink.io/en/messages/common.html#ATTITUDE). float& roll; + // Pitch angle (-pi..+pi) (unit: rad) // (https://mavlink.io/en/messages/common.html#ATTITUDE). float& pitch; + // Yaw angle (-pi..+pi) (unit: rad) // (https://mavlink.io/en/messages/common.html#ATTITUDE). float& yaw; + // Roll angular speed (unit: rad/s) // (https://mavlink.io/en/messages/common.html#ATTITUDE). float& rollspeed; + // Pitch angular speed (unit: rad/s) // (https://mavlink.io/en/messages/common.html#ATTITUDE). float& pitchspeed; + // yawspeed Yaw angular speed (unit: rad/s) // (https://mavlink.io/en/messages/common.html#ATTITUDE). float& yawspeed; +}; + +class TelemetryManager { + public: + + // struct containing variables that relate to the state of the drone (lat, lng, yaw, pitch, etc..). + StateData stateData; + + // the buffer that stores the bytes received from the ground station. + MavlinkTranslator& MT; + + // The object that facilitates communication with the ground station + GroundStationCommunication& GSC; + + + // System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE). + MAV_STATE& state; + // System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG). + MAV_MODE_FLAG& mode; + + /** * @brief Create and configure FreeRTOS tasks. * @@ -98,43 +119,14 @@ class TelemetryManager { /** * @brief Construct a new Telemetry Manager object. Does not initialize the tasks. * To do so call the init() method. - * @param lat The latitude of the drone (unit: - * degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param lon The longitude of the drone(unit: - * degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param alt Altitude (MSL) (unit: - * mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param relative_alt Altitude above home (unit: - * mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param vx Ground X Speed (Latitude, positive north) (unit: - * cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param vy Ground Y Speed (Longitude, positive east) (unit: - * cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param vz Ground Z Speed (Altitude, positive down) (unit: - * cm/s)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param hdg Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * (unit: cdeg)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - * @param time_boot_ms Timestamp (time since system boot) (unit: ms) - * (https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). + * @param stateData The state of the drone (lat, lng, yaw, pitch, etc..). * @param state System status flag (https://mavlink.io/en/messages/minimal.html#MAV_STATE). * @param mode System mode bitmap (https://mavlink.io/en/messages/minimal.html#MAV_MODE_FLAG). - * @param roll Roll angle (-pi..+pi) (unit: rad) - * (https://mavlink.io/en/messages/common.html#ATTITUDE). - * @param pitch Pitch angle (-pi..+pi) (unit: rad) - * (https://mavlink.io/en/messages/common.html#ATTITUDE). - * @param yaw Yaw angle (-pi..+pi) (unit: rad) - * (https://mavlink.io/en/messages/common.html#ATTITUDE). - * @param rollspeed Roll angular speed (unit: rad/s) - * (https://mavlink.io/en/messages/common.html#ATTITUDE). - * @param pitchspeed Pitch angular speed (unit: rad/s) - * (https://mavlink.io/en/messages/common.html#ATTITUDE). - * @param yawspeed Yaw angular speed (unit: rad/s) - * (https://mavlink.io/en/messages/common.html#ATTITUDE). + * @param GSC Object to handle communication with the groundstation + * @param MT Object to translate MAVLink data */ - TelemetryManager(int32_t& lat, int32_t& lon, int32_t& alt, int32_t& relative_alt, int16_t& vx, - int16_t& vy, int16_t& vz, uint16_t& hdg, int32_t& time_boot_ms, - MAV_STATE& state, MAV_MODE_FLAG& mode, float& roll, float& pitch, float& yaw, - float& rollspeed, float& pitchspeed, float& yawspeed, GroundStationCommunication& GSC); + TelemetryManager(StateData& stateData, MAV_STATE& state, MAV_MODE_FLAG& mode, + GroundStationCommunication& GSC, MavlinkTranslator& MT); /** * @brief Destroy the Telemetry Manager object. Also destroys the FreeRTOS tasks associated with diff --git a/TelemetryManager/Src/TelemetryManager.cpp b/TelemetryManager/Src/TelemetryManager.cpp index 5d3b3bb..67f19f7 100644 --- a/TelemetryManager/Src/TelemetryManager.cpp +++ b/TelemetryManager/Src/TelemetryManager.cpp @@ -4,29 +4,13 @@ // FreeRTOS task handle for the routineDataTransmission task TaskHandle_t routineDataTransmissionH = NULL; -TelemetryManager::TelemetryManager(int32_t& lat, int32_t& lon, int32_t& alt, int32_t& relative_alt, - int16_t& vx, int16_t& vy, int16_t& vz, uint16_t& hdg, - int32_t& time_boot_ms, MAV_STATE& state, MAV_MODE_FLAG& mode, - float& roll, float& pitch, float& yaw, float& rollspeed, - float& pitchspeed, float& yawspeed, GroundStationCommunication& GSC) - : GSC(GSC) - lat(lat), - lon(lon), - alt(alt), - relative_alt(relative_alt), - vx(vx), - vy(vy), - vz(vz), - hdg(hdg), - time_boot_ms(time_boot_ms), +TelemetryManager::TelemetryManager(StateData& stateData, MAV_STATE& state, MAV_MODE_FLAG& mode, + GroundStationCommunication& GSC, MavlinkTranslator& MT) + : stateData(stateData), state(state), - mode(mode), - roll(roll), - pitch(pitch), - yaw(yaw), - rollspeed(rollspeed), - pitchspeed(pitchspeed), - yawspeed(yawspeed) {} + mode(mode) + GSC(GSC), + MT(MT) {} TelemetryManager::~TelemetryManager() { // Destructor @@ -84,8 +68,10 @@ void routineDataTransmission(void* pvParameters) { // Pack the message with the actual data mavlink_msg_global_position_int_pack(system_id, component_id, &globalPositionIntMsg, - tm->time_boot_ms, tm->lat, tm->lon, tm->alt, - tm->relative_alt, tm->vx, tm->vy, tm->vz, tm->hdg); + tm->stateData.time_boot_ms, tm->stateData.lat, + tm->stateData.lon, tm->stateData.alt, + tm->stateData.relative_alt, tm->stateData.vx, + tm->stateData.vy, tm->stateData.vz, tm->stateData.hdg); // Add the packed message to the byte queue for later transmission tm->MT.addMavlinkMsgToByteQueue(globalPositionIntMsg, tm->GSC.highPriorityTransmitBuffer); @@ -93,8 +79,11 @@ void routineDataTransmission(void* pvParameters) { // Create an attitude message mavlink_message_t attitudeMsg = {0}; // Pack the message with the actual data - mavlink_msg_attitude_pack(system_id, component_id, &attitudeMsg, tm->time_boot_ms, tm->roll, - tm->pitch, tm->yaw, tm->rollspeed, tm->pitchspeed, tm->yawspeed); + mavlink_msg_attitude_pack(system_id, component_id, &attitudeMsg, + tm->stateData.time_boot_ms, tm->stateData.roll, tm->stateData.pitch, + tm->stateData.yaw, tm->stateData.rollspeed, tm->stateData.pitchspeed, + tm->stateData.yawspeed); + // Add the packed message to the byte queue for later transmission tm->MT.addMavlinkMsgToByteQueue(attitudeMsg, tm->GSC.highPriorityTransmitBuffer);