From 039b26720dc5c1fb7c49e0585c1bd1a8601974b1 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 3 May 2024 08:15:40 -0500 Subject: [PATCH] AP_MSP:add option for backward compatibility --- libraries/AP_MSP/AP_MSP.cpp | 4 +-- libraries/AP_MSP/AP_MSP.h | 1 + libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 37 ++++++++++++++++------- 3 files changed, 29 insertions(+), 13 deletions(-) diff --git a/libraries/AP_MSP/AP_MSP.cpp b/libraries/AP_MSP/AP_MSP.cpp index 9f41ed928afbbe..b6dbd5038fbb2c 100644 --- a/libraries/AP_MSP/AP_MSP.cpp +++ b/libraries/AP_MSP/AP_MSP.cpp @@ -44,8 +44,8 @@ const AP_Param::GroupInfo AP_MSP::var_info[] = { // @Param: _OPTIONS // @DisplayName: MSP OSD Options - // @Description: A bitmask to set some MSP specific options: EnableTelemetryMode-allows "push" mode telemetry when only rx line of OSD ic connected to autopilot, EnableBTFLFonts-uses indexes corresponding to Betaflight fonts if OSD uses those instead of ArduPilot fonts. - // @Bitmask: 0:EnableTelemetryMode, 1: unused, 2:EnableBTFLFonts + // @Description: A bitmask to set some MSP specific options: EnableTelemetryMode-allows "push" mode telemetry when only rx line of OSD ic connected to autopilot, EnableBTFLFonts-uses indexes corresponding to Betaflight fonts if OSD uses those instead of ArduPilot fonts. Raw MSP RC- uses directly received RC input for MSP telem as did prior versions to 4.6 reported instead of scaled/reversed + // @Bitmask: 0:EnableTelemetryMode, 1: unused, 2:EnableBTFLFonts 3: Raw MSP RC // @User: Standard AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, 0), diff --git a/libraries/AP_MSP/AP_MSP.h b/libraries/AP_MSP/AP_MSP.h index 096e5a609304b8..5b84e36c31436b 100644 --- a/libraries/AP_MSP/AP_MSP.h +++ b/libraries/AP_MSP/AP_MSP.h @@ -57,6 +57,7 @@ class AP_MSP TELEMETRY_MODE = 1U<<0, TELEMETRY_DISABLE_DJI_WORKAROUNDS = 1U<<1, DISPLAYPORT_BTFL_SYMBOLS = 1U<<2, + RAW_RC_TELEM = 1U<<3, }; bool is_option_enabled(const Option option) const; diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index 2ee0a6a43cb442..cb1ed095893985 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -1056,17 +1056,32 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst) #if AP_RC_CHANNEL_ENABLED MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) { + const AP_MSP *msp = AP::msp(); + if (msp == nullptr) { + return MSP_RESULT_ERROR; + } #if AP_RCMAPPER_ENABLED const RCMapper* rcmap = AP::rcmap(); if (rcmap == nullptr) { return MSP_RESULT_ERROR; } - - // note: rcmap channels start at 1 - float roll = rc().rc_channel(rcmap->roll()-1)->norm_input_dz(); - float pitch = -rc().rc_channel(rcmap->pitch()-1)->norm_input_dz(); - float yaw = rc().rc_channel(rcmap->yaw()-1)->norm_input_dz(); - float throttle = rc().rc_channel(rcmap->throttle()-1)->norm_input_dz(); + uint16_t values[16] = {}; + rc().get_radio_in(values, ARRAY_SIZE(values)); + uint16_t roll = values[rcmap->roll()-1]; //A + uint16_t pitch = values[rcmap->pitch()-1]; //E + uint16_t yaw = values[rcmap->yaw()-1]; //R + uint16_t thr = values[rcmap->throttle()-1]; //T + + if (msp->is_option_enabled(AP_MSP::Option::RAW_RC_TELEM)) { + roll = rc().rc_channel(rcmap->roll()-1)->norm_input_dz(); + pitch = -rc().rc_channel(rcmap->pitch()-1)->norm_input_dz(); + yaw = rc().rc_channel(rcmap->yaw()-1)->norm_input_dz(); + thr = rc().rc_channel(rcmap->throttle()-1)->norm_input_dz(); + roll = roll * 500 + 1500; + pitch = pitch * 500 + 1500; + yaw = yaw * 500 + 1500; + thr = thr * 500 + 1500; + } const struct PACKED { uint16_t a; @@ -1075,12 +1090,12 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) uint16_t t; } rc { // send only 4 channels, MSP order is AERT - a : uint16_t(roll*500+1500), // A - e : uint16_t(pitch*500+1500), // E - r : uint16_t(yaw*500+1500), // R - t : uint16_t(throttle*1000+1000) // T + a : roll, // A + e : pitch, // E + r : yaw, // R + t : thr // T }; - + sbuf_write_data(dst, &rc, sizeof(rc)); return MSP_RESULT_ACK; #else