diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index b3a30b214d..46336acea7 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -1136,6 +1136,29 @@ void update_current_flight_mode(void) } } +void update_nav_wp() +{ + if(wp_control == LOITER_MODE){ + // calc a pitch to the target + calc_loiter_nav(); + + // rotate pitch and roll to the copter frame of reference + calc_loiter_output(); + + } else { + // how far are we from the ideal trajectory? + // this pushes us back on course + update_crosstrack(); + + // calc a rate dampened pitch to the target + calc_rate_nav(); + + // rotate that pitch to the copter frame of reference + calc_nav_output(); + } + +} + // called after a GPS read void update_navigation() { @@ -1146,25 +1169,7 @@ void update_navigation() if(control_mode == AUTO){ verify_commands(); - if(wp_control == LOITER_MODE){ - // calc a pitch to the target - calc_loiter_nav(); - - // rotate pitch and roll to the copter frame of reference - calc_loiter_output(); - - } else { - // how far are we from the ideal trajectory? - // this pushes us back on course - update_crosstrack(); - - // calc a rate dampened pitch to the target - calc_rate_nav(); - - // rotate that pitch to the copter frame of reference - calc_nav_output(); - } - + update_nav_wp(); // this tracks a location so the copter is always pointing towards it. if(yaw_tracking == MAV_ROI_LOCATION){ @@ -1178,20 +1183,13 @@ void update_navigation() switch(control_mode){ case LOITER: - // calc a pitch to the target - calc_loiter_nav(); - - // limit tilt - limit_nav_pitch_roll(g.pitch_max.get()); + wp_control = LOITER_MODE; + update_nav_wp(); break; case RTL: - // calc a pitch to the target - calc_loiter_nav(); - - // limit tilt - limit_nav_pitch_roll(g.pitch_max.get()); - update_crosstrack(); + wp_control = (wp_distance < 700) ? LOITER_MODE : WP_MODE; + update_nav_wp(); break; } } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index a980c35cb7..afb0c4f283 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -375,7 +375,7 @@ # define NAV_LOITER_P 2.5 // upped to be a bit more aggressive #endif #ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.15 // upped a bit to deal with wind faster +# define NAV_LOITER_I 0.05 // upped a bit to deal with wind faster #endif #ifndef NAV_LOITER_D # define NAV_LOITER_D 0.00 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 4122590000..47a4d6704a 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -43,13 +43,13 @@ bool check_missed_wp() return (abs(temp) > 10000); //we pased the waypoint by 10 ° } -#define DIST_ERROR_MAX 1800 +#define DIST_ERROR_MAX 700 void calc_loiter_nav() { /* Becuase we are using lat and lon to do our distance errors here's a quick chart: 100 = 1m - 1000 = 11m + 1000 = 11m = 36 feet 1800 = 19.80m = 60 feet 3000 = 33m 10000 = 111m @@ -62,15 +62,15 @@ void calc_loiter_nav() // Y PITCH lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH - long_error = constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error - // Convert distance into ROLL X - nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0); + nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750, + nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch) - // PITCH Y - nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); // invert lat (for pitch) + long pmax = g.pitch_max.get(); + nav_lon = constrain(nav_lon, -pmax, pmax); + nav_lat = constrain(nav_lat, -pmax, pmax); } void calc_loiter_output() @@ -104,9 +104,6 @@ void calc_loiter_output() //limit our copter pitch - this will change if we go to a fully rate limited approach. - long pmax = g.pitch_max.get(); - nav_roll = constrain(nav_roll, -pmax, pmax); - nav_pitch = constrain(nav_pitch, -pmax, pmax); //limit_nav_pitch_roll(g.pitch_max.get()); } diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 0934822605..ed9307eec3 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -210,6 +210,33 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv) g.rc_4.set_angle(9000); dTnav = 200; + //dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305 + while(1){ + delay(20); + read_radio(); + current_loc.lng = g.rc_1.control_in; + current_loc.lat = g.rc_2.control_in; + + calc_loiter_nav(); + + Serial.printf("Lon_e: %ld, nLon, %ld, Lat_e %ld, nLat %ld\n", long_error, nav_lon, lat_error, nav_lat); + + if(Serial.available() > 0){ + return (0); + } + } +} +//*/ + +/* +{ + print_hit_enter(); + delay(1000); + + g.rc_6.set_range(0,900); + g.rc_4.set_angle(9000); + dTnav = 200; + //dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305 while(1){ @@ -227,7 +254,6 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv) } } */ - static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv) {