2015-05-29 23:12:49 -03:00
# include "Copter.h"
2013-10-29 10:11:48 -03:00
// Code to detect a crash main ArduCopter code
2015-06-17 08:37:15 -03:00
# define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
2020-01-30 01:40:55 -04:00
# define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond target angle is signal we are out of control
# define CRASH_CHECK_ANGLE_MIN_DEG 15.0f // vehicle must be leaning at least 15deg to trigger crash check
# define CRASH_CHECK_SPEED_MAX 10.0f // vehicle must be moving at less than 10m/s to trigger crash check
2015-06-17 08:37:15 -03:00
# define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
2013-10-29 10:11:48 -03:00
2018-08-12 11:19:47 -03:00
// Code to detect a thrust loss main ArduCopter code
2018-10-06 05:17:41 -03:00
# define THRUST_LOSS_CHECK_TRIGGER_SEC 1 // 1 second descent while level and high throttle indicates thrust loss
2018-09-11 02:58:17 -03:00
# define THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD 1500 // we can't expect to maintain altitude beyond 15 degrees on all aircraft
2018-09-07 20:47:08 -03:00
# define THRUST_LOSS_CHECK_MINIMUM_THROTTLE 0.9f // we can expect to maintain altitude above 90 % throttle
2018-08-12 11:19:47 -03:00
2013-10-29 10:11:48 -03:00
// crash_check - disarms motors if a crash has been detected
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
2015-06-17 08:37:15 -03:00
// called at MAIN_LOOP_RATE
2015-05-29 23:12:49 -03:00
void Copter : : crash_check ( )
2013-10-29 10:11:48 -03:00
{
2015-06-17 08:37:15 -03:00
static uint16_t crash_counter ; // number of iterations vehicle may have been crashed
2013-10-29 10:11:48 -03:00
2015-08-30 23:15:46 -03:00
// return immediately if disarmed, or crash checking disabled
2017-01-09 03:31:26 -04:00
if ( ! motors - > armed ( ) | | ap . land_complete | | g . fs_crash_check = = 0 ) {
2015-06-17 08:37:15 -03:00
crash_counter = 0 ;
2013-10-29 10:11:48 -03:00
return ;
}
2019-09-28 10:37:57 -03:00
// exit immediately if in standby
if ( standby_active ) {
crash_counter = 0 ;
return ;
}
2013-10-29 10:11:48 -03:00
// return immediately if we are not in an angle stabilize flight mode or we are flipping
2019-09-07 20:33:56 -03:00
if ( control_mode = = Mode : : Number : : ACRO | | control_mode = = Mode : : Number : : FLIP ) {
2015-06-17 08:37:15 -03:00
crash_counter = 0 ;
2013-10-29 10:11:48 -03:00
return ;
}
2019-11-28 16:21:07 -04:00
# if MODE_AUTOROTATE_ENABLED == ENABLED
//return immediately if in autorotation mode
if ( control_mode = = Mode : : Number : : AUTOROTATE ) {
crash_counter = 0 ;
return ;
}
# endif
2015-06-17 08:37:15 -03:00
// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
2018-06-22 17:21:46 -03:00
const float filtered_acc = land_accel_ef_filter . get ( ) . length ( ) ;
if ( filtered_acc > = CRASH_CHECK_ACCEL_MAX ) {
2015-06-17 08:37:15 -03:00
crash_counter = 0 ;
return ;
}
2013-10-30 01:27:06 -03:00
2020-01-30 01:40:55 -04:00
// check for lean angle over 15 degrees
const float lean_angle_deg = degrees ( acosf ( ahrs . cos_roll ( ) * ahrs . cos_pitch ( ) ) ) ;
if ( lean_angle_deg < = CRASH_CHECK_ANGLE_MIN_DEG ) {
crash_counter = 0 ;
return ;
}
2015-06-17 10:08:48 -03:00
// check for angle error over 30 degrees
2017-01-09 03:31:26 -04:00
const float angle_error = attitude_control - > get_att_error_angle_deg ( ) ;
2016-06-17 07:43:44 -03:00
if ( angle_error < = CRASH_CHECK_ANGLE_DEVIATION_DEG ) {
2015-06-17 08:37:15 -03:00
crash_counter = 0 ;
2015-06-17 10:08:48 -03:00
return ;
}
2020-01-30 01:40:55 -04:00
// check for speed under 10m/s (if available)
Vector3f vel_ned ;
if ( ahrs . get_velocity_NED ( vel_ned ) & & ( vel_ned . length ( ) > = CRASH_CHECK_SPEED_MAX ) ) {
crash_counter = 0 ;
return ;
}
2015-06-17 10:08:48 -03:00
// we may be crashing
crash_counter + + ;
// check if crashing for 2 seconds
2016-06-04 22:21:31 -03:00
if ( crash_counter > = ( CRASH_CHECK_TRIGGER_SEC * scheduler . get_loop_rate_hz ( ) ) ) {
2019-03-24 22:31:46 -03:00
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : CRASH_CHECK , LogErrorCode : : CRASH_CHECK_CRASH ) ;
2018-07-05 20:59:05 -03:00
// keep logging even if disarmed:
2019-01-18 00:24:08 -04:00
AP : : logger ( ) . set_force_log_disarmed ( true ) ;
2015-06-17 10:08:48 -03:00
// send message to gcs
2018-06-22 17:21:46 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_EMERGENCY , " Crash: Disarming: AngErr=%.0f>%.0f, Accel=%.1f<%.1f " , angle_error , CRASH_CHECK_ANGLE_DEVIATION_DEG , filtered_acc , CRASH_CHECK_ACCEL_MAX ) ;
2015-06-17 10:08:48 -03:00
// disarm motors
2020-02-21 09:09:57 -04:00
copter . arming . disarm ( AP_Arming : : Method : : CRASH ) ;
2013-10-29 10:11:48 -03:00
}
}
2014-04-02 12:20:20 -03:00
2018-09-07 20:47:08 -03:00
// check for loss of thrust and trigger thrust boost in motors library
2018-08-12 11:19:47 -03:00
void Copter : : thrust_loss_check ( )
{
static uint16_t thrust_loss_counter ; // number of iterations vehicle may have been crashed
2018-10-06 05:17:41 -03:00
// exit immediately if thrust boost is already engaged
2018-08-12 11:19:47 -03:00
if ( motors - > get_thrust_boost ( ) ) {
return ;
}
// return immediately if disarmed
if ( ! motors - > armed ( ) | | ap . land_complete ) {
thrust_loss_counter = 0 ;
return ;
}
2019-09-28 10:37:57 -03:00
// exit immediately if in standby
if ( standby_active ) {
return ;
}
2018-08-12 11:19:47 -03:00
// check for desired angle over 15 degrees
// todo: add thrust angle to AC_AttitudeControl
2018-09-11 02:58:17 -03:00
const Vector3f angle_target = attitude_control - > get_att_target_euler_cd ( ) ;
2018-10-06 05:17:41 -03:00
if ( sq ( angle_target . x ) + sq ( angle_target . y ) > sq ( THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD ) ) {
2018-08-12 11:19:47 -03:00
thrust_loss_counter = 0 ;
return ;
}
2018-09-07 20:47:08 -03:00
// check for throttle over 90% or throttle saturation
2018-08-12 11:19:47 -03:00
if ( ( attitude_control - > get_throttle_in ( ) < THRUST_LOSS_CHECK_MINIMUM_THROTTLE ) & & ( ! motors - > limit . throttle_upper ) ) {
thrust_loss_counter = 0 ;
return ;
}
2018-10-07 00:22:59 -03:00
// check throttle is over 25% to prevent checks triggering from thrust limitations caused by low commanded throttle
2018-08-12 11:19:47 -03:00
if ( ( attitude_control - > get_throttle_in ( ) < 0.25f ) ) {
thrust_loss_counter = 0 ;
return ;
}
2018-10-06 05:17:41 -03:00
// check for descent
if ( ! is_negative ( inertial_nav . get_velocity_z ( ) ) ) {
2018-08-12 11:19:47 -03:00
thrust_loss_counter = 0 ;
return ;
}
2018-10-07 00:22:59 -03:00
// check for angle error over 30 degrees to ensure the aircraft has attitude control
const float angle_error = attitude_control - > get_att_error_angle_deg ( ) ;
if ( angle_error > = CRASH_CHECK_ANGLE_DEVIATION_DEG ) {
thrust_loss_counter = 0 ;
return ;
}
// the aircraft is descending with low requested roll and pitch, at full available throttle, with attitude control
2018-09-07 20:47:08 -03:00
// we may have lost thrust
2018-08-12 11:19:47 -03:00
thrust_loss_counter + + ;
// check if thrust loss for 1 second
if ( thrust_loss_counter > = ( THRUST_LOSS_CHECK_TRIGGER_SEC * scheduler . get_loop_rate_hz ( ) ) ) {
2018-10-07 00:22:59 -03:00
// reset counter
thrust_loss_counter = 0 ;
2019-03-24 22:31:46 -03:00
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : THRUST_LOSS_CHECK , LogErrorCode : : FAILSAFE_OCCURRED ) ;
2018-08-12 11:19:47 -03:00
// send message to gcs
2018-09-11 02:58:17 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_EMERGENCY , " Potential Thrust Loss (%d) " , ( int ) motors - > get_lost_motor ( ) + 1 ) ;
2018-08-12 11:19:47 -03:00
// enable thrust loss handling
motors - > set_thrust_boost ( true ) ;
2018-10-07 00:22:59 -03:00
// the motors library disables this when it is no longer needed to achieve the commanded output
2018-08-12 11:19:47 -03:00
}
}
2014-04-02 12:20:20 -03:00
# if PARACHUTE == ENABLED
// Code to detect a crash main ArduCopter code
2015-06-17 08:37:15 -03:00
# define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute
# define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control
2014-04-02 12:20:20 -03:00
// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected
// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second
2015-06-17 08:37:15 -03:00
// called at MAIN_LOOP_RATE
2015-05-29 23:12:49 -03:00
void Copter : : parachute_check ( )
2014-04-02 12:20:20 -03:00
{
2020-04-23 01:14:18 -03:00
static uint16_t control_loss_count ; // number of iterations we have been out of control
2014-04-02 12:20:20 -03:00
static int32_t baro_alt_start ;
// exit immediately if parachute is not enabled
if ( ! parachute . enabled ( ) ) {
return ;
}
2020-09-16 03:22:17 -03:00
// pass is_flying to parachute library
parachute . set_is_flying ( ! ap . land_complete ) ;
// pass sink rate to parachute library
parachute . set_sink_rate ( - inertial_nav . get_velocity_z ( ) * 0.01f ) ;
2019-09-28 10:37:57 -03:00
// exit immediately if in standby
if ( standby_active ) {
return ;
}
2014-04-02 21:56:30 -03:00
// call update to give parachute a chance to move servo or relay back to off position
parachute . update ( ) ;
2014-04-02 12:20:20 -03:00
// return immediately if motors are not armed or pilot's throttle is above zero
2017-01-09 03:31:26 -04:00
if ( ! motors - > armed ( ) ) {
2014-04-02 12:20:20 -03:00
control_loss_count = 0 ;
return ;
}
2020-08-04 19:28:52 -03:00
if ( parachute . release_initiated ( ) ) {
copter . arming . disarm ( AP_Arming : : Method : : PARACHUTE_RELEASE ) ;
return ;
}
2014-04-02 12:20:20 -03:00
// return immediately if we are not in an angle stabilize flight mode or we are flipping
2019-09-07 20:33:56 -03:00
if ( control_mode = = Mode : : Number : : ACRO | | control_mode = = Mode : : Number : : FLIP ) {
2014-04-02 12:20:20 -03:00
control_loss_count = 0 ;
return ;
}
2014-04-03 05:55:34 -03:00
// ensure we are flying
if ( ap . land_complete ) {
2014-04-02 21:56:30 -03:00
control_loss_count = 0 ;
return ;
}
2014-04-03 05:55:34 -03:00
// ensure the first control_loss event is from above the min altitude
2015-05-12 09:49:44 -03:00
if ( control_loss_count = = 0 & & parachute . alt_min ( ) ! = 0 & & ( current_loc . alt < ( int32_t ) parachute . alt_min ( ) * 100 ) ) {
2014-04-03 05:55:34 -03:00
return ;
}
2020-09-16 03:22:17 -03:00
// trigger parachute release based on sink rate
parachute . check_sink_rate ( ) ;
2015-06-17 22:48:54 -03:00
// check for angle error over 30 degrees
2017-01-09 03:31:26 -04:00
const float angle_error = attitude_control - > get_att_error_angle_deg ( ) ;
2016-06-17 07:43:44 -03:00
if ( angle_error < = CRASH_CHECK_ANGLE_DEVIATION_DEG ) {
2016-09-05 07:36:28 -03:00
if ( control_loss_count > 0 ) {
control_loss_count - - ;
}
2015-06-17 22:48:54 -03:00
return ;
}
2014-04-02 12:20:20 -03:00
2015-06-17 22:48:54 -03:00
// increment counter
2016-06-04 22:21:31 -03:00
if ( control_loss_count < ( PARACHUTE_CHECK_TRIGGER_SEC * scheduler . get_loop_rate_hz ( ) ) ) {
2014-04-02 12:20:20 -03:00
control_loss_count + + ;
2015-06-17 22:48:54 -03:00
}
2014-04-02 12:20:20 -03:00
2015-06-17 22:48:54 -03:00
// record baro alt if we have just started losing control
if ( control_loss_count = = 1 ) {
baro_alt_start = baro_alt ;
// exit if baro altitude change indicates we are not falling
} else if ( baro_alt > = baro_alt_start ) {
control_loss_count = 0 ;
return ;
// To-Do: add check that the vehicle is actually falling
// check if loss of control for at least 1 second
2016-06-04 22:21:31 -03:00
} else if ( control_loss_count > = ( PARACHUTE_CHECK_TRIGGER_SEC * scheduler . get_loop_rate_hz ( ) ) ) {
2015-06-17 22:48:54 -03:00
// reset control loss counter
2014-04-02 12:20:20 -03:00
control_loss_count = 0 ;
2019-03-24 22:31:46 -03:00
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : CRASH_CHECK , LogErrorCode : : CRASH_CHECK_LOSS_OF_CONTROL ) ;
2015-06-17 22:48:54 -03:00
// release parachute
parachute_release ( ) ;
2014-04-02 12:20:20 -03:00
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
2015-05-29 23:12:49 -03:00
void Copter : : parachute_release ( )
2014-04-02 12:20:20 -03:00
{
// disarm motors
2020-02-21 09:09:57 -04:00
arming . disarm ( AP_Arming : : Method : : PARACHUTE_RELEASE ) ;
2014-04-02 12:20:20 -03:00
// release parachute
parachute . release ( ) ;
2016-03-03 21:47:31 -04:00
// deploy landing gear
2017-06-10 01:11:00 -03:00
landinggear . set_position ( AP_LandingGear : : LandingGear_Deploy ) ;
2014-04-02 12:20:20 -03:00
}
2014-04-02 21:56:30 -03:00
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
// checks if the vehicle is landed
2015-05-29 23:12:49 -03:00
void Copter : : parachute_manual_release ( )
2014-04-02 21:56:30 -03:00
{
2014-04-07 00:16:57 -03:00
// exit immediately if parachute is not enabled
if ( ! parachute . enabled ( ) ) {
return ;
}
2015-05-12 09:54:07 -03:00
// do not release if vehicle is landed
2014-04-02 21:56:30 -03:00
// do not release if we are landed or below the minimum altitude above home
2015-05-12 09:54:07 -03:00
if ( ap . land_complete ) {
// warn user of reason for failure
2017-07-08 21:56:49 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Parachute: Landed " ) ;
2019-03-24 22:31:46 -03:00
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : PARACHUTES , LogErrorCode : : PARACHUTE_LANDED ) ;
2015-05-12 09:54:07 -03:00
return ;
}
// do not release if we are landed or below the minimum altitude above home
if ( ( parachute . alt_min ( ) ! = 0 & & ( current_loc . alt < ( int32_t ) parachute . alt_min ( ) * 100 ) ) ) {
2014-04-02 21:56:30 -03:00
// warn user of reason for failure
2017-07-08 21:56:49 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_ALERT , " Parachute: Too low " ) ;
2019-03-24 22:31:46 -03:00
AP : : logger ( ) . Write_Error ( LogErrorSubsystem : : PARACHUTES , LogErrorCode : : PARACHUTE_TOO_LOW ) ;
2014-04-02 21:56:30 -03:00
return ;
}
// if we get this far release parachute
parachute_release ( ) ;
}
2014-04-03 05:55:34 -03:00
2014-04-06 23:18:54 -03:00
# endif // PARACHUTE == ENABLED