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

Rover: mode fixes for reversing

This commit is contained in:
Randy Mackay 2017-08-03 15:19:57 +09:00
parent 66f1437e4f
commit 4f5e82f406
7 changed files with 59 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
APMrover2/mode.cpp
View File

@ -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();
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));
}

12
APMrover2/mode.h
View File

@ -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
};

33
APMrover2/mode_auto.cpp
View File

@ -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);
}

8
APMrover2/mode_guided.cpp
View File

@ -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:
{

4
APMrover2/mode_hold.cpp
View File

@ -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) {
// hold mode never reverses
rover.set_reverse(false);
}
}

7
APMrover2/mode_rtl.cpp
View File

@ -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);

11
APMrover2/mode_steering.cpp
View File

@ -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);
}