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

Loiter: Made the "retro loiter" routines configurable. Add RETRO_LOITER_MODE ENABLED to APM_Config.h to enable the older loiter shtuff.

Browse Source
This commit is contained in:
Adam M Rivera 2012-04-19 10:16:29 -05:00
parent a7bc3d2cc6
commit d29f1ef331
3 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/ArduCopter.pde
View File

@ -922,9 +922,11 @@ void loop()
// check for new GPS messages
// --------------------------
//if(GPS_enabled){
// update_GPS();
//}
#if RETRO_LOITER_MODE == DISABLED
if(GPS_enabled){
update_GPS();
}
#endif
// perform 10hz tasks
// ------------------
@ -996,9 +998,11 @@ static void medium_loop()
case 0:
medium_loopCounter++;
#if RETRO_LOITER_MODE == ENABLED
if(GPS_enabled){
update_GPS();
}
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled){
@ -1036,8 +1040,11 @@ static void medium_loop()
// this calculates the velocity for Loiter
// only called when there is new data
// ----------------------------------
//calc_XY_velocity();
#if RETRO_LOITER_MODE == ENABLED
calc_GPS_velocity();
#else
calc_XY_velocity();
#endif
// If we have optFlow enabled we can grab a more accurate speed
// here and override the speed from the GPS

5
ArduCopter/config.h
View File

@ -942,4 +942,9 @@
# define QUATERNION_ENABLE DISABLED
#endif
// TEMPORARY FOR TESTING
#ifndef RETRO_LOITER_MODE
# define RETRO_LOITER_MODE DISABLED
#endif
#endif // __ARDUCOPTER_CONFIG_H__

12
ArduCopter/navigation.pde
View File

@ -162,8 +162,10 @@ static void calc_location_error(struct Location *next_loc)
#define NAV_RATE_ERR_MAX 250
static void calc_loiter(int x_error, int y_error)
{
#if RETRO_LOITER_MODE == ENABLED
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
#endif
int32_t p,i,d; // used to capture pid values for logging
int32_t output;
@ -179,7 +181,10 @@ static void calc_loiter(int x_error, int y_error)
}
#endif
x_rate_error = constrain(x_target_speed - x_actual_speed, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX); // calc the speed error
x_rate_error = x_target_speed - x_actual_speed; // calc the speed error
#if RETRO_LOITER_MODE == ENABLED
x_rate_error = constrain(x_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
#endif
p = g.pid_loiter_rate_lon.get_p(x_rate_error);
i = g.pid_loiter_rate_lon.get_i(x_rate_error, dTnav);
d = g.pid_loiter_rate_lon.get_d(x_rate_error, dTnav);
@ -206,7 +211,10 @@ static void calc_loiter(int x_error, int y_error)
}
#endif
y_rate_error = constrain(y_target_speed - y_actual_speed, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
y_rate_error = y_target_speed - y_actual_speed;
#if RETRO_LOITER_MODE == ENABLED
y_rate_error = constrain(y_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
#endif
p = g.pid_loiter_rate_lat.get_p(y_rate_error);
i = g.pid_loiter_rate_lat.get_i(y_rate_error, dTnav);
d = g.pid_loiter_rate_lat.get_d(y_rate_error, dTnav);