mirror of https://github.com/ArduPilot/ardupilot
Altered CIRCLE mode to allow Loiter_N_Turns to work in AP mode.
This commit is contained in:
parent
3f1faf70ac
commit
790bd6f8c9
|
@ -327,7 +327,6 @@ static long target_bearing; // deg * 100 : 0 to 360 location of the plane
|
|||
//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target
|
||||
|
||||
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
|
||||
static long circle_angle = 0;
|
||||
static byte wp_control; // used to control - navgation or loiter
|
||||
|
||||
static byte command_must_index; // current command memory location
|
||||
|
@ -402,9 +401,15 @@ static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, o
|
|||
|
||||
// Loiter management
|
||||
// -----------------
|
||||
static long saved_target_bearing; // deg * 100
|
||||
static unsigned long loiter_time; // millis : when we started LOITER mode
|
||||
static unsigned long loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
static long original_target_bearing; // deg * 100, used to check we are not passing the WP
|
||||
static long old_target_bearing; // used to track difference in angle
|
||||
|
||||
static int loiter_total; // deg : how many times to loiter * 360
|
||||
//static int loiter_delta; // deg : how far we just turned
|
||||
static int loiter_sum; // deg : how far we have turned around a waypoint
|
||||
static long loiter_time; // millis : when we started LOITER mode
|
||||
static int loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
|
||||
|
||||
// these are the values for navigation control functions
|
||||
// ----------------------------------------------------
|
||||
|
@ -1135,7 +1140,6 @@ static void update_navigation()
|
|||
switch(control_mode){
|
||||
case AUTO:
|
||||
verify_commands();
|
||||
|
||||
// note: wp_control is handled by commands_logic
|
||||
|
||||
// calculates desired Yaw
|
||||
|
@ -1147,6 +1151,7 @@ static void update_navigation()
|
|||
|
||||
case GUIDED:
|
||||
wp_control = WP_MODE;
|
||||
|
||||
update_auto_yaw();
|
||||
update_nav_wp();
|
||||
break;
|
||||
|
@ -1161,6 +1166,7 @@ static void update_navigation()
|
|||
|
||||
wp_control = WP_MODE;
|
||||
}else{
|
||||
// lets just jump to Loiter Mode after RTL
|
||||
set_mode(LOITER);
|
||||
//xtrack_enabled = false;
|
||||
}
|
||||
|
@ -1171,33 +1177,19 @@ static void update_navigation()
|
|||
|
||||
// switch passthrough to LOITER
|
||||
case LOITER:
|
||||
wp_control = LOITER_MODE;
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
break;
|
||||
|
||||
case CIRCLE:
|
||||
yaw_tracking = MAV_ROI_WPNEXT;
|
||||
yaw_tracking = MAV_ROI_WPNEXT;
|
||||
wp_control = CIRCLE_MODE;
|
||||
|
||||
// calculates desired Yaw
|
||||
update_auto_yaw();
|
||||
{
|
||||
circle_angle = wrap_360(target_bearing + 3000 + 18000);
|
||||
|
||||
target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle/100)));
|
||||
target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle/100)));
|
||||
}
|
||||
|
||||
// calc the lat and long error to the target
|
||||
calc_location_error(&target_WP);
|
||||
|
||||
// use error as the desired rate towards the target
|
||||
// nav_lon, nav_lat is calculated
|
||||
calc_nav_rate(long_error, lat_error, 200, 0);
|
||||
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll();
|
||||
update_nav_wp();
|
||||
break;
|
||||
|
||||
}
|
||||
|
@ -1383,6 +1375,39 @@ static void update_nav_wp()
|
|||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll();
|
||||
|
||||
}else if(wp_control == CIRCLE_MODE){
|
||||
|
||||
// check if we have missed the WP
|
||||
int loiter_delta = (target_bearing - old_target_bearing)/100;
|
||||
|
||||
// reset the old value
|
||||
old_target_bearing = target_bearing;
|
||||
|
||||
// wrap values
|
||||
if (loiter_delta > 180) loiter_delta -= 360;
|
||||
if (loiter_delta < -180) loiter_delta += 360;
|
||||
|
||||
// sum the angle around the WP
|
||||
loiter_sum += abs(loiter_delta);
|
||||
|
||||
|
||||
// creat a virtual waypoint that circles the next_WP
|
||||
// Count the degrees we have circulated the WP
|
||||
int circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100;
|
||||
|
||||
target_WP.lng = next_WP.lng + (g.loiter_radius * cos(radians(90 - circle_angle)));
|
||||
target_WP.lat = next_WP.lat + (g.loiter_radius * sin(radians(90 - circle_angle)));
|
||||
|
||||
// calc the lat and long error to the target
|
||||
calc_location_error(&target_WP);
|
||||
|
||||
// use error as the desired rate towards the target
|
||||
// nav_lon, nav_lat is calculated
|
||||
calc_nav_rate(long_error, lat_error, 200, 0);
|
||||
|
||||
// rotate pitch and roll to the copter frame of reference
|
||||
calc_nav_pitch_roll();
|
||||
|
||||
} else {
|
||||
// for long journey's reset the wind resopnse
|
||||
// it assumes we are standing still.
|
||||
|
|
|
@ -176,11 +176,7 @@ static void set_next_WP(struct Location *wp)
|
|||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
saved_target_bearing = target_bearing;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
//crosstrack_bearing = target_bearing; // Used for track following
|
||||
original_target_bearing = target_bearing;
|
||||
|
||||
gcs.print_current_waypoints();
|
||||
}
|
||||
|
|
|
@ -142,7 +142,7 @@ static bool verify_must()
|
|||
break;
|
||||
|
||||
case MAV_CMD_NAV_LOITER_TURNS:
|
||||
return true;
|
||||
return verify_loiter_turns();
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LOITER_TIME:
|
||||
|
@ -297,8 +297,7 @@ static void do_loiter_unlimited()
|
|||
|
||||
static void do_loiter_turns()
|
||||
{
|
||||
/*
|
||||
wp_control = LOITER_MODE;
|
||||
wp_control == CIRCLE_MODE;
|
||||
|
||||
if(next_command.lat == 0)
|
||||
set_next_WP(¤t_loc);
|
||||
|
@ -306,7 +305,7 @@ static void do_loiter_turns()
|
|||
set_next_WP(&next_command);
|
||||
|
||||
loiter_total = next_command.p1 * 360;
|
||||
*/
|
||||
loiter_sum = 0;
|
||||
}
|
||||
|
||||
static void do_loiter_time()
|
||||
|
@ -431,6 +430,20 @@ static bool verify_loiter_time()
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool verify_loiter_turns()
|
||||
{
|
||||
// have we rotated around the center enough times?
|
||||
// -----------------------------------------------
|
||||
if(loiter_sum > loiter_total) {
|
||||
loiter_total = 0;
|
||||
loiter_sum = 0;
|
||||
//gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete"));
|
||||
// clear the command queue;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_RTL()
|
||||
{
|
||||
if (wp_distance <= g.waypoint_radius) {
|
||||
|
|
|
@ -165,6 +165,7 @@
|
|||
|
||||
#define LOITER_MODE 1
|
||||
#define WP_MODE 2
|
||||
#define CIRCLE_MODE 3
|
||||
|
||||
// Waypoint options
|
||||
#define WP_OPTION_ALT_RELATIVE 1
|
||||
|
|
|
@ -28,7 +28,7 @@ static byte navigate()
|
|||
|
||||
static bool check_missed_wp()
|
||||
{
|
||||
long temp = target_bearing - saved_target_bearing;
|
||||
long temp = target_bearing - original_target_bearing;
|
||||
temp = wrap_180(temp);
|
||||
return (abs(temp) > 10000); //we pased the waypoint by 10 °
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue