Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpenDroneID: set EMERGENCY status on crash or chute deploy 

RemoteID modules are required to set EMERGENCY status on uncontrolled
descent or crash. This fixes our implementation to do that, either via
existing vehicle crash checking code or via a parachute release
This commit is contained in:
Andrew Tridgell 2023-01-10 16:44:33 +11:00
parent d9ee361b3b
commit 4485ba0dfa
2 changed files with 30 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_OpenDroneID/AP_OpenDroneID.cpp
View File

@ -40,6 +40,8 @@
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_Vehicle/AP_Vehicle.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
@ -65,8 +67,8 @@ const AP_Param::GroupInfo AP_OpenDroneID::var_info[] = {
// @Param: OPTIONS // @Param: OPTIONS
// @DisplayName: OpenDroneID options // @DisplayName: OpenDroneID options
// @Description: Options for OpenDroneID subsystem. Bit 0 means to enforce arming checks // @Description: Options for OpenDroneID subsystem
// @Bitmask: 0:EnforceArming // @Bitmask: 0:EnforceArming, 1:AllowNonGPSPosition
AP_GROUPINFO("OPTIONS", 4, AP_OpenDroneID, _options, 0), AP_GROUPINFO("OPTIONS", 4, AP_OpenDroneID, _options, 0),
// @Param: BARO_ACC // @Param: BARO_ACC
@ -254,12 +256,36 @@ void AP_OpenDroneID::send_location_message()
const AP_GPS::GPS_Status gps_status = gps.status(); const AP_GPS::GPS_Status gps_status = gps.status();
const bool got_bad_gps_fix = (gps_status < AP_GPS::GPS_Status::GPS_OK_FIX_3D); const bool got_bad_gps_fix = (gps_status < AP_GPS::GPS_Status::GPS_OK_FIX_3D);
const bool armed = hal.util->get_soft_armed();
Location current_location; Location current_location;
if (!ahrs.get_position(current_location)) { if (!ahrs.get_position(current_location)) {
return; return;
} }
const uint8_t uav_status = hal.util->get_soft_armed()? MAV_ODID_STATUS_AIRBORNE : MAV_ODID_STATUS_GROUND; uint8_t uav_status = hal.util->get_soft_armed()? MAV_ODID_STATUS_AIRBORNE : MAV_ODID_STATUS_GROUND;
// set emergency status if chute is released
const auto *parachute = AP::parachute();
if (parachute != nullptr && parachute->released()) {
uav_status = MAV_ODID_STATUS_EMERGENCY;
}
// if we are armed with no GPS fix and we haven't specifically
// allowed for non-GPS operation then declare an emergency
if (got_bad_gps_fix && armed && !option_enabled(Options::AllowNonGPSPosition)) {
uav_status = MAV_ODID_STATUS_EMERGENCY;
}
// if we are disarmed and falling at over 3m/s then declare an
// emergency. This covers cases such as deliberate crash with
// advanced failsafe and an unintended reboot or in-flight disarm
if (!got_bad_gps_fix && !armed && gps.velocity().z > 3.0) {
uav_status = MAV_ODID_STATUS_EMERGENCY;
}
// if we have watchdogged while armed then declare an emergency
if (hal.util->was_watchdog_armed()) {
uav_status = MAV_ODID_STATUS_EMERGENCY;
}
float direction = ODID_INV_DIR; float direction = ODID_INV_DIR;
if (!got_bad_gps_fix) { if (!got_bad_gps_fix) {

1
libraries/AP_OpenDroneID/AP_OpenDroneID.h
View File

@ -115,6 +115,7 @@ private:
enum Options : int16_t { enum Options : int16_t {
EnforceArming = (1U << 0U), EnforceArming = (1U << 0U),
AllowNonGPSPosition = (1U << 1U),
}; };
// check if an option is set // check if an option is set