Rover: added filter to auto steering, and fixed throttle pid

This commit is contained in:
Andrew Tridgell 2013-02-08 10:21:30 +11:00
parent 5ee7abd892
commit 582d1c47b3
6 changed files with 17 additions and 22 deletions

View File

@ -91,6 +91,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <Filter.h> // Filter library
#include <Butter.h> // Filter library - butterworth filter
#include <AP_Buffer.h> // FIFO buffer library
#include <ModeFilter.h> // Mode Filter from Filter library
#include <AverageFilter.h> // 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

View File

@ -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
*****************************************/

View File

@ -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;

View File

@ -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;

View File

@ -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();
}
}

View File

@ -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)