From 473340029e73befa8460bd449b854b99264e298c Mon Sep 17 00:00:00 2001 From: Frank0587 <92010529+Frank0587@users.noreply.github.com> Date: Tue, 29 Oct 2024 07:22:54 +0100 Subject: [PATCH] AP_RCTelemetry: Fix Baro and Vario values Add the missing byte swapping for 16bit values --- libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index fb7564e00085e..77002b39cd610 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -975,7 +975,7 @@ int8_t AP_CRSF_Telem::get_vertical_speed_packed() // prepare vario data void AP_CRSF_Telem::calc_baro_vario() { - _telem.bcast.baro_vario.altitude_packed = get_altitude_packed(); + _telem.bcast.baro_vario.altitude_packed = htobe16(get_altitude_packed()); _telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed(); _telem_size = sizeof(BaroVarioFrame); @@ -987,7 +987,7 @@ void AP_CRSF_Telem::calc_baro_vario() // prepare vario data void AP_CRSF_Telem::calc_vario() { - _telem.bcast.vario.v_speed = int16_t(get_vspeed_ms() * 100.0f); + _telem.bcast.vario.v_speed = htobe16(int16_t(get_vspeed_ms() * 100.0f)); _telem_size = sizeof(VarioFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VARIO;