From 7a4ad9ff6f4ad57fe4fa9d3b0a274661bd2f3429 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 21 Apr 2018 15:45:12 +0900 Subject: [PATCH] Rover: move avoidance into calc_throttle also enable avoidance for steering and guided modes --- APMrover2/mode.cpp | 10 +++++++++- APMrover2/mode.h | 4 ++-- APMrover2/mode_acro.cpp | 5 +---- APMrover2/mode_auto.cpp | 8 ++++---- APMrover2/mode_guided.cpp | 6 +++--- APMrover2/mode_rtl.cpp | 2 +- APMrover2/mode_smart_rtl.cpp | 4 ++-- APMrover2/mode_steering.cpp | 2 +- 8 files changed, 23 insertions(+), 18 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index c9ba5f0c58..4d955eb514 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -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; diff --git a/APMrover2/mode.h b/APMrover2/mode.h index d938ecea9a..c88ddfe36d 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -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; } diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp index 290a20ec69..95e33949e5 100644 --- a/APMrover2/mode_acro.cpp +++ b/APMrover2/mode_acro.cpp @@ -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); } } diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index ab3a86ef1f..80ef2d90bc 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -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); } diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index be5c7dc3fe..2e37e81f0d 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -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); diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 671cea37c3..3aacb68d52 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -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(); diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp index c451d4f076..88e470b190 100644 --- a/APMrover2/mode_smart_rtl.cpp +++ b/APMrover2/mode_smart_rtl.cpp @@ -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(); diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index 65f8fd60c2..98a3526769 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -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); }