aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: modes use stop_vehicle to stop gently

Browse Source 
modes slow to a stop instead of immediately setting motors to zero
vehicle centers steering when stopping
This commit is contained in:
Randy Mackay 2017-08-10 12:06:43 +09:00
parent 0b917cfd36
commit 95c5ada3e9
6 changed files with 40 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
APMrover2/mode.cpp
View File

@ -82,6 +82,27 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed)
}
}
// performs a controlled stop with steering centered
bool Mode::stop_vehicle()
{
// call throttle controller and convert output to -100 to +100 range
bool stopped = false;
float throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.have_skid_steering(), g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise / 100.0f, stopped);
// apply limits on throttle
if (is_negative(throttle_out)) {
g2.motors.set_throttle(constrain_float(throttle_out, -g.throttle_max, -g.throttle_min));
} else {
g2.motors.set_throttle(constrain_float(throttle_out, g.throttle_min, g.throttle_max));
}
// do not attempt to steer
g2.motors.set_steering(0.0f);
// return true once stopped
return stopped;
}
// estimate maximum vehicle speed (in m/s)
float Mode::calc_speed_max(float cruise_speed, float cruise_throttle)
{

3
APMrover2/mode.h
View File

@ -98,6 +98,9 @@ protected:
// on things like proximity to corners and current speed
virtual void calc_throttle(float target_speed, bool nudge_allowed = true);
// performs a controlled stop. returns true once vehicle has stopped
bool stop_vehicle();
// estimate maximum vehicle speed (in m/s)
float calc_speed_max(float cruise_speed, float cruise_throttle);

12
APMrover2/mode_auto.cpp
View File

@ -54,8 +54,7 @@ void ModeAuto::update()
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true);
} else {
// we have reached the destination so stop
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
stop_vehicle();
lateral_acceleration = 0.0f;
}
break;
@ -72,8 +71,7 @@ void ModeAuto::update()
// check if we have reached target
_reached_heading = (fabsf(yaw_error) < radians(5));
} else {
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
stop_vehicle();
}
break;
}
@ -178,11 +176,7 @@ void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed)
{
// 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
if (g2.motors.have_skid_steering()) {
g2.motors.set_steering(0.0f);
}
stop_vehicle();
return;
}
Mode::calc_throttle(target_speed, nudge_allowed);

11
APMrover2/mode_guided.cpp
View File

@ -30,15 +30,12 @@ void ModeGuided::update()
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// continue driving towards destination
// drive towards destination
calc_lateral_acceleration(_origin, _destination);
calc_nav_steer();
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
// we've reached destination so stop
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
stop_vehicle();
}
break;
}
@ -57,7 +54,7 @@ void ModeGuided::update()
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true);
} else {
g2.motors.set_throttle(g.throttle_min.get());
stop_vehicle();
g2.motors.set_steering(0.0f);
}
break;
@ -76,7 +73,7 @@ void ModeGuided::update()
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true);
} else {
g2.motors.set_throttle(g.throttle_min.get());
stop_vehicle();
g2.motors.set_steering(0.0f);
}
break;

3
APMrover2/mode_rtl.cpp
View File

@ -35,8 +35,7 @@ void ModeRTL::update()
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
} else {
// we've reached destination so stop
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
stop_vehicle();
lateral_acceleration = 0.0f;
}
}

13
APMrover2/mode_steering.cpp
View File

@ -9,7 +9,7 @@ void ModeSteering::update()
// get speed forward
float speed;
if (!attitude_control.get_forward_speed(speed)) {
// no valid speed so so stop
// no valid speed so stop
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
@ -37,9 +37,12 @@ void ModeSteering::update()
// mark us as in_reverse when using a negative throttle
rover.set_reverse(reversed);
// run steering controller
calc_nav_steer(reversed);
// run speed to throttle output controller
calc_throttle(target_speed, false);
if (is_zero(target_speed)) {
stop_vehicle();
} else {
// run steering controller
calc_nav_steer(reversed);
calc_throttle(target_speed, false);
}
}