diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 463b9b1407..0e9316927c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -666,6 +666,8 @@ static void medium_loop() if (g.log_bitmask & MASK_LOG_NTUN) Log_Write_Nav_Tuning(); } + }else{ + g_gps->new_data = false; } break; @@ -921,12 +923,13 @@ static void update_GPS(void) //current_loc.lat = -1224318000; // Lat * 10 * *7 //current_loc.alt = 100; // alt * 10 * *7 //return; - if(gps_watchdog < 10){ + if(gps_watchdog < 12){ gps_watchdog++; }else{ // we have lost GPS signal for a moment. Reduce our error to avoid flyaways - nav_roll >>= 1; - nav_pitch >>= 1; + // commented temporarily + //nav_roll >>= 1; + //nav_pitch >>= 1; } if (g_gps->new_data && g_gps->fix) { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 59e5aa7815..3039ca197a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -467,7 +467,7 @@ // Navigation control gains // #ifndef LOITER_P -# define LOITER_P .4 // +# define LOITER_P 1.0 // #endif #ifndef LOITER_I # define LOITER_I 0.01 // @@ -480,14 +480,14 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #ifndef NAV_I -# define NAV_I 0.15 // this +# define NAV_I 0.10 // this #endif #ifndef NAV_IMAX -# define NAV_IMAX 20 // degrees +# define NAV_IMAX 16 // degrees #endif #ifndef WAYPOINT_SPEED_MAX -# define WAYPOINT_SPEED_MAX 400 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph #endif diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 766a23a7d5..5ba03676fe 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -55,14 +55,9 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH } - -// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0); - #define NAV_ERR_MAX 400 static void calc_loiter(int x_error, int y_error) { - // moved to globals for logging - x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); @@ -87,16 +82,25 @@ static void calc_loiter(int x_error, int y_error) #endif y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum + y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); - //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); - x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -600, 600); + x_rate_error = constrain(x_rate_error, -250, 250); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } +// nav_roll, nav_pitch +static void calc_loiter_pitch_roll() +{ + // rotate the vector + nav_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; + + // flip pitch because forward is negative + nav_pitch = -nav_pitch; +} + static void calc_nav_rate(int max_speed) { /* @@ -153,17 +157,6 @@ static void calc_nav_pitch_roll() } -// nav_roll, nav_pitch -static void calc_loiter_pitch_roll() -{ - // rotate the vector - nav_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; - - // flip pitch because forward is negative - nav_pitch = -nav_pitch; -} - static long get_altitude_error() { return next_WP.alt - current_loc.alt;