diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 77d183d30f..506a48f11d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -13,6 +13,7 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include +#include #endif #define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio) @@ -363,6 +364,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #endif #if HAL_EFI_ENABLED SCHED_TASK_CLASS(AP_EFI, &vehicle.efi, update, 50, 200, 250), +#endif +#if HAL_INS_ACCELCAL_ENABLED + SCHED_TASK(one_Hz_update, 1, 100, 252), #endif SCHED_TASK(update_arming, 1, 50, 253), }; @@ -679,6 +683,36 @@ void AP_Vehicle::update_arming() AP::arming().update(); } +/* + one Hz checks common to all vehicles + */ +void AP_Vehicle::one_Hz_update(void) +{ + one_Hz_counter++; + + /* + every 10s check if using a 2M firmware on a 1M board + */ + if (one_Hz_counter % 10U == 0) { +#if defined(BOARD_CHECK_F427_USE_1M) && (BOARD_FLASH_SIZE>1024) + if (!hal.util->get_soft_armed() && check_limit_flash_1M()) { + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, BOARD_CHECK_F427_USE_1M); + } +#endif + } + + /* + every 30s check if using a 1M firmware on a 2M board + */ + if (one_Hz_counter % 30U == 0) { +#if defined(BOARD_CHECK_F427_USE_1M) && (BOARD_FLASH_SIZE<=1024) + if (!hal.util->get_soft_armed() && !check_limit_flash_1M()) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, BOARD_CHECK_F427_USE_2M); + } +#endif + } +} + AP_Vehicle *AP_Vehicle::_singleton = nullptr; AP_Vehicle *AP_Vehicle::get_singleton() diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 87e55daf22..a2245a79d8 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -445,6 +445,10 @@ private: // run notch update at either loop rate or 200Hz void update_dynamic_notch_at_specified_rate(); + // decimation for 1Hz update + uint8_t one_Hz_counter; + void one_Hz_update(); + bool likely_flying; // true if vehicle is probably flying uint32_t _last_flying_ms; // time when likely_flying last went true uint32_t _last_notch_update_ms[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; // last time update_dynamic_notch() was run @@ -453,6 +457,7 @@ private: bool done_safety_init; + uint32_t _last_internal_errors; // backup of AP_InternalError::internal_errors bitmask AP_CustomRotations custom_rotations;