diff --git a/SystemManager/Src/SystemManager.cpp b/SystemManager/Src/SystemManager.cpp index dc80723..59da5b9 100644 --- a/SystemManager/Src/SystemManager.cpp +++ b/SystemManager/Src/SystemManager.cpp @@ -11,7 +11,8 @@ #include "sbus_defines.h" #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 @@ -33,17 +34,20 @@ 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 + + // Struct containing the state of the drone + StateData stateData; + + // values to be assigned to stateData + int32_t alt = 0; 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; @@ -51,9 +55,45 @@ SystemManager::SystemManager() 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); + // use the memory address of the above, since stateData uses pointers + stateData.alt = &alt; + stateData.lat = ⪫ + stateData.lon = &lon; + stateData.relative_alt = &relative_alt; + stateData.vx = &vx; + stateData.vy = &vy; + stateData.vz = &vz; + stateData.hdg = &hdg; + stateData.time_boot_ms = &time_boot_ms; + stateData.roll = &roll; + stateData.pitch = &pitch; + stateData.yaw = &yaw; + stateData.rollspeed = &rollspeed; + stateData.pitchspeed = &pitchspeed; + stateData.yawspeed = &yawspeed; + + MAV_STATE state = MAV_STATE::MAV_STATE_STANDBY; + MAV_MODE_FLAG mode = MAV_MODE_FLAG::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + + // 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 + // ground station. + uint8_t* lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE]; + + // the buffer that stores routine/high_priority bytes (ex. heading, general state data) to be + // sent to the ground station. + uint8_t* highPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE]; + + GroundStationCommunication GSC = *(new GroundStationCommunication(DMAReceiveBuffer, lowPriorityTransmitBuffer, + highPriorityTransmitBuffer, RFD900_BUF_SIZE)); + + // 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 43aa435..7d8b9ba 100644 --- a/TelemetryManager/Inc/TelemetryManager.hpp +++ b/TelemetryManager/Inc/TelemetryManager.hpp @@ -21,70 +21,88 @@ #include "MavlinkTranslator.hpp" #include "task.h" -class TelemetryManager { - public: - GroundStationCommunication GSC; - MavlinkTranslator MT; - // the buffer that stores the bytes received from the ground station. - TMCircularBuffer* DMAReceiveBuffer; - // the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the - // ground station. - uint8_t* lowPriorityTransmitBuffer; - // the buffer that stores routine/high_priority bytes (ex. heading, general state data) to be - // sent to the ground station. - uint8_t* highPriorityTransmitBuffer; - +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; + int32_t* alt; + // The latitude of the drone (unit: // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - int32_t& lat; + int32_t* lat; + // The longitude of the drone (unit: // degE7)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - int32_t& lon; + int32_t* lon; + // Altitude above home (unit: // mm)(https://mavlink.io/en/messages/common.html#GLOBAL_POSITION_INT). - int32_t& relative_alt; + 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; + 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; + 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; + 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; + 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; + int32_t* time_boot_ms; + // Roll angle (-pi..+pi) (unit: rad) // (https://mavlink.io/en/messages/common.html#ATTITUDE). - float& roll; + float* roll; + // Pitch angle (-pi..+pi) (unit: rad) // (https://mavlink.io/en/messages/common.html#ATTITUDE). - float& pitch; + float* pitch; + // Yaw angle (-pi..+pi) (unit: rad) // (https://mavlink.io/en/messages/common.html#ATTITUDE). - float& yaw; + float* yaw; + // Roll angular speed (unit: rad/s) // (https://mavlink.io/en/messages/common.html#ATTITUDE). - float& rollspeed; + float* rollspeed; + // Pitch angular speed (unit: rad/s) // (https://mavlink.io/en/messages/common.html#ATTITUDE). - float& pitchspeed; + float* pitchspeed; + // yawspeed Yaw angular speed (unit: rad/s) // (https://mavlink.io/en/messages/common.html#ATTITUDE). - float& yawspeed; + 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. @@ -101,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); + 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 f154490..1840292 100644 --- a/TelemetryManager/Src/TelemetryManager.cpp +++ b/TelemetryManager/Src/TelemetryManager.cpp @@ -4,34 +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) - : DMAReceiveBuffer(new TMCircularBuffer(rfd900_circular_buffer)), - lowPriorityTransmitBuffer(new uint8_t[RFD900_BUF_SIZE]), - highPriorityTransmitBuffer(new uint8_t[RFD900_BUF_SIZE]), - GSC(*DMAReceiveBuffer, lowPriorityTransmitBuffer, highPriorityTransmitBuffer, - RFD900_BUF_SIZE), - 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) {} + GSC(GSC), + MT(MT) {} TelemetryManager::~TelemetryManager() { // Destructor @@ -89,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); @@ -98,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);