mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AP_ExternalAHRS: Provide warning for init failure
* If the device hasn't initialized in 5 seconds, give a warning Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
parent
29fde1d6bd
commit
6caf427dd2
@ -71,7 +71,11 @@ AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_fro
|
|||||||
uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
|
uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
|
||||||
|
|
||||||
hal.scheduler->delay(5000);
|
hal.scheduler->delay(5000);
|
||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised");
|
if(initialised()) {
|
||||||
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised.");
|
||||||
|
} else {
|
||||||
|
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MicroStrain7 ExternalAHRS failed to initialise.");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void AP_ExternalAHRS_MicroStrain7::update_thread(void)
|
void AP_ExternalAHRS_MicroStrain7::update_thread(void)
|
||||||
|
Loading…
Reference in New Issue
Block a user