From 78a5e4500f2e535d184ef29888ba978045ba598e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Aug 2017 20:32:58 +0900 Subject: [PATCH] Rover: send GCS warning when cruise learning fails to start Also const-ified some variables, added some brackets and comments --- APMrover2/control_modes.cpp | 4 ++-- APMrover2/cruise_learn.cpp | 26 +++++++++++++------------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index 9849dad034..8ec5ae9988 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -110,7 +110,7 @@ void Rover::reset_control_switch() // ready auxiliary switch's position aux_switch_pos Rover::read_aux_switch_pos() { - uint16_t radio_in = channel_aux->get_radio_in(); + const uint16_t radio_in = channel_aux->get_radio_in(); if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; return AUX_SWITCH_MIDDLE; @@ -126,7 +126,7 @@ void Rover::init_aux_switch() void Rover::read_aux_switch() { // do not consume input during rc or throttle failsafe - if (failsafe.bits & FAILSAFE_EVENT_THROTTLE || failsafe.bits & FAILSAFE_EVENT_RC) { + if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (failsafe.bits & FAILSAFE_EVENT_RC)) { return; } diff --git a/APMrover2/cruise_learn.cpp b/APMrover2/cruise_learn.cpp index e739b3cb12..779ea8453b 100644 --- a/APMrover2/cruise_learn.cpp +++ b/APMrover2/cruise_learn.cpp @@ -3,20 +3,19 @@ // start cruise throttle and speed learning void Rover::cruise_learn_start() { - // if disarmed do nothing - if (!arming.is_armed()) { + // if disarmed or no speed available do nothing + float speed; + if (!arming.is_armed() || !g2.attitude_control.get_forward_speed(speed)) { cruise_learn.learning = false; - gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning NOT stated"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning NOT started"); return; } - // when switch is high, start learning - float speed; - if (g2.attitude_control.get_forward_speed(speed)) { - cruise_learn.learning = true; - cruise_learn.speed_filt.reset(speed); - cruise_learn.throttle_filt.reset(g2.motors.get_throttle()); - gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started"); - } + // start learning + cruise_learn.learning = true; + cruise_learn.speed_filt.reset(speed); + cruise_learn.throttle_filt.reset(g2.motors.get_throttle()); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started"); + // To-Do: add dataflash logging of learning started event } // update cruise learning with latest speed and throttle @@ -37,8 +36,8 @@ void Rover::cruise_learn_complete() { // when switch is moved low, save learned cruise value if (cruise_learn.learning) { - float thr = cruise_learn.throttle_filt.get(); - float speed = cruise_learn.speed_filt.get(); + const float thr = cruise_learn.throttle_filt.get(); + const float speed = cruise_learn.speed_filt.get(); if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) { g.throttle_cruise.set_and_save(thr); g.speed_cruise.set_and_save(speed); @@ -47,5 +46,6 @@ void Rover::cruise_learn_complete() gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning failed"); } cruise_learn.learning = false; + // To-Do: add dataflash logging of learning completion event } }