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

Rover: move avoidance into calc_throttle 

also enable avoidance for steering and guided modes
This commit is contained in:
Randy Mackay 2018-04-21 15:45:12 +09:00
parent 9c2da896b9
commit 29e8866ea9
8 changed files with 23 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APMrover2/mode.cpp
View File

@ -169,13 +169,21 @@ void Mode::set_desired_speed_to_default(bool rtl)
_desired_speed = get_speed_default(rtl);
}
void Mode::calc_throttle(float target_speed, bool nudge_allowed)
void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
{
// add in speed nudging
if (nudge_allowed) {
target_speed = calc_speed_nudge(target_speed, g.speed_cruise, g.throttle_cruise * 0.01f);
}
// get acceleration limited target speed
target_speed = attitude_control.get_desired_speed_accel_limited(target_speed);
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
if (avoidance_enabled) {
g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
}
// call throttle controller and convert output to -100 to +100 range
float throttle_out;

4
APMrover2/mode.h
View File

@ -116,7 +116,7 @@ protected:
// 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, bool nudge_allowed = true);
virtual void calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled);
// performs a controlled stop. returns true once vehicle has stopped
bool stop_vehicle();
@ -194,7 +194,7 @@ public:
// methods that affect movement of the vehicle in this mode
void update() override;
void calc_throttle(float target_speed, bool nudge_allowed = true);
void calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled);
// attributes of the mode
bool is_autopilot_mode() const override { return true; }

5
APMrover2/mode_acro.cpp
View File

@ -35,10 +35,7 @@ void ModeAcro::update()
// convert pilot throttle input to desired speed
float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
// apply object avoidance to desired speed using half vehicle's maximum acceleration/deceleration
rover.g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_accel_max(), ahrs.yaw, target_speed, rover.G_Dt);
calc_throttle(target_speed, false);
calc_throttle(target_speed, false, true);
}
}

8
APMrover2/mode_auto.cpp
View File

@ -56,7 +56,7 @@ void ModeAuto::update()
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
} else {
// we have reached the destination so stop
stop_vehicle();
@ -69,7 +69,7 @@ void ModeAuto::update()
if (!_reached_heading) {
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, true);
calc_throttle(_desired_speed, true, false);
// check if we have reached within 5 degrees of target
_reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
} else {
@ -197,12 +197,12 @@ bool ModeAuto::check_trigger(void)
return false;
}
void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed)
void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)
{
// If not autostarting set the throttle to minimum
if (!check_trigger()) {
stop_vehicle();
return;
}
Mode::calc_throttle(target_speed, nudge_allowed);
Mode::calc_throttle(target_speed, nudge_allowed, avoidance_enabled);
}

6
APMrover2/mode_guided.cpp
View File

@ -31,7 +31,7 @@ void ModeGuided::update()
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
} else {
stop_vehicle();
}
@ -48,7 +48,7 @@ void ModeGuided::update()
if (have_attitude_target) {
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, true);
calc_throttle(_desired_speed, true, true);
} else {
stop_vehicle();
g2.motors.set_steering(0.0f);
@ -67,7 +67,7 @@ void ModeGuided::update()
// run steering and throttle controllers
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true);
calc_throttle(_desired_speed, true, true);
} else {
stop_vehicle();
g2.motors.set_steering(0.0f);

2
APMrover2/mode_rtl.cpp
View File

@ -35,7 +35,7 @@ void ModeRTL::update()
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
} else {
// we've reached destination so stop
stop_vehicle();

4
APMrover2/mode_smart_rtl.cpp
View File

@ -68,7 +68,7 @@ void ModeSmartRTL::update()
}
// continue driving towards destination
calc_steering_to_waypoint(_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
break;
case SmartRTL_StopAtHome:
@ -77,7 +77,7 @@ void ModeSmartRTL::update()
if (rover.is_boat()) {
// boats attempt to hold position at home
calc_steering_to_waypoint(rover.current_loc, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
} else {
// rovers stop
stop_vehicle();

2
APMrover2/mode_steering.cpp
View File

@ -54,5 +54,5 @@ void ModeSteering::update()
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
// run speed to throttle controller
calc_throttle(target_speed, false);
calc_throttle(target_speed, false, true);
}