From ac7f36494bb371a561afc0f3ab30a8fae18b98ae Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Apr 2014 09:30:39 +1000 Subject: [PATCH] Rover: use the next navigation course to adjust steering gain in turns this should make waypoints along a straight path not reduce speed --- APMrover2/APMrover2.pde | 3 +++ APMrover2/Steering.pde | 11 +++++++++-- APMrover2/commands_logic.pde | 3 +++ 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 936b90d522..b295e506b5 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -387,6 +387,9 @@ static bool have_position; static bool rtl_complete = false; +// angle of our next navigation waypoint +static int32_t next_navigation_leg_cd; + // ground speed error in m/s static float groundspeed_error; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 0f0f8441d2..0883dc27b3 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -98,11 +98,18 @@ static void calc_throttle(float target_speed) */ float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS)); steer_rate = constrain_float(steer_rate, 0.0, 1.0); - float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01; + + // use g.speed_turn_gain for a 90 degree turn, and in proportion + // for other turn angles + int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor); + float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1); + float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f; + + float reduction = 1.0 - steer_rate*speed_turn_reduction; if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) { // in auto-modes we reduce speed when approaching waypoints - float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist); + float reduction2 = 1.0 - speed_turn_reduction*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist); if (reduction2 < reduction) { reduction = reduction2; } diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 4dec0d983a..5e97f88aeb 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -26,6 +26,9 @@ start_command(const AP_Mission::Mission_Command& cmd) gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id); + // remember the course of our next navigation leg + next_navigation_leg_cd = mission.get_next_ground_course_cd(0); + switch(cmd.id){ case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint do_nav_wp(cmd);