mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-24 01:28:29 -04:00
AP_RCProtocol: fix fport rssi
This commit is contained in:
parent
c098a089b7
commit
551e6d9f8b
@ -124,8 +124,8 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame)
|
|||||||
|
|
||||||
bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
|
bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
|
||||||
|
|
||||||
// we scale rssi by 2x to make it match the value displayed in OpenTX
|
// fport rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
|
||||||
const uint8_t scaled_rssi = MIN(frame.control.rssi*2, 255);
|
const uint8_t scaled_rssi = MIN(frame.control.rssi * 5.1f, 255);
|
||||||
|
|
||||||
add_input(MAX_CHANNELS, values, failsafe, scaled_rssi);
|
add_input(MAX_CHANNELS, values, failsafe, scaled_rssi);
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user