From 582d1c47b3a90059507d0a157a639ee42fbb058d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Feb 2013 10:21:30 +1100 Subject: [PATCH] Rover: added filter to auto steering, and fixed throttle pid --- APMrover2/APMrover2.pde | 5 +++-- APMrover2/Steering.pde | 13 ++----------- APMrover2/commands.pde | 3 ++- APMrover2/commands_logic.pde | 6 +++--- APMrover2/control_modes.pde | 6 +++--- APMrover2/navigation.pde | 6 ++++-- 6 files changed, 17 insertions(+), 22 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 951b461913..3b6bfb2d6e 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -91,6 +91,7 @@ version 2.1 of the License, or (at your option) any later version. #include // RC Channel Library #include // Range finder library #include // Filter library +#include // Filter library - butterworth filter #include // FIFO buffer library #include // Mode Filter from Filter library #include // Mode Filter from Filter library @@ -416,8 +417,8 @@ static int16_t throttle_last = 0, throttle = 500; //////////////////////////////////////////////////////////////////////////////// // Location Errors //////////////////////////////////////////////////////////////////////////////// -// Difference between current bearing and desired bearing. Hundredths of a degree -static int32_t bearing_error; +// Difference between current bearing and desired bearing. in centi-degrees +static int32_t bearing_error_cd; // Difference between current altitude and desired altitude. Centimeters static int32_t altitude_error; // Distance perpandicular to the course line that we are off trackline. Meters diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 15f7908ef5..28b7f6d0b1 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -23,7 +23,7 @@ static void calc_throttle() groundspeed_error = g.speed_cruise - ground_speed; - throttle = throttle_target + g.pidSpeedThrottle.get_pid(groundspeed_error); + throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100); g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get()); } @@ -42,22 +42,13 @@ static void calc_nav_steer() // negative error = left turn // positive error = right turn - - nav_steer = g.pidNavSteer.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100 + nav_steer = g.pidNavSteer.get_pid(bearing_error_cd, nav_gain_scaler); if (obstacle) { // obstacle avoidance nav_steer += 9000; // if obstacle in front turn 90° right } } -// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. -// Keeps outdated data out of our calculations -static void reset_I(void) -{ - g.pidNavSteer.reset_I(); - g.pidSpeedThrottle.reset_I(); -} - /***************************************** * Set the flight control servos based on the current calculated values *****************************************/ diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index d08d66dffb..bfc8cf1b6a 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -185,7 +185,8 @@ void init_home() static void restart_nav() { - reset_I(); + g.pidNavSteer.reset_I(); + g.pidSpeedThrottle.reset_I(); prev_WP = current_loc; nav_command_ID = NO_COMMAND; nav_command_index = 0; diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 9dd45b9bd7..9e989fd3f2 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -8,11 +8,11 @@ handle_process_nav_cmd() { // reset navigation integrators // ------------------------- - reset_I(); + g.pidNavSteer.reset_I(); + + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); - gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); switch(next_nav_command.id){ - case MAV_CMD_NAV_TAKEOFF: do_takeoff(); break; diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index 55b843b09f..0934c23530 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -24,10 +24,10 @@ static void read_control_switch() oldSwitchPosition = switchPosition; prev_WP = current_loc; - // reset navigation integrators + // reset navigation and speed integrators // ------------------------- - reset_I(); - + g.pidNavSteer.reset_I(); + g.pidSpeedThrottle.reset_I(); } } diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index a77d255d65..2c8b6f793a 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -40,8 +40,10 @@ static void navigate() static void calc_bearing_error() { - bearing_error = nav_bearing - ahrs.yaw_sensor; - bearing_error = wrap_180(bearing_error); + static butter10hz1_6 butter; + + bearing_error_cd = wrap_180(nav_bearing - ahrs.yaw_sensor); + bearing_error_cd = butter.filter(bearing_error_cd); } static long wrap_360(long error)