mirror of https://github.com/ArduPilot/ardupilot
Minor adjustments to Loiter behavior.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2357 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
dfd231197b
commit
125e4ace80
|
@ -1136,16 +1136,8 @@ void update_current_flight_mode(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// called after a GPS read
|
void update_nav_wp()
|
||||||
void update_navigation()
|
|
||||||
{
|
{
|
||||||
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
|
|
||||||
// ------------------------------------------------------------------------
|
|
||||||
|
|
||||||
// distance and bearing calcs only
|
|
||||||
if(control_mode == AUTO){
|
|
||||||
verify_commands();
|
|
||||||
|
|
||||||
if(wp_control == LOITER_MODE){
|
if(wp_control == LOITER_MODE){
|
||||||
// calc a pitch to the target
|
// calc a pitch to the target
|
||||||
calc_loiter_nav();
|
calc_loiter_nav();
|
||||||
|
@ -1165,6 +1157,19 @@ void update_navigation()
|
||||||
calc_nav_output();
|
calc_nav_output();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// called after a GPS read
|
||||||
|
void update_navigation()
|
||||||
|
{
|
||||||
|
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
|
||||||
|
// ------------------------------------------------------------------------
|
||||||
|
|
||||||
|
// distance and bearing calcs only
|
||||||
|
if(control_mode == AUTO){
|
||||||
|
verify_commands();
|
||||||
|
|
||||||
|
update_nav_wp();
|
||||||
|
|
||||||
// this tracks a location so the copter is always pointing towards it.
|
// this tracks a location so the copter is always pointing towards it.
|
||||||
if(yaw_tracking == MAV_ROI_LOCATION){
|
if(yaw_tracking == MAV_ROI_LOCATION){
|
||||||
|
@ -1178,20 +1183,13 @@ void update_navigation()
|
||||||
|
|
||||||
switch(control_mode){
|
switch(control_mode){
|
||||||
case LOITER:
|
case LOITER:
|
||||||
// calc a pitch to the target
|
wp_control = LOITER_MODE;
|
||||||
calc_loiter_nav();
|
update_nav_wp();
|
||||||
|
|
||||||
// limit tilt
|
|
||||||
limit_nav_pitch_roll(g.pitch_max.get());
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case RTL:
|
case RTL:
|
||||||
// calc a pitch to the target
|
wp_control = (wp_distance < 700) ? LOITER_MODE : WP_MODE;
|
||||||
calc_loiter_nav();
|
update_nav_wp();
|
||||||
|
|
||||||
// limit tilt
|
|
||||||
limit_nav_pitch_roll(g.pitch_max.get());
|
|
||||||
update_crosstrack();
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -375,7 +375,7 @@
|
||||||
# define NAV_LOITER_P 2.5 // upped to be a bit more aggressive
|
# define NAV_LOITER_P 2.5 // upped to be a bit more aggressive
|
||||||
#endif
|
#endif
|
||||||
#ifndef NAV_LOITER_I
|
#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
|
#endif
|
||||||
#ifndef NAV_LOITER_D
|
#ifndef NAV_LOITER_D
|
||||||
# define NAV_LOITER_D 0.00
|
# define NAV_LOITER_D 0.00
|
||||||
|
|
|
@ -43,13 +43,13 @@ bool check_missed_wp()
|
||||||
return (abs(temp) > 10000); //we pased the waypoint by 10 °
|
return (abs(temp) > 10000); //we pased the waypoint by 10 °
|
||||||
}
|
}
|
||||||
|
|
||||||
#define DIST_ERROR_MAX 1800
|
#define DIST_ERROR_MAX 700
|
||||||
void calc_loiter_nav()
|
void calc_loiter_nav()
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
Becuase we are using lat and lon to do our distance errors here's a quick chart:
|
Becuase we are using lat and lon to do our distance errors here's a quick chart:
|
||||||
100 = 1m
|
100 = 1m
|
||||||
1000 = 11m
|
1000 = 11m = 36 feet
|
||||||
1800 = 19.80m = 60 feet
|
1800 = 19.80m = 60 feet
|
||||||
3000 = 33m
|
3000 = 33m
|
||||||
10000 = 111m
|
10000 = 111m
|
||||||
|
@ -62,15 +62,15 @@ void calc_loiter_nav()
|
||||||
// Y PITCH
|
// Y PITCH
|
||||||
lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH
|
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
|
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
|
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, dTnav, 1.0); // X 700 * 2.5 = 1750,
|
||||||
nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
|
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
|
||||||
|
|
||||||
// PITCH Y
|
long pmax = g.pitch_max.get();
|
||||||
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); // invert lat (for pitch)
|
nav_lon = constrain(nav_lon, -pmax, pmax);
|
||||||
|
nav_lat = constrain(nav_lat, -pmax, pmax);
|
||||||
}
|
}
|
||||||
|
|
||||||
void calc_loiter_output()
|
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.
|
//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());
|
//limit_nav_pitch_roll(g.pitch_max.get());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -210,6 +210,33 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
|
||||||
g.rc_4.set_angle(9000);
|
g.rc_4.set_angle(9000);
|
||||||
dTnav = 200;
|
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
|
//dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305
|
||||||
|
|
||||||
while(1){
|
while(1){
|
||||||
|
@ -227,7 +254,6 @@ test_wp_nav(uint8_t argc, const Menu::arg *argv)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
|
|
||||||
static int8_t
|
static int8_t
|
||||||
test_failsafe(uint8_t argc, const Menu::arg *argv)
|
test_failsafe(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue