AP_Frsky_Telem: frame 0x5007 refactoring

Icatme · Mar 19, 2021 · 592fba7 · 592fba7
1 parent 0536be1
commit 592fba7
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 20 deletions.
diff --git 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
@@ -85,11 +85,12 @@ class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry
     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