diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 6b8eaca3f5..7133e5639d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -2,6 +2,7 @@ #include #include +#include #define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros) @@ -213,6 +214,14 @@ void AP_Vehicle::send_watchdog_reset_statustext() ); } +bool AP_Vehicle::is_crashed() const +{ + if (AP::arming().is_armed()) { + return false; + } + return AP::arming().last_disarm_method() == AP_Arming::Method::CRASH; +} + // @LoggerMessage: FTN // @Description: Filter Tuning Messages // @Field: TimeUS: microseconds since system startup diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index afd6a2d595..53fd6c7173 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -166,6 +166,9 @@ public: return AP_HAL::millis() - _last_flying_ms; } + // returns true if the vehicle has crashed + virtual bool is_crashed() const; + /* methods to control vehicle for use by scripting */