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

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
This commit is contained in:
Leonard Hall 2016-06-17 20:13:44 +09:30 committed by Randy Mackay
parent 7d54c268f6
commit 3b7658c502
2 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/crash_check.cpp
View File

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

13
ArduCopter/land_detector.cpp
View File

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