Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

Airspeed patch:

Browse Source 
pre-calculated airspeed resistance pitches copter automatically to gain a certain speed allowing the speed controller to work off of a better set point - similar to Alt hold.
added param tilt_comp with a default of 54 which equals 19.5° of pitch to go 6m/s
upped Z and Y target speeds to int32_t for speed squared calculation
This commit is contained in:
Jason Short 2012-07-09 13:10:12 -07:00
parent de64c05f8e
commit 038116f521
3 changed files with 13 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Parameters.h
View File

@ -217,7 +217,7 @@ public:
AP_Int8 super_simple; AP_Int8 super_simple;
AP_Int8 rtl_land_enabled; AP_Int8 rtl_land_enabled;
AP_Int16 rtl_approach_alt; AP_Int16 rtl_approach_alt;
AP_Float tilt_comp; AP_Int8 tilt_comp;
AP_Int8 axis_enabled; AP_Int8 axis_enabled;
AP_Int8 copter_leds_mode; // Operating mode of LED lighting system AP_Int8 copter_leds_mode; // Operating mode of LED lighting system
@ -341,7 +341,7 @@ public:
super_simple (SUPER_SIMPLE), super_simple (SUPER_SIMPLE),
rtl_land_enabled (0), rtl_land_enabled (0),
rtl_approach_alt (RTL_APPROACH_ALT), rtl_approach_alt (RTL_APPROACH_ALT),
tilt_comp (.0054), tilt_comp (54),
axis_enabled (AXIS_LOCK_ENABLED), axis_enabled (AXIS_LOCK_ENABLED),
copter_leds_mode (9), copter_leds_mode (9),

4
ArduCopter/config.h
View File

@ -734,13 +734,13 @@
// WP Navigation control gains // WP Navigation control gains
// //
#ifndef NAV_P #ifndef NAV_P
# define NAV_P 2.4 // # define NAV_P 2.2 //
#endif #endif
#ifndef NAV_I #ifndef NAV_I
# define NAV_I 0.17 // Wind control # define NAV_I 0.17 // Wind control
#endif #endif
#ifndef NAV_D #ifndef NAV_D
# define NAV_D 0.00 // # define NAV_D 0.00 // .95
#endif #endif
#ifndef NAV_IMAX #ifndef NAV_IMAX
# define NAV_IMAX 18 // degrees # define NAV_IMAX 18 // degrees

22
ArduCopter/navigation.pde
View File

@ -173,7 +173,7 @@ static void calc_loiter(int x_error, int y_error)
g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator()); g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator());
} }
static void calc_nav_rate(int max_speed) static void calc_nav_rate(int16_t max_speed)
{ {
float temp, temp_x, temp_y; float temp, temp_x, temp_y;
@ -191,19 +191,17 @@ static void calc_nav_rate(int max_speed)
temp_y = sin(temp); temp_y = sin(temp);
// rotate desired spped vector: // rotate desired spped vector:
int16_t x_target_speed = max_speed * temp_x - cross_speed * temp_y; int32_t x_target_speed = max_speed * temp_x - cross_speed * temp_y;
int16_t y_target_speed = cross_speed * temp_x + max_speed * temp_y; int32_t y_target_speed = cross_speed * temp_x + max_speed * temp_y;
// East / West // East / West
x_rate_error = x_target_speed - x_actual_speed; // 413 x_rate_error = x_target_speed - x_actual_speed; // 413
x_rate_error = constrain(x_rate_error, -1000, 1000); x_rate_error = constrain(x_rate_error, -1000, 1000);
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav); nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
temp = x_target_speed * x_target_speed; int32_t tilt = (x_target_speed * x_target_speed * (int32_t)g.tilt_comp) / 10000;
temp *= g.tilt_comp;
if(x_target_speed < 0) if(x_target_speed < 0) tilt = -tilt;
temp = -temp; nav_lon += tilt;
nav_lon += temp;
nav_lon = constrain(nav_lon, -3000, 3000); nav_lon = constrain(nav_lon, -3000, 3000);
@ -211,12 +209,10 @@ static void calc_nav_rate(int max_speed)
y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = y_target_speed - y_actual_speed; // 413
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav); nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
temp = y_target_speed * y_target_speed; tilt = (y_target_speed * y_target_speed * (int32_t)g.tilt_comp) / 10000;
temp *= g.tilt_comp;
if(y_target_speed < 0) if(y_target_speed < 0) tilt = -tilt;
temp = -temp; nav_lat += tilt;
nav_lat += temp;
nav_lat = constrain(nav_lat, -3000, 3000); nav_lat = constrain(nav_lat, -3000, 3000);
// copy over I term to Loiter_Rate // copy over I term to Loiter_Rate