Copter: send both SYS_STATUS or POWER_STATUS or neither

This commit is contained in:
Peter Barker 2018-12-04 16:08:09 +11:00 committed by Randy Mackay
parent a5b5688960
commit 3d8588a636
1 changed files with 4 additions and 2 deletions

View File

@ -277,9 +277,11 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
// send extended status only once vehicle has been initialised
// to avoid unnecessary errors being reported to user
if (copter.ap.initialised) {
CHECK_PAYLOAD_SIZE(SYS_STATUS);
if (PAYLOAD_SIZE(chan, SYS_STATUS) +
PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) {
return false;
}
copter.send_sys_status(chan);
CHECK_PAYLOAD_SIZE(POWER_STATUS);
send_power_status();
}
break;