From 4f5e82f40622269117704f87948b07cc1b36f043 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 3 Aug 2017 15:19:57 +0900 Subject: [PATCH] Rover: mode fixes for reversing --- APMrover2/mode.cpp | 29 +++++++++++++++-------------- APMrover2/mode.h | 12 ++++++++---- APMrover2/mode_auto.cpp | 33 +++++++++++++++++++-------------- APMrover2/mode_guided.cpp | 8 ++++---- APMrover2/mode_hold.cpp | 6 +++--- APMrover2/mode_rtl.cpp | 7 +++---- APMrover2/mode_steering.cpp | 11 +++++++---- 7 files changed, 59 insertions(+), 47 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 4981318684..5495b3f066 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -17,9 +17,6 @@ void Mode::exit() lateral_acceleration = 0.0f; rover.g.pidSpeedThrottle.reset_I(); - if (!rover.in_auto_reverse) { - rover.set_reverse(false); - } } bool Mode::enter() @@ -52,7 +49,7 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) _desired_speed = target_speed; } -void Mode::calc_throttle(float target_speed) +void Mode::calc_throttle(float target_speed, bool reversed) { // get ground speed from vehicle const float &groundspeed = rover.ground_speed; @@ -65,13 +62,13 @@ void Mode::calc_throttle(float target_speed) float throttle = throttle_target + (g.pidSpeedThrottle.get_pid(_speed_error * 100.0f) / 100.0f); - if (rover.in_reverse) { + if (reversed) { g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min)); } else { g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max)); } - if (!rover.in_reverse && g.braking_percent != 0 && _speed_error < -g.braking_speederr) { + if (!reversed && g.braking_percent != 0 && _speed_error < -g.braking_speederr) { // the user has asked to use reverse throttle to brake. Apply // it in proportion to the ground speed error, but only when // our ground speed error is more than BRAKING_SPEEDERR. @@ -82,10 +79,6 @@ void Mode::calc_throttle(float target_speed) const float brake_gain = constrain_float(((-_speed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f); const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain; g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min)); - - // temporarily set us in reverse to allow the PWM setting to - // go negative - rover.set_reverse(true); } } @@ -148,14 +141,19 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed) // calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination // this function update lateral_acceleration and _yaw_error_cd members -void Mode::calc_lateral_acceleration(const struct Location &origin, const struct Location &destination) +void Mode::calc_lateral_acceleration(const struct Location &origin, const struct Location &destination, bool reversed) { // Calculate the required turn of the wheels // negative error = left turn // positive error = right turn + rover.nav_controller->set_reverse(reversed); rover.nav_controller->update_waypoint(origin, destination); lateral_acceleration = rover.nav_controller->lateral_acceleration(); - _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor); + if (reversed) { + _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor + 18000); + } else { + _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor); + } if (rover.use_pivot_steering(_yaw_error_cd)) { if (is_positive(_yaw_error_cd)) { lateral_acceleration = g.turn_max_g * GRAVITY_MSS; @@ -169,16 +167,19 @@ void Mode::calc_lateral_acceleration(const struct Location &origin, const struct /* calculate steering angle given lateral_acceleration */ -void Mode::calc_nav_steer() +void Mode::calc_nav_steer(bool reversed) { // add obstacle avoidance response to lateral acceleration target - if (!rover.in_reverse) { + if (!reversed) { lateral_acceleration += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g; } // constrain to max G force lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS); + // set controller reversal + rover.steerController.set_reverse(reversed); + // send final steering command to motor library g2.motors.set_steering(rover.steerController.get_steering_out_lat_accel(lateral_acceleration)); } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index a967478627..ca3aa76850 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -83,14 +83,14 @@ protected: virtual void _exit() { return; } // calculate steering angle given a desired lateral acceleration - virtual void calc_nav_steer(); + void calc_nav_steer(bool reversed = false); // calculate desired lateral acceleration - void calc_lateral_acceleration(const struct Location &origin, const struct Location &destination); + void calc_lateral_acceleration(const struct Location &origin, const struct Location &destination, bool reversed = false); // calculates the amount of throttle that should be output based // on things like proximity to corners and current speed - virtual void calc_throttle(float target_speed); + virtual void calc_throttle(float target_speed, bool reversed = false); // calculate pilot input to nudge throttle up or down int16_t calc_throttle_nudge(); @@ -128,7 +128,7 @@ public: // methods that affect movement of the vehicle in this mode void update() override; - void calc_throttle(float target_speed) override; + void calc_throttle(float target_speed, bool reversed = false); // attributes of the mode bool is_autopilot_mode() const override { return true; } @@ -146,6 +146,9 @@ public: void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override; bool reached_heading(); + // execute the mission in reverse (i.e. backing up) + void set_reversed(bool value); + protected: bool _enter() override; @@ -165,6 +168,7 @@ private: bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode bool _stay_active_at_dest; // true when we should actively maintain position even after reaching the destination + bool _reversed; // execute the mission by backing up }; diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index f9dfcf7977..2542560d54 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -17,6 +17,9 @@ bool ModeAuto::_enter() auto_triggered = false; g2.motors.slew_limit_throttle(true); + // initialise reversed to be false + set_reversed(false); + // restart mission processing mission.start_or_resume(); return true; @@ -24,9 +27,7 @@ bool ModeAuto::_enter() void ModeAuto::_exit() { - // If we are changing out of AUTO mode reset the loiter timer - rover.loiter_start_time = 0; - // ... and stop running the mission + // stop running the mission if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } @@ -34,10 +35,6 @@ void ModeAuto::_exit() void ModeAuto::update() { - if (!rover.in_auto_reverse) { - rover.set_reverse(false); - } - switch (_submode) { case Auto_WP: { @@ -53,9 +50,9 @@ void ModeAuto::update() bool active_at_destination = _reached_destination && _stay_active_at_dest && (_distance_to_destination > rover.g.waypoint_radius); if (!_reached_destination || active_at_destination) { // continue driving towards destination - calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination); - calc_nav_steer(); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed)); + calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination, _reversed); + calc_nav_steer(_reversed); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), _reversed); } else { // we have reached the destination so stop g2.motors.set_throttle(g.throttle_min.get()); @@ -123,6 +120,15 @@ bool ModeAuto::reached_heading() return true; } +// execute the mission in reverse (i.e. backing up) +void ModeAuto::set_reversed(bool value) +{ + if (_reversed != value) { + _reversed = value; + rover.set_reverse(_reversed); + } +} + /* check for triggering of start of auto mode */ @@ -168,10 +174,9 @@ bool ModeAuto::check_trigger(void) return false; } -void ModeAuto::calc_throttle(float target_speed) +void ModeAuto::calc_throttle(float target_speed, bool reversed) { - // If not autostarting OR we are loitering at a waypoint - // then set the throttle to minimum + // If not autostarting set the throttle to minimum if (!check_trigger()) { g2.motors.set_throttle(g.throttle_min.get()); // Stop rotation in case of loitering and skid steering @@ -180,5 +185,5 @@ void ModeAuto::calc_throttle(float target_speed) } return; } - Mode::calc_throttle(target_speed); + Mode::calc_throttle(target_speed, reversed); } diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index fd210e280c..a9cb495bc7 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -9,16 +9,16 @@ bool ModeGuided::_enter() */ lateral_acceleration = 0.0f; set_desired_location(rover.current_loc); + + // guided mode never travels in reverse + rover.set_reverse(false); + g2.motors.slew_limit_throttle(true); return true; } void ModeGuided::update() { - if (!rover.in_auto_reverse) { - rover.set_reverse(false); - } - switch (_guided_mode) { case Guided_WP: { diff --git a/APMrover2/mode_hold.cpp b/APMrover2/mode_hold.cpp index f208b6acf1..1f2ade6dfa 100644 --- a/APMrover2/mode_hold.cpp +++ b/APMrover2/mode_hold.cpp @@ -6,7 +6,7 @@ void ModeHold::update() // hold position - stop motors and center steering g2.motors.set_throttle(0.0f); g2.motors.set_steering(0.0f); - if (!rover.in_auto_reverse) { - rover.set_reverse(false); - } + + // hold mode never reverses + rover.set_reverse(false); } diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index c934c2ed26..f32a606f90 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -11,16 +11,15 @@ bool ModeRTL::_enter() // set destination set_desired_location(rover.home); + // RTL never reverses + rover.set_reverse(false); + g2.motors.slew_limit_throttle(true); return true; } void ModeRTL::update() { - if (!rover.in_auto_reverse) { - rover.set_reverse(false); - } - if (!_reached_destination) { // calculate distance to home _distance_to_destination = get_distance(rover.current_loc, _destination); diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index bbf57c2db6..ba559c1c10 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -21,16 +21,19 @@ void ModeSteering::update() lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f); // reverse target lateral acceleration if backing up + bool reversed = false; if (is_negative(target_speed)) { + reversed = true; lateral_acceleration = -lateral_acceleration; + target_speed = fabsf(target_speed); } - // mark us as in_reverse when using a negative throttle to stop AHRS getting off - rover.set_reverse(is_negative(target_speed)); + // mark us as in_reverse when using a negative throttle + rover.set_reverse(reversed); // run steering controller - calc_nav_steer(); + calc_nav_steer(reversed); // run speed to throttle output controller - calc_throttle(target_speed); + calc_throttle(target_speed, reversed); }