Rover: Fixed a bug in reverse

If a user was driving in reverse and when into an auto mission the
rover would try to do the whole mission in reverse.  This fixes that.
This commit is contained in:
Grant Morphett 2017-03-15 14:00:14 +11:00
parent ebedc8b01e
commit 9fa6ed9af4
4 changed files with 20 additions and 0 deletions

View File

@ -102,6 +102,8 @@ void Rover::setup()
AP_Notify::flags.failsafe_battery = false;
in_auto_reverse = false;
rssi.init();
init_ardupilot();
@ -430,12 +432,18 @@ void Rover::update_current_mode(void)
switch (control_mode){
case AUTO:
case RTL:
if (!in_auto_reverse) {
set_reverse(false);
}
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(g.speed_cruise);
break;
case GUIDED: {
if (!in_auto_reverse) {
set_reverse(false);
}
switch (guided_mode){
case Guided_Angle:
nav_set_yaw_speed();
@ -502,6 +510,7 @@ void Rover::update_current_mode(void)
we set the exact value in set_servos(), but it helps for
logging
*/
in_auto_reverse = false;
channel_throttle->set_servo_out(channel_throttle->get_control_in());
channel_steer->set_servo_out(channel_steer->pwm_to_angle());
@ -514,6 +523,9 @@ void Rover::update_current_mode(void)
// hold position - stop motors and center steering
channel_throttle->set_servo_out(0);
channel_steer->set_servo_out(0);
if (!in_auto_reverse) {
set_reverse(false);
}
break;
case INITIALISING:

View File

@ -351,6 +351,9 @@ private:
// set if we are driving backwards
bool in_reverse;
// set if the users asks for auto reverse
bool in_auto_reverse;
static const AP_Scheduler::Task scheduler_tasks[];
// use this to prevent recursion during sensor init

View File

@ -513,8 +513,10 @@ void Rover::log_picture()
void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1) {
in_auto_reverse = true;
set_reverse(true);
} else {
in_auto_reverse = false;
set_reverse(false);
}
}

View File

@ -277,6 +277,9 @@ void Rover::set_mode(enum mode mode)
control_mode = mode;
throttle_last = 0;
throttle = 500;
if (!in_auto_reverse) {
set_reverse(false);
}
g.pidSpeedThrottle.reset_I();
#if FRSKY_TELEM_ENABLED == ENABLED