From 7a43a2178418a6767df08d862719065ff15d90df Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Sep 2018 08:47:08 +0900 Subject: [PATCH] Copter: formatting fixes to thrust loss check --- ArduCopter/crash_check.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 414b31a952..aaeea8cc7a 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -7,8 +7,8 @@ // 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_ANGLE_DEVIATION_CD 150 // 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_ANGLE_DEVIATION_CD 150 // 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 // 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 @@ -58,9 +58,7 @@ void Copter::crash_check() } } -// 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 -// called at MAIN_LOOP_RATE +// check for loss of thrust and trigger thrust boost in motors library void Copter::thrust_loss_check() { static uint16_t thrust_loss_counter; // number of iterations vehicle may have been crashed @@ -79,18 +77,18 @@ void Copter::thrust_loss_check() // check for desired angle over 15 degrees // todo: add thrust angle to AC_AttitudeControl const Vector3f angle_target = attitude_control->get_att_target_euler_cd(); - if ((fabs(angle_target.x) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD) || (fabs(angle_target.y) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD)) { + if ((fabsf(angle_target.x) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD) || (fabsf(angle_target.y) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD)) { thrust_loss_counter = 0; return; } - // check for throttle over 90 % or throttle saturation + // check for throttle over 90% or throttle saturation if ((attitude_control->get_throttle_in() < THRUST_LOSS_CHECK_MINIMUM_THROTTLE) && (!motors->limit.throttle_upper)) { thrust_loss_counter = 0; return; } - // check for throttle over 90 % or throttle saturation + // check throttle is over 25% if ((attitude_control->get_throttle_in() < 0.25f)) { thrust_loss_counter = 0; return; @@ -102,7 +100,7 @@ void Copter::thrust_loss_check() return; } - // we may be crashing + // we may have lost thrust thrust_loss_counter++; // check if thrust loss for 1 second @@ -110,8 +108,7 @@ void Copter::thrust_loss_check() // log an error in the dataflash Log_Write_Error(ERROR_SUBSYSTEM_THRUST_LOSS_CHECK, ERROR_CODE_FAILSAFE_OCCURRED); // send message to gcs - //gcs().send_text(MAV_SEVERITY_EMERGENCY,"Potential Thrust Loss"); - gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor()); + gcs_send_text_fmt(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor() + 1); // enable thrust loss handling motors->set_thrust_boost(true); }