Copter: land and crash detector use thrust angle error

The thrust angle error is the difference between our desired thrust vector
and the actual thrust vector

Also some changes to use definitions in place of constants in the checks

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;
     }

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;