Skip to content

Commit

Permalink
Fix rxrssi when rcin.rssi is 255 (invalid/unknown) (#3372)
Browse files Browse the repository at this point in the history
  • Loading branch information
EosBandi authored Jul 30, 2024
1 parent 36e6b3c commit e900cb1
Showing 1 changed file with 5 additions and 2 deletions.
7 changes: 5 additions & 2 deletions ExtLibs/ArduPilot/CurrentState.cs
Original file line number Diff line number Diff line change
Expand Up @@ -3411,8 +3411,11 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
ch15in = rcin.chan15_raw;
ch16in = rcin.chan16_raw;

//percent
rxrssi = (int)(rcin.rssi / 255.0 * 100.0);
// As per mavlink, rcin.rssi is 0-254, 0 being the worst and 254 being the best and 255 invalid/unknown
// get the percentage
rxrssi = (int)(rcin.rssi / 254.0 * 100.0);
// if invalid, set to 0
if (rcin.rssi == 255) rxrssi = 0;

//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW);
}
Expand Down

0 comments on commit e900cb1

Please sign in to comment.