diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 1df3b1dd93..3b17cd0007 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -4,7 +4,7 @@ // Code to detect a crash main ArduCopter code #define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash -#define CRASH_CHECK_ANGLE_DEVIATION_CD 3000.0f // 30 degrees beyond angle max is signal we are inverted +#define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond angle max is signal we are inverted #define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed // crash_check - disarms motors if a crash has been detected @@ -33,8 +33,8 @@ void Copter::crash_check() } // check for angle error over 30 degrees - const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - if (norm(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { + const float angle_error = attitude_control.get_att_error_angle_deg(); + if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) { crash_counter = 0; return; } @@ -99,8 +99,8 @@ void Copter::parachute_check() } // check for angle error over 30 degrees - const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - if (norm(angle_error.x, angle_error.y) <= CRASH_CHECK_ANGLE_DEVIATION_CD) { + const float angle_error = attitude_control.get_att_error_angle_deg(); + if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) { control_loss_count = 0; return; } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 114d6863f1..739add6d11 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -2,6 +2,11 @@ #include "Copter.h" +// Code to detect a crash main ArduCopter code +#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing +#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing +#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity + // counter to verify landings static uint32_t land_detector_count = 0; @@ -141,16 +146,16 @@ void Copter::update_throttle_thr_mix() // check for aggressive flight requests - requested roll or pitch angle below 15 degrees const Vector3f angle_target = attitude_control.get_att_target_euler_cd(); - bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f); + bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD); // check for large external disturbance - angle error over 30 degrees - const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); - bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f); + const float angle_error = attitude_control.get_att_error_angle_deg(); + bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG); // check for large acceleration - falling or high turbulence Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; - bool accel_moving = (accel_ef.length() > 3.0f); + bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING); // check for requested decent bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;