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

Copter: Fix feedback in crash check

This commit is contained in:
Leonard Hall 2018-10-06 17:47:41 +09:30 committed by Randy Mackay
parent 0cf2850593
commit 1a3d181756
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/crash_check.cpp
View File

@ -6,7 +6,7 @@
#define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed #define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
// Code to detect a thrust loss main ArduCopter code // Code to detect a thrust loss main ArduCopter code
#define THRUST_LOSS_CHECK_TRIGGER_SEC 1 // 1 second decent while level and high throttle indictes thrust loss #define THRUST_LOSS_CHECK_TRIGGER_SEC 1 // 1 second descent while level and high throttle indicates thrust loss
#define THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD 1500 // we can't expect to maintain altitude beyond 15 degrees on all aircraft #define THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD 1500 // we can't expect to maintain altitude beyond 15 degrees on all aircraft
#define THRUST_LOSS_CHECK_MINIMUM_THROTTLE 0.9f // we can expect to maintain altitude above 90 % throttle #define THRUST_LOSS_CHECK_MINIMUM_THROTTLE 0.9f // we can expect to maintain altitude above 90 % throttle
@ -63,7 +63,7 @@ void Copter::thrust_loss_check()
{ {
static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed
// exit immediately if motor failure handling already engaged // exit immediately if thrust boost is already engaged
if (motors->get_thrust_boost()) { if (motors->get_thrust_boost()) {
return; return;
} }
@ -77,7 +77,7 @@ void Copter::thrust_loss_check()
// check for desired angle over 15 degrees // check for desired angle over 15 degrees
// todo: add thrust angle to AC_AttitudeControl // todo: add thrust angle to AC_AttitudeControl
const Vector3f angle_target = attitude_control->get_att_target_euler_cd(); const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
if (safe_sqrt(sq(angle_target.x) + sq(angle_target.y)) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD) { if (sq(angle_target.x) + sq(angle_target.y) > sq(THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD)) {
thrust_loss_counter = 0; thrust_loss_counter = 0;
return; return;
} }
@ -94,8 +94,8 @@ void Copter::thrust_loss_check()
return; return;
} }
// check for decent // check for descent
if (inertial_nav.get_velocity_z() >= 0.0f) { if (!is_negative(inertial_nav.get_velocity_z())) {
thrust_loss_counter = 0; thrust_loss_counter = 0;
return; return;
} }