From 592fba7416906409018066d4236db6ed9ad3a170 Mon Sep 17 00:00:00 2001 From: yaapu Date: Fri, 5 Mar 2021 16:10:02 +0100 Subject: [PATCH] AP_Frsky_Telem: frame 0x5007 refactoring --- .../AP_Frsky_SPort_Passthrough.cpp | 29 +++++++------------ .../AP_Frsky_SPort_Passthrough.h | 3 +- 2 files changed, 12 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 9a7936791a..a78bc87463 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -407,35 +407,26 @@ bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void) */ uint32_t AP_Frsky_SPort_Passthrough::calc_param(void) { - const AP_BattMonitor &_battery = AP::battery(); - - uint32_t param = 0; - uint8_t last_param = AP::battery().num_instances() > 1 ? BATT_CAPACITY_2 : BATT_CAPACITY_1; + uint8_t param_id = _paramID; //cache it because it gets changed inside the switch + uint32_t param_value = 0; - // cycle through paramIDs - if (_paramID >= last_param) { - _paramID = 0; - } - - _paramID++; switch (_paramID) { + case NONE: case FRAME_TYPE: - param = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h + param_value = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h + _paramID = BATT_CAPACITY_1; break; - case BATT_FS_VOLTAGE: // was used to send the battery failsafe voltage, lend slot to next param - case BATT_FS_CAPACITY: // was used to send the battery failsafe capacity in mAh, lend slot to next param case BATT_CAPACITY_1: - _paramID = 4; - param = (uint32_t)roundf(_battery.pack_capacity_mah(0)); // battery pack capacity in mAh + param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(0)); // battery pack capacity in mAh + _paramID = AP::battery().num_instances() > 1 ? BATT_CAPACITY_2 : FRAME_TYPE; break; case BATT_CAPACITY_2: - param = (uint32_t)roundf(_battery.pack_capacity_mah(1)); // battery pack capacity in mAh + param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(1)); // battery pack capacity in mAh + _paramID = FRAME_TYPE; break; } //Reserve first 8 bits for param ID, use other 24 bits to store parameter value - param = (_paramID << PARAM_ID_OFFSET) | (param & PARAM_VALUE_LIMIT); - - return param; + return (param_id << PARAM_ID_OFFSET) | (param_value & PARAM_VALUE_LIMIT); } /* diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index af92135bde..a1ce63c65b 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -85,11 +85,12 @@ private: AP_Frsky_Parameters *&_frsky_parameters; enum PassthroughParam : uint8_t { + NONE = 0, FRAME_TYPE = 1, BATT_FS_VOLTAGE = 2, BATT_FS_CAPACITY = 3, BATT_CAPACITY_1 = 4, - BATT_CAPACITY_2 = 5 + BATT_CAPACITY_2 = 5, }; // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format