Browse Source

AP_RCProtocol: fix fport rssi

copter407
yaapu 4 years ago committed by Randy Mackay
parent
commit
1226808909
  1. 4
      libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp

4
libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp

@ -124,8 +124,8 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame) @@ -124,8 +124,8 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame)
bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
// we scale rssi by 2x to make it match the value displayed in OpenTX
const uint8_t scaled_rssi = MIN(frame.control.rssi*2, 255);
// fport rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
const uint8_t scaled_rssi = MIN(frame.control.rssi * 5.1f, 255);
add_input(MAX_CHANNELS, values, failsafe, scaled_rssi);
}

Loading…
Cancel
Save