AP_CRSF_Telem: adjusted flight mode frame length based on actually used bytes

This commit is contained in:
yaapu 2021-10-13 21:16:11 +02:00 committed by Peter Barker
parent 0627ee66c5
commit 1850851869
1 changed files with 4 additions and 3 deletions

View File

@ -817,9 +817,10 @@ void AP_CRSF_Telem::calc_flight_mode()
{
AP_Notify * notify = AP_Notify::get_singleton();
if (notify) {
hal.util->snprintf(_telem.bcast.flightmode.flight_mode, 16, "%s", notify->get_flight_mode_str());
_telem_size = sizeof(AP_CRSF_Telem::FlightModeFrame);
// Note: snprintf() always terminates the string
hal.util->snprintf(_telem.bcast.flightmode.flight_mode, sizeof(AP_CRSF_Telem::FlightModeFrame), "%s", notify->get_flight_mode_str());
// Note: strlen(_telem.bcast.flightmode.flight_mode) is safe because called on a guaranteed null terminated string
_telem_size = strlen(_telem.bcast.flightmode.flight_mode) + 1; //send the terminator as well
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_FLIGHT_MODE;
_telem_pending = true;
}