AP_RCTelemetry: fixes for vario

ArduPilot · Apr 28, 2024 · 53b938d · 53b938d
1 parent f4e70dc
commit 53b938d
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 12 deletions.
diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
@@ -931,9 +931,9 @@ void AP_CRSF_Telem::calc_battery()
 
 uint16_t AP_CRSF_Telem::get_altitude_packed()
 {
-    int32_t altitude_dm = get_nav_alt_m() * 10;
+    int32_t altitude_dm = get_nav_alt_m(Location::AltFrame::ABOVE_HOME) * 10;
 
-    enum {
+    enum : uint32_t {
         ALT_MIN_DM = 10000,                     // minimum altitude in dm
         ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM, // altitude of precision changing in dm
         ALT_MAX_DM = 0x7ffe * 10 - 5,           // maximum altitude in dm
@@ -943,29 +943,29 @@ uint16_t AP_CRSF_Telem::get_altitude_packed()
         return 0; // minimum
     }
     if (altitude_dm > ALT_MAX_DM) { // more than maximum
-        return 0xfffe; // maximum
+        return 0xFFFEU; // maximum
     }
     if(altitude_dm < ALT_THRESHOLD_DM) { //dm-resolution range
-        return altitude_dm + ALT_MIN_DM;
+        return uint16_t(altitude_dm + ALT_MIN_DM);
     }
-    return ((altitude_dm + 5) / 10) | 0x8000; // meter-resolution range
+    return uint16_t((altitude_dm + 5) / 10) | uint16_t(0x8000); // meter-resolution range
 }
 
 int8_t AP_CRSF_Telem::get_vertical_speed_packed()
 {
     float vspeed = get_vspeed_ms();
-    int16_t vertical_speed_cm_s = vspeed * 100;
+    float vertical_speed_cm_s = vspeed * 100;
     const int16_t Kl = 100; // linearity constant;
-    const float Kr = .026; // range constant;
-    int8_t vspeed_packed = (int8_t)(logf(abs(vertical_speed_cm_s)/Kl + 1)/Kr);
-    return vspeed_packed * (vertical_speed_cm_s < 0 ? -1 : 1);
+    const float Kr = .026f; // range constant;
+    int8_t vspeed_packed = int8_t(logf(fabsf(vertical_speed_cm_s)/Kl + 1)/Kr);
+    return vspeed_packed * (is_negative(vertical_speed_cm_s) ? -1 : 1);
 }
 
 // prepare vario data
 void AP_CRSF_Telem::calc_baro_vario()
 {
     _telem.bcast.baro_vario.altitude_packed = get_altitude_packed();
-    _telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed();;
+    _telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed();
 
     _telem_size = sizeof(BaroVarioFrame);
     _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BARO_VARIO;

diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
@@ -319,7 +319,7 @@ float AP_RCTelemetry::get_vspeed_ms(void)
 /*
  * prepare altitude between vehicle and home location data
  */
-float AP_RCTelemetry::get_nav_alt_m(void)
+float AP_RCTelemetry::get_nav_alt_m(Location::AltFrame frame)
 {
     Location loc;
     float current_height = 0;

diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.h b/libraries/AP_RCTelemetry/AP_RCTelemetry.h
@@ -18,6 +18,7 @@
 #include <AP_HAL/utility/RingBuffer.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Common/Location.h>
 
 #define TELEM_PAYLOAD_STATUS_CAPACITY          5 // size of the message buffer queue (max number of messages waiting to be sent)
 
@@ -78,7 +79,7 @@ class AP_RCTelemetry {
     }
 
     static float get_vspeed_ms(void);
-    static float get_nav_alt_m(void);
+    static float get_nav_alt_m(Location::AltFrame frame = Location::AltFrame::ABSOLUTE);
 
 protected:
     uint8_t run_wfq_scheduler(const bool use_shaper = true);