|
|
|
@ -327,9 +327,21 @@ void AP_BoardConfig::sensor_config_error(const char *reason)
@@ -327,9 +327,21 @@ void AP_BoardConfig::sensor_config_error(const char *reason)
|
|
|
|
|
before this, so the user can change parameters (and in |
|
|
|
|
particular BRD_TYPE if needed) |
|
|
|
|
*/ |
|
|
|
|
uint32_t last_print_ms = 0; |
|
|
|
|
bool have_gcs = GCS::instance() != nullptr; |
|
|
|
|
while (true) { |
|
|
|
|
printf("Sensor failure: %s\n", reason); |
|
|
|
|
gcs().send_text(MAV_SEVERITY_ERROR, "Check BRD_TYPE: %s", reason); |
|
|
|
|
hal.scheduler->delay(3000); |
|
|
|
|
uint32_t now = AP_HAL::millis(); |
|
|
|
|
if (now - last_print_ms >= 3000) { |
|
|
|
|
last_print_ms = now; |
|
|
|
|
printf("Sensor failure: %s\n", reason); |
|
|
|
|
if (have_gcs) { |
|
|
|
|
gcs().send_text(MAV_SEVERITY_ERROR, "Check BRD_TYPE: %s", reason); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if (have_gcs) { |
|
|
|
|
gcs().update_receive(); |
|
|
|
|
gcs().update_send(); |
|
|
|
|
} |
|
|
|
|
hal.scheduler->delay(5); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|