Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

added option in code for Loiter specific gains

Browse Source
This commit is contained in:
Jason Short 2012-02-15 09:10:07 -08:00
parent e249c8466d
commit 9a3e862b55
1 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/navigation.pde
View File

@ -117,8 +117,12 @@ static void calc_loiter(int x_error, int y_error)
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed; y_rate_error = y_target_speed - y_actual_speed;
calc_nav_lon(x_rate_error); //nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
calc_nav_lat(y_rate_error); //nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav);
nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3000, 3000);
nav_lat = constrain(nav_lat, -3000, 3000);
nav_lat = nav_lat + y_iterm; nav_lat = nav_lat + y_iterm;
nav_lon = nav_lon + x_iterm; nav_lon = nav_lon + x_iterm;
@ -176,8 +180,11 @@ static void calc_nav_rate(int max_speed)
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
int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav);
calc_nav_lon(x_rate_error); nav_lon = g.pid_nav_lon.get_pid(x_rate_error, dTnav);
calc_nav_lat(y_rate_error); nav_lat = g.pid_nav_lat.get_pid(y_rate_error, dTnav);
nav_lon = constrain(nav_lon, -3000, 3000);
nav_lat = constrain(nav_lat, -3000, 3000);
nav_lon = nav_lon + x_iterm; nav_lon = nav_lon + x_iterm;
nav_lat = nav_lat + y_iterm; nav_lat = nav_lat + y_iterm;
@ -209,19 +216,18 @@ static void calc_nav_rate(int max_speed)
} }
static void calc_nav_lon(int rate) /*static void calc_nav_lon(int rate)
{ {
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav); nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
//nav_lon = get_corrected_angle(rate, nav_lon);
nav_lon = constrain(nav_lon, -3000, 3000); nav_lon = constrain(nav_lon, -3000, 3000);
} }
static void calc_nav_lat(int rate) static void calc_nav_lat(int rate)
{ {
nav_lat = g.pid_nav_lat.get_pid(rate, dTnav); nav_lat = g.pid_nav_lat.get_pid(rate, dTnav);
//nav_lat = get_corrected_angle(rate, nav_lat);
nav_lat = constrain(nav_lat, -3000, 3000); nav_lat = constrain(nav_lat, -3000, 3000);
} }
*/
//static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out) //static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
/*{ /*{
@ -252,11 +258,11 @@ static void calc_loiter_pitch_roll()
{ {
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y); //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
// rotate the vector // rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; auto_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y; auto_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
// flip pitch because forward is negative // flip pitch because forward is negative
nav_pitch = -nav_pitch; auto_pitch = -auto_pitch;
} }
static int16_t calc_desired_speed(int16_t max_speed) static int16_t calc_desired_speed(int16_t max_speed)