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

14
ArduCopter/ArduCopter.pde
View File

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

14
ArduCopter/navigation.pde
View File

@ -90,14 +90,12 @@ static void calc_XY_velocity(){
last_latitude = g_gps->latitude; last_latitude = g_gps->latitude;
} }
#if RETRO_LOITER_MODE == ENABLED
static void calc_GPS_velocity() static void calc_GPS_velocity()
{ {
float temp = radians((float)g_gps->ground_course/100.0); float temp = radians((float)g_gps->ground_course/100.0);
x_actual_speed = (float)g_gps->ground_speed * sin(temp); x_actual_speed = (float)g_gps->ground_speed * sin(temp);
y_actual_speed = (float)g_gps->ground_speed * cos(temp); y_actual_speed = (float)g_gps->ground_speed * cos(temp);
} }
#endif
static void calc_location_error(struct Location *next_loc) 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 #define NAV_RATE_ERR_MAX 250
static void calc_loiter(int x_error, int y_error) static void calc_loiter(int x_error, int y_error)
{ {
#if RETRO_LOITER_MODE == ENABLED if(g.retro_loiter){
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
y_error = constrain(y_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 p,i,d; // used to capture pid values for logging
int32_t output; int32_t output;
@ -184,9 +182,9 @@ static void calc_loiter(int x_error, int y_error)
#endif #endif
x_rate_error = x_target_speed - x_actual_speed; // calc the speed error x_rate_error = x_target_speed - x_actual_speed; // calc the speed error
#if RETRO_LOITER_MODE == ENABLED if(g.retro_loiter){
x_rate_error = constrain(x_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX); 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); p = g.pid_loiter_rate_lon.get_p(x_rate_error);
i = g.pid_loiter_rate_lon.get_i(x_rate_error, dTnav); i = g.pid_loiter_rate_lon.get_i(x_rate_error, dTnav);
d = g.pid_loiter_rate_lon.get_d(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 #endif
y_rate_error = y_target_speed - y_actual_speed; y_rate_error = y_target_speed - y_actual_speed;
#if RETRO_LOITER_MODE == ENABLED if(g.retro_loiter){
y_rate_error = constrain(y_rate_error, -NAV_RATE_ERR_MAX, NAV_RATE_ERR_MAX); 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); p = g.pid_loiter_rate_lat.get_p(y_rate_error);
i = g.pid_loiter_rate_lat.get_i(y_rate_error, dTnav); i = g.pid_loiter_rate_lat.get_i(y_rate_error, dTnav);
d = g.pid_loiter_rate_lat.get_d(y_rate_error, dTnav); d = g.pid_loiter_rate_lat.get_d(y_rate_error, dTnav);