mirror of https://github.com/ArduPilot/ardupilot
Loiter should work now.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1921 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
7e843ee679
commit
93a7a76c9b
|
@ -257,6 +257,7 @@ int climb_rate; // m/s * 100 - For future implementation of controlled a
|
|||
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
|
||||
|
||||
int last_ground_speed; // used to dampen navigation
|
||||
byte wp_control; // used to control - navgation or loiter
|
||||
|
||||
byte command_must_index; // current command memory location
|
||||
byte command_may_index; // current command memory location
|
||||
|
@ -327,9 +328,6 @@ byte yaw_tracking = TRACK_NONE; // no tracking, point at next wp, or at a tar
|
|||
// Loiter management
|
||||
// -----------------
|
||||
long saved_target_bearing; // deg * 100
|
||||
//int loiter_total; // deg : how many times to loiter * 360
|
||||
//int loiter_delta; // deg : how far we just turned
|
||||
//int loiter_sum; // deg : how far we have turned around a waypoint
|
||||
long loiter_time; // millis : when we started LOITER mode
|
||||
long loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
|
||||
|
@ -591,21 +589,6 @@ void medium_loop()
|
|||
// --------------------------------------------
|
||||
update_navigation();
|
||||
}
|
||||
|
||||
// we call these regardless of GPS because of the rapid nature of the yaw sensor
|
||||
// -----------------------------------------------------------------------------
|
||||
//if(wp_distance < 800){ // 8 meters
|
||||
//if(g.rc_6.control_in > 500){ // 8 meters
|
||||
//calc_loiter_nav();
|
||||
// calc_waypoint_nav();
|
||||
//}else{
|
||||
// calc_waypoint_nav();
|
||||
//}
|
||||
invalid_nav = true;
|
||||
|
||||
// replace with invalidater byte
|
||||
//calc_waypoint_nav();
|
||||
|
||||
break;
|
||||
|
||||
// command processing
|
||||
|
@ -946,7 +929,6 @@ void update_current_flight_mode(void)
|
|||
}
|
||||
break;
|
||||
|
||||
//case LOITER:
|
||||
case STABILIZE:
|
||||
// clear any AP naviagtion values
|
||||
nav_pitch = 0;
|
||||
|
@ -1107,11 +1089,17 @@ void update_navigation()
|
|||
if(control_mode == AUTO || control_mode == GCS_AUTO){
|
||||
verify_commands();
|
||||
|
||||
// calc a rate dampened pitch to the target
|
||||
calc_rate_nav();
|
||||
if(wp_control == LOITER_MODE){
|
||||
// calc a pitch to the target
|
||||
calc_loiter_nav();
|
||||
|
||||
// rotate that pitch to the copter frame of reference
|
||||
calc_nav_output();
|
||||
} else {
|
||||
// calc a rate dampened pitch to the target
|
||||
calc_rate_nav();
|
||||
|
||||
// rotate that pitch to the copter frame of reference
|
||||
calc_nav_output();
|
||||
}
|
||||
|
||||
//limit our copter pitch - this will change if we go to a fully rate limited approach.
|
||||
limit_nav_pitch_roll(g.pitch_max.get());
|
||||
|
@ -1122,8 +1110,22 @@ void update_navigation()
|
|||
}
|
||||
|
||||
}else{
|
||||
|
||||
switch(control_mode){
|
||||
case LOITER:
|
||||
// calc a pitch to the target
|
||||
calc_loiter_nav();
|
||||
|
||||
// limit tilt
|
||||
limit_nav_pitch_roll(g.pitch_max.get());
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
// calc a pitch to the target
|
||||
calc_loiter_nav();
|
||||
|
||||
// limit tilt
|
||||
limit_nav_pitch_roll(g.pitch_max.get());
|
||||
update_crosstrack();
|
||||
break;
|
||||
}
|
||||
|
@ -1169,6 +1171,8 @@ void update_alt()
|
|||
// read barometer
|
||||
baro_alt = read_barometer();
|
||||
|
||||
|
||||
// XXX temp removed fr debugging
|
||||
//filter out bad sonar reads
|
||||
//int temp = sonar.read();
|
||||
|
||||
|
@ -1182,12 +1186,15 @@ void update_alt()
|
|||
update_sonar_light(sonar_alt > 100);
|
||||
|
||||
// decide if we're using sonar
|
||||
if (baro_alt < 1200){
|
||||
if ((baro_alt < 1200) || sonar_alt < 300){
|
||||
|
||||
// correct alt for angle of the sonar
|
||||
sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
temp = max(temp, 0.707);
|
||||
|
||||
if(sonar_alt < 500){
|
||||
sonar_alt = (float)sonar_alt * temp;
|
||||
|
||||
if(sonar_alt < 400){
|
||||
altitude_sensor = SONAR;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -193,11 +193,13 @@ bool verify_may()
|
|||
}
|
||||
|
||||
/********************************************************************************/
|
||||
// Nav (Must) commands
|
||||
//
|
||||
/********************************************************************************/
|
||||
|
||||
void do_RTL(void)
|
||||
{
|
||||
// we need to change this.
|
||||
wp_control = LOITER_MODE;
|
||||
control_mode = LOITER;
|
||||
Location temp = home;
|
||||
temp.alt = read_alt_to_hold();
|
||||
|
@ -216,8 +218,14 @@ void do_RTL(void)
|
|||
// Log_Write_Mode(control_mode);
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
// Nav (Must) commands
|
||||
/********************************************************************************/
|
||||
|
||||
void do_takeoff()
|
||||
{
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
Location temp = current_loc;
|
||||
|
||||
// next_command.alt is a relative altitude!!!
|
||||
|
@ -233,6 +241,8 @@ void do_takeoff()
|
|||
|
||||
void do_nav_wp()
|
||||
{
|
||||
wp_control = WP_MODE;
|
||||
|
||||
// next_command.alt is a relative altitude!!!
|
||||
next_command.alt += home.alt;
|
||||
|
||||
|
@ -256,6 +266,8 @@ void do_nav_wp()
|
|||
|
||||
void do_land()
|
||||
{
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
Serial.println("dlnd ");
|
||||
|
||||
// not really used right now, might be good for debugging
|
||||
|
@ -276,6 +288,8 @@ void do_land()
|
|||
|
||||
void do_loiter_unlimited()
|
||||
{
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
//Serial.println("dloi ");
|
||||
if(next_command.lat == 0)
|
||||
set_next_WP(¤t_loc);
|
||||
|
@ -286,6 +300,8 @@ void do_loiter_unlimited()
|
|||
void do_loiter_turns()
|
||||
{
|
||||
/*
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
if(next_command.lat == 0)
|
||||
set_next_WP(¤t_loc);
|
||||
else
|
||||
|
@ -298,6 +314,7 @@ void do_loiter_turns()
|
|||
void do_loiter_time()
|
||||
{
|
||||
/*
|
||||
wp_control = LOITER_MODE;
|
||||
if(next_command.lat == 0)
|
||||
set_next_WP(¤t_loc);
|
||||
else
|
||||
|
@ -393,6 +410,9 @@ bool verify_nav_wp()
|
|||
if(wp_verify_byte & NAV_LOCATION){
|
||||
// we have reached our goal
|
||||
|
||||
// loiter at the WP
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
if ((millis() - loiter_time) > loiter_time_max) {
|
||||
wp_verify_byte |= NAV_DELAY;
|
||||
//gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
|
||||
|
|
|
@ -129,6 +129,9 @@
|
|||
#define TRACK_NEXT_WP 2
|
||||
#define TRACK_TARGET_WP 4
|
||||
|
||||
#define LOITER_MODE 1
|
||||
#define WP_MODE 2
|
||||
|
||||
// Waypoint options
|
||||
#define WP_OPTION_ALT_RELATIVE 1
|
||||
#define WP_OPTION_ALT_CHANGE 2
|
||||
|
|
|
@ -224,8 +224,6 @@ void update_crosstrack(void)
|
|||
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line
|
||||
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
}else{
|
||||
reset_crosstrack();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue