AP_CRSF_Telem: adjusted status text frame size based on actually used bytes

This commit is contained in:
yaapu 2021-10-13 11:40:01 +02:00 committed by Peter Barker
parent 1850851869
commit b97e335940
1 changed files with 6 additions and 5 deletions

View File

@ -1279,10 +1279,11 @@ void AP_CRSF_Telem::calc_status_text()
_telem_type = get_custom_telem_frame_id();
_telem.bcast.custom_telem.status_text.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_STATUS_TEXT;
_telem.bcast.custom_telem.status_text.severity = _statustext.next.severity;
strncpy_noterm(_telem.bcast.custom_telem.status_text.text, _statustext.next.text, PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE);
// add a potentially missing terminator
_telem.bcast.custom_telem.status_text.text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE-1] = 0;
_telem_size = 2 + PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE; // sub_type(1) + severity(1) + text(50)
// Note: snprintf() always terminates the string
hal.util->snprintf(_telem.bcast.custom_telem.status_text.text, AP_CRSF_Telem::PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE, "%s", _statustext.next.text);
// frame size = sub_type(1) + severity(1) + strlen(text) + terminator
// Note: strlen(_telem.bcast.custom_telem.status_text.text) is safe because called on a guaranteed null terminated string
_telem_size = 2 + strlen(_telem.bcast.custom_telem.status_text.text) + 1;
_telem_pending = true;
_statustext.available = false;
}