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

ArduCopter/Navigation: Updated code to reference the new g.retro_loiter param instead of the compile define.

This commit is contained in:
Adam M Rivera 2012-04-23 00:19:18 -05:00
parent ec06638db6
commit 2ee8237b5f
2 changed files with 19 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/ArduCopter.pde
View File

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

22
ArduCopter/navigation.pde
View File

@ -90,14 +90,12 @@ static void calc_XY_velocity(){
last_latitude = g_gps->latitude;
}
#if RETRO_LOITER_MODE == ENABLED
static void calc_GPS_velocity()
{
float temp = radians((float)g_gps->ground_course/100.0);
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
}
#endif
static void calc_location_error(struct Location *next_loc)
{
@ -164,10 +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
if(g.retro_loiter){
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
}
int32_t p,i,d; // used to capture pid values for logging
int32_t output;
@ -184,9 +182,9 @@ static void calc_loiter(int x_error, int y_error)
#endif
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
if(g.retro_loiter){
x_rate_error = constrain(x_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
}
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);
@ -214,9 +212,9 @@ static void calc_loiter(int x_error, int y_error)
#endif
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
if(g.retro_loiter){
y_rate_error = constrain(y_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX);
}
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);