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

Rover: send GCS warning when cruise learning fails to start 

Also const-ified some variables, added some brackets and comments
This commit is contained in:
Randy Mackay 2017-08-24 20:32:58 +09:00
parent 1600823b12
commit 78a5e4500f
2 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/control_modes.cpp
View File

@ -110,7 +110,7 @@ void Rover::reset_control_switch()
// ready auxiliary switch's position // ready auxiliary switch's position
aux_switch_pos Rover::read_aux_switch_pos() 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_LOW) return AUX_SWITCH_LOW;
if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH;
return AUX_SWITCH_MIDDLE; return AUX_SWITCH_MIDDLE;
@ -126,7 +126,7 @@ void Rover::init_aux_switch()
void Rover::read_aux_switch() void Rover::read_aux_switch()
{ {
// do not consume input during rc or throttle failsafe // 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; return;
} }

26
APMrover2/cruise_learn.cpp
View File

@ -3,20 +3,19 @@
// start cruise throttle and speed learning // start cruise throttle and speed learning
void Rover::cruise_learn_start() void Rover::cruise_learn_start()
{ {
// if disarmed do nothing // if disarmed or no speed available do nothing
if (!arming.is_armed()) { float speed;
if (!arming.is_armed() || !g2.attitude_control.get_forward_speed(speed)) {
cruise_learn.learning = false; 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; return;
} }
// when switch is high, start learning // start learning
float speed; cruise_learn.learning = true;
if (g2.attitude_control.get_forward_speed(speed)) { cruise_learn.speed_filt.reset(speed);
cruise_learn.learning = true; cruise_learn.throttle_filt.reset(g2.motors.get_throttle());
cruise_learn.speed_filt.reset(speed); gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started");
cruise_learn.throttle_filt.reset(g2.motors.get_throttle()); // To-Do: add dataflash logging of learning started event
gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started");
}
} }
// update cruise learning with latest speed and throttle // 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 // when switch is moved low, save learned cruise value
if (cruise_learn.learning) { if (cruise_learn.learning) {
float thr = cruise_learn.throttle_filt.get(); const float thr = cruise_learn.throttle_filt.get();
float speed = cruise_learn.speed_filt.get(); const float speed = cruise_learn.speed_filt.get();
if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) { if (thr >= 10.0f && thr <= 100.0f && is_positive(speed)) {
g.throttle_cruise.set_and_save(thr); g.throttle_cruise.set_and_save(thr);
g.speed_cruise.set_and_save(speed); 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"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning failed");
} }
cruise_learn.learning = false; cruise_learn.learning = false;
// To-Do: add dataflash logging of learning completion event
} }
} }