Rover: only report system status after init has completed

This reduces errors reported to the GCS during startup
This commit is contained in:
Randy Mackay 2017-06-28 11:32:01 +09:00
parent 0a57177cee
commit 351b37fc31
3 changed files with 14 additions and 4 deletions

View File

@ -307,10 +307,14 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
return true; return true;
case MSG_EXTENDED_STATUS1: case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS); // send extended status only once vehicle has been initialised
rover.send_extended_status1(chan); // to avoid unnecessary errors being reported to user
CHECK_PAYLOAD_SIZE(POWER_STATUS); if (initialised) {
send_power_status(); CHECK_PAYLOAD_SIZE(SYS_STATUS);
rover.send_extended_status1(chan);
CHECK_PAYLOAD_SIZE(POWER_STATUS);
send_power_status();
}
break; break;
case MSG_EXTENDED_STATUS2: case MSG_EXTENDED_STATUS2:

View File

@ -207,6 +207,9 @@ private:
AP_Mount camera_mount; AP_Mount camera_mount;
#endif #endif
// true if initialisation has completed
bool initialised;
// if USB is connected // if USB is connected
bool usb_connected; bool usb_connected;

View File

@ -217,6 +217,9 @@ void Rover::init_ardupilot()
// disable safety if requested // disable safety if requested
BoardConfig.init_safety(); BoardConfig.init_safety();
// flag that initialisation has completed
initialised = true;
} }
//********************************************************************************* //*********************************************************************************