From f96b2721f3415ce6cff2ed7a9e133e159e5d05b7 Mon Sep 17 00:00:00 2001 From: larry-pan Date: Sun, 13 Oct 2024 19:55:13 -0400 Subject: [PATCH] Pointers for stateData --- SystemManager/Src/SystemManager.cpp | 3 ++- TelemetryManager/Inc/TelemetryManager.hpp | 30 +++++++++++------------ TelemetryManager/Src/TelemetryManager.cpp | 14 +++++------ 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/SystemManager/Src/SystemManager.cpp b/SystemManager/Src/SystemManager.cpp index 86e766f3..a0301ffa 100644 --- a/SystemManager/Src/SystemManager.cpp +++ b/SystemManager/Src/SystemManager.cpp @@ -57,7 +57,8 @@ SystemManager::SystemManager() 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)); + 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. diff --git a/TelemetryManager/Inc/TelemetryManager.hpp b/TelemetryManager/Inc/TelemetryManager.hpp index 334590aa..7d8b9bac 100644 --- a/TelemetryManager/Inc/TelemetryManager.hpp +++ b/TelemetryManager/Inc/TelemetryManager.hpp @@ -26,62 +26,62 @@ struct StateData { * 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; + 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; }; diff --git a/TelemetryManager/Src/TelemetryManager.cpp b/TelemetryManager/Src/TelemetryManager.cpp index 67f19f7a..7c9b8247 100644 --- a/TelemetryManager/Src/TelemetryManager.cpp +++ b/TelemetryManager/Src/TelemetryManager.cpp @@ -68,10 +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->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); + *(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); @@ -80,9 +80,9 @@ void routineDataTransmission(void* pvParameters) { mavlink_message_t attitudeMsg = {0}; // Pack the message with the actual data 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); + *(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);