diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 5201ec673c..5da218672e 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -76,6 +76,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(dataflash_periodic, 50, 300), SCHED_TASK(button_update, 5, 100), SCHED_TASK(stats_update, 1, 100), + SCHED_TASK(crash_check, 10, 1000), }; /* diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index af62c607ae..be0961087e 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -365,6 +365,25 @@ void Rover::Log_Write_RC(void) } } +struct PACKED log_Error { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t sub_system; + uint8_t error_code; +}; + +// Write an error packet +void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) +{ + struct log_Error pkt = { + LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG), + time_us : AP_HAL::micros64(), + sub_system : sub_system, + error_code : error_code, + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + void Rover::Log_Write_Baro(void) { DataFlash.Log_Write_Baro(barometer); @@ -436,6 +455,8 @@ const LogStructure Rover::log_structure[] = { "STER", "Qff", "TimeUS,Demanded,Achieved" }, { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, + { LOG_ERROR_MSG, sizeof(log_Error), + "ERR", "QBB", "TimeUS,Subsys,ECode" }, }; void Rover::log_init(void) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 2c1a20955c..10efe8ac26 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -316,6 +316,13 @@ const AP_Param::Info Rover::var_info[] = { // @User: Standard GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE", 0), + // @Param: FS_CRASH_CHECK + // @DisplayName: Crash check action + // @Description: What to do on a crash event. When enabled the rover will go to hold if a crash is detected. + // @Values: 0:Disabled,1:HOLD,2:HoldAndDisarm + // @User: Standard + GSCALAR(fs_crash_check, "FS_CRASH_CHECK", FS_CRASH_HOLD), + // @Param: RNGFND_TRIGGR_CM // @DisplayName: Rangefinder trigger distance // @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index a7eae85195..39c1906a1a 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -136,6 +136,7 @@ public: k_param_fs_throttle_enabled, k_param_fs_throttle_value, k_param_fs_gcs_enabled, + k_param_fs_crash_check, // obstacle control k_param_sonar_enabled = 190, // deprecated, can be removed @@ -271,6 +272,7 @@ public: AP_Int8 fs_throttle_enabled; AP_Int16 fs_throttle_value; AP_Int8 fs_gcs_enabled; + AP_Int8 fs_crash_check; // obstacle control AP_Int16 sonar_trigger_cm; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 7417f16b20..b668633961 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -389,7 +389,6 @@ private: // Guided GuidedMode guided_mode; // stores which GUIDED mode the vehicle is in - private: // private member functions void ahrs_update(); @@ -439,6 +438,7 @@ private: void Log_Write_Current(); void Log_Write_Attitude(); void Log_Write_RC(void); + void Log_Write_Error(uint8_t sub_system, uint8_t error_code); void Log_Write_Baro(void); void Log_Write_Home_And_Origin(); void Log_Write_Vehicle_Startup_Messages(); @@ -547,6 +547,7 @@ private: bool in_stationary_loiter(void); void set_loiter_active(const AP_Mission::Mission_Command& cmd); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); + void crash_check(); public: bool print_log_menu(void); diff --git a/APMrover2/crash_check.cpp b/APMrover2/crash_check.cpp new file mode 100644 index 0000000000..e532829a07 --- /dev/null +++ b/APMrover2/crash_check.cpp @@ -0,0 +1,44 @@ +#include "Rover.h" + +// Code to detect a crash or block +#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds blocked indicates a crash +#define CRASH_CHECK_THROTTLE_MIN 5.0f // vehicle must have a throttle greater that 5% to be considered crashed +#define CRASH_CHECK_VEL_MIN 0.08f // vehicle must have a velocity under 0.08 m/s to be considered crashed + +// crash_check - disarms motors if a crash or block has been detected +// crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running +// called at 10Hz +void Rover::crash_check() +{ + static uint16_t crash_counter; // number of iterations vehicle may have been crashed + + // return immediately if disarmed, or crash checking disabled or in HOLD mode + if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (control_mode != GUIDED && control_mode != AUTO)) { + crash_counter = 0; + return; + } + + // TODO : Check if min vel can be calculated + // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise; + + if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN)|| ((100 * fabsf(channel_throttle->norm_output())) < CRASH_CHECK_THROTTLE_MIN)) { + crash_counter = 0; + return; + } + + // we may be crashing + crash_counter++; + + // check if crashing for 2 seconds + if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) { + // log an error in the dataflash + Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); + // send message to gcs + gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash: Going to HOLD"); + // change mode to hold and disarm + set_mode(HOLD); + if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) { + disarm_motors(); + } + } +} diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 018bd20152..bb7edfebdc 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -60,6 +60,7 @@ enum GuidedMode { #define LOG_ARM_DISARM_MSG 0x08 #define LOG_STEERING_MSG 0x0D #define LOG_GUIDEDTARGET_MSG 0x0E +#define LOG_ERROR_MSG 0x13 #define TYPE_AIRSTART_MSG 0x00 #define TYPE_GROUNDSTART_MSG 0x01 @@ -90,3 +91,15 @@ enum GuidedMode { #define MAVLINK_SET_POS_TYPE_MASK_FORCE (1<<9) #define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10) #define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11) + +// Error message sub systems and error codes +#define ERROR_SUBSYSTEM_CRASH_CHECK 12 +// subsystem specific error codes -- crash checker +#define ERROR_CODE_CRASH_CHECK_CRASH 1 + +enum fs_crash_action { + FS_CRASH_DISABLE = 0, + FS_CRASH_HOLD = 1, + FS_CRASH_HOLD_AND_DISARM = 2 +}; +