mirror of https://github.com/ArduPilot/ardupilot
Rover: only report system status after init has completed
This reduces errors reported to the GCS during startup
This commit is contained in:
parent
0a57177cee
commit
351b37fc31
|
@ -307,10 +307,14 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
|
|||
return true;
|
||||
|
||||
case MSG_EXTENDED_STATUS1:
|
||||
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
||||
rover.send_extended_status1(chan);
|
||||
CHECK_PAYLOAD_SIZE(POWER_STATUS);
|
||||
send_power_status();
|
||||
// send extended status only once vehicle has been initialised
|
||||
// to avoid unnecessary errors being reported to user
|
||||
if (initialised) {
|
||||
CHECK_PAYLOAD_SIZE(SYS_STATUS);
|
||||
rover.send_extended_status1(chan);
|
||||
CHECK_PAYLOAD_SIZE(POWER_STATUS);
|
||||
send_power_status();
|
||||
}
|
||||
break;
|
||||
|
||||
case MSG_EXTENDED_STATUS2:
|
||||
|
|
|
@ -207,6 +207,9 @@ private:
|
|||
AP_Mount camera_mount;
|
||||
#endif
|
||||
|
||||
// true if initialisation has completed
|
||||
bool initialised;
|
||||
|
||||
// if USB is connected
|
||||
bool usb_connected;
|
||||
|
||||
|
|
|
@ -217,6 +217,9 @@ void Rover::init_ardupilot()
|
|||
|
||||
// disable safety if requested
|
||||
BoardConfig.init_safety();
|
||||
|
||||
// flag that initialisation has completed
|
||||
initialised = true;
|
||||
}
|
||||
|
||||
//*********************************************************************************
|
||||
|
|
Loading…
Reference in New Issue