Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Arducopter: INS 

updated Nav control to use INS
This commit is contained in:
Jason Short 2012-07-18 11:17:12 -07:00
parent 98725be0ec
commit f73a2004be
1 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/navigation.pde
View File

@ -60,10 +60,12 @@ static void calc_XY_velocity(){
#if INERTIAL_NAV == ENABLED #if INERTIAL_NAV == ENABLED
// inertial_nav // inertial_nav
xy_error_correction(); xy_error_correction();
#endif current_loc.lng = xLeadFilter.get_position(g_gps->longitude, accels_velocity.x);
current_loc.lat = yLeadFilter.get_position(g_gps->latitude, accels_velocity.y);
#else
current_loc.lng = xLeadFilter.get_position(g_gps->longitude, x_actual_speed); current_loc.lng = xLeadFilter.get_position(g_gps->longitude, x_actual_speed);
current_loc.lat = yLeadFilter.get_position(g_gps->latitude, y_actual_speed); current_loc.lat = yLeadFilter.get_position(g_gps->latitude, y_actual_speed);
#endif
} }
static void calc_location_error(struct Location *next_loc) static void calc_location_error(struct Location *next_loc)
@ -195,7 +197,13 @@ static void calc_nav_rate(int16_t max_speed)
int32_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
// calculate rate error
#if INERTIAL_NAV == ENABLED
x_rate_error = x_target_speed - accels_velocity.x; // calc the speed error
#else
x_rate_error = x_target_speed - x_actual_speed; // 413 x_rate_error = x_target_speed - x_actual_speed; // 413
#endif
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);
int32_t tilt = (x_target_speed * x_target_speed * (int32_t)g.tilt_comp) / 10000; int32_t tilt = (x_target_speed * x_target_speed * (int32_t)g.tilt_comp) / 10000;
@ -206,7 +214,13 @@ static void calc_nav_rate(int16_t max_speed)
// North / South // North / South
// calculate rate error
#if INERTIAL_NAV == ENABLED
y_rate_error = y_target_speed - accels_velocity.y; // calc the speed error
#else
y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = y_target_speed - y_actual_speed; // 413
#endif
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);
tilt = (y_target_speed * y_target_speed * (int32_t)g.tilt_comp) / 10000; tilt = (y_target_speed * y_target_speed * (int32_t)g.tilt_comp) / 10000;