mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
AP_ExternalAHRS: Don't send init message till booted up
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
parent
ecd11d1903
commit
029d22c2a9
@ -93,11 +93,12 @@ void AP_ExternalAHRS_MicroStrain7::update_thread(void)
|
|||||||
void AP_ExternalAHRS_MicroStrain7::check_initialise_state(void)
|
void AP_ExternalAHRS_MicroStrain7::check_initialise_state(void)
|
||||||
{
|
{
|
||||||
const auto new_init_state = initialised();
|
const auto new_init_state = initialised();
|
||||||
if (!last_init_state && new_init_state) {
|
// Only send the message after fully booted up, otherwise it gets dropped.
|
||||||
|
if (!last_init_state && new_init_state && AP_HAL::millis() > 5000) {
|
||||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised.");
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised.");
|
||||||
}
|
|
||||||
last_init_state = new_init_state;
|
last_init_state = new_init_state;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
|
// Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet.
|
||||||
|
Loading…
Reference in New Issue
Block a user