Rover: more cleanup, and fixed navigation code

removed loiter code

APMrover2/APM_Config_HILmode.h

@@ -404,25 +404,7 @@
 // mainly intended to allow users to start using the APM without running the
 // WaypointWriter first.
 //
-// LOITER_RADIUS_DEFAULT                    OPTIONAL
-//
-// When the user performs a factory reset on the APM, set the loiter radius
-// (the distance the APM will attempt to maintain from a waypoint while
-// loitering) to this value in meters.  This is mainly intended to allow
-// users to start using the APM without running the WaypointWriter first.
-//
-// USE_CURRENT_ALT							OPTIONAL
-// ALT_HOLD_HOME							OPTIONAL
-//
-// When the user performs a factory reset on the APM, set the flag for weather
-// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
-// Also, set the value of USE_CURRENT_ALT in meters.  This is mainly intended to allow
-// users to start using the APM without running the WaypointWriter first.
-//
 #define WP_RADIUS_DEFAULT       1   // meters
-#define LOITER_RADIUS_DEFAULT	5   
-#define USE_CURRENT_ALT		TRUE
-#define ALT_HOLD_HOME		0
 
 //////////////////////////////////////////////////////////////////////////////
 // INPUT_VOLTAGE                            OPTIONAL

APMrover2/APM_Config_Rover.h

@@ -412,25 +412,7 @@ This feature works only if the ROV_AWPR_NAV is set to 0
 // mainly intended to allow users to start using the APM without running the
 // WaypointWriter first.
 //
-// LOITER_RADIUS_DEFAULT                    OPTIONAL
-//
-// When the user performs a factory reset on the APM, set the loiter radius
-// (the distance the APM will attempt to maintain from a waypoint while
-// loitering) to this value in meters.  This is mainly intended to allow
-// users to start using the APM without running the WaypointWriter first.
-//
-// USE_CURRENT_ALT							OPTIONAL
-// ALT_HOLD_HOME							OPTIONAL
-//
-// When the user performs a factory reset on the APM, set the flag for weather
-// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
-// Also, set the value of USE_CURRENT_ALT in meters.  This is mainly intended to allow
-// users to start using the APM without running the WaypointWriter first.
-//
 #define WP_RADIUS_DEFAULT       1   // meters
-#define LOITER_RADIUS_DEFAULT	5   // 60
-#define USE_CURRENT_ALT		TRUE
-#define ALT_HOLD_HOME		0
 
 //////////////////////////////////////////////////////////////////////////////
 // INPUT_VOLTAGE                            OPTIONAL

APMrover2/APMrover2.pde

@@ -318,18 +318,18 @@ static const char *comma = ",";
 
 static const char* flight_mode_strings[] = {
 	"Manual",
-	"Circle",
+	"",
 	"Learning",
 	"",
 	"",
-	"FBW_A",
+	"",
-	"FBW_B",
+	"",
 	"",
 	"",
 	"",
 	"Auto",
 	"RTL",
-	"Loiter",
+	"",
 	"",
 	"",
 	"",
@@ -374,7 +374,7 @@ static bool rc_override_active = false;
 ////////////////////////////////////////////////////////////////////////////////
 // A tracking variable for type of failsafe active
 // Used for failsafe based on loss of RC signal or GCS signal
-static int 	failsafe;					
+static int16_t 	failsafe;					
 // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
 // RC receiver should be set up to output a low throttle value when signal is lost
 static bool 	ch3_failsafe;
@@ -407,7 +407,7 @@ static byte 	ground_start_count	= 5;
 // Used to compute a speed estimate from the first valid gps fixes to decide if we are 
 // on the ground or in the air.  Used to decide if a ground start is appropriate if we
 // booted with an air start.
-static int     ground_start_avg;
+static int16_t     ground_start_avg;
 static int32_t          gps_base_alt;		
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -418,18 +418,15 @@ const	float radius_of_earth 	= 6378100;	// meters
 const	float gravity 			= 9.81;		// meters/ sec^2
 // This is the currently calculated direction to fly.  
 // deg * 100 : 0 to 360
-static long	nav_bearing;
+static int32_t nav_bearing;
-// This is the direction to the next waypoint or loiter center 
+// This is the direction to the next waypoint
 // deg * 100 : 0 to 360
-static long	target_bearing;	
+static int32_t target_bearing;	
 //This is the direction from the last waypoint to the next waypoint 
 // deg * 100 : 0 to 360
-static long	crosstrack_bearing;
+static int32_t crosstrack_bearing;
 // A gain scaler to account for ground speed/headwind/tailwind
 static float	nav_gain_scaler 		= 1;		
-// Direction held during phases of takeoff and landing
-// deg * 100 dir of plane,  A value of -1 indicates the course has not been set/is not in use
-static long    hold_course       	 	= -1;		// deg * 100 dir of plane
 static bool rtl_complete = false;
 
 // There may be two active commands in Auto mode.  
@@ -443,7 +440,7 @@ static byte	non_nav_command_ID	= NO_COMMAND;
 
 static float	groundspeed_error;	
 // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
-static int     throttle_nudge = 0;
+static int16_t     throttle_nudge = 0;
 // The distance as reported by Sonar in cm – Values are 20 to 700 generally.
 static int16_t		sonar_dist;
 static bool obstacle = false;
@@ -452,17 +449,17 @@ static bool obstacle = false;
 // Ground speed
 ////////////////////////////////////////////////////////////////////////////////
 // The amount current ground speed is below min ground speed.  Centimeters per second
-static long		groundspeed_undershoot = 0;
+static int32_t 	groundspeed_undershoot = 0;
-static long		ground_speed = 0;
+static int32_t 	ground_speed = 0;
-static int throttle_last = 0, throttle = 500;
+static int16_t throttle_last = 0, throttle = 500;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Location Errors
 ////////////////////////////////////////////////////////////////////////////////
 // Difference between current bearing and desired bearing.  Hundredths of a degree
-static long	bearing_error;
+static int32_t bearing_error;
 // Difference between current altitude and desired altitude.  Centimeters
-static long	altitude_error;
+static int32_t altitude_error;
 // Distance perpandicular to the course line that we are off trackline.  Meters 
 static float	crosstrack_error;
 
@@ -495,42 +492,26 @@ static float	current_total1;
 //static float	current_total2;									// Totalized current (Amp-hours) from battery 2
 
 // JLN Update
-unsigned long  timesw                  = 0;
+uint32_t  timesw                  = 0;
 static bool speed_boost = false;
 
-////////////////////////////////////////////////////////////////////////////////
-// Loiter management
-////////////////////////////////////////////////////////////////////////////////
-// Previous target bearing.  Used to calculate loiter rotations.  Hundredths of a degree
-static long 	old_target_bearing;
-// Total desired rotation in a loiter.  Used for Loiter Turns commands.  Degrees
-static int		loiter_total; 
-// The amount in degrees we have turned since recording old_target_bearing
-static int 	loiter_delta;
-// Total rotation in a loiter.  Used for Loiter Turns commands and to check for missed waypoints.  Degrees
-static int		loiter_sum;
-// The amount of time we have been in a Loiter.  Used for the Loiter Time command.  Milliseconds.
-static long 	loiter_time;
-// The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
-static int 	loiter_time_max;
-
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation control variables
 ////////////////////////////////////////////////////////////////////////////////
 // The instantaneous desired bank angle.  Hundredths of a degree
-static long	nav_roll;
+static int32_t nav_roll;
 // The instantaneous desired pitch angle.  Hundredths of a degree
-static long	nav_pitch;
+static int32_t nav_pitch;
 // Calculated radius for the wp turn based on ground speed and max turn angle
-static long    wp_radius;
+static int32_t    wp_radius;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Waypoint distances
 ////////////////////////////////////////////////////////////////////////////////
 // Distance between plane and next waypoint.  Meters
-static long	wp_distance;
+static int32_t wp_distance;
 // Distance between previous and next waypoint.  Meters
-static long	wp_totalDistance;
+static int32_t wp_totalDistance;
 
 ////////////////////////////////////////////////////////////////////////////////
 // repeating event control
@@ -538,27 +519,27 @@ static long	wp_totalDistance;
 // Flag indicating current event type
 static byte 		event_id;
 // when the event was started in ms
-static long 		event_timer;
+static int32_t 		event_timer;
 // how long to delay the next firing of event in millis
 static uint16_t 	event_delay;					
 // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
-static int 		event_repeat = 0;
+static int16_t 		event_repeat = 0;
 // per command value, such as PWM for servos
-static int 		event_value; 
+static int16_t 		event_value; 
 // the value used to cycle events (alternate value to event_value)
-static int 		event_undo_value;
+static int16_t 		event_undo_value;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Conditional command
 ////////////////////////////////////////////////////////////////////////////////
 // A value used in condition commands (eg delay, change alt, etc.)
 // For example in a change altitude command, it is the altitude to change to.
-static long 	condition_value;
+static int32_t 	condition_value;
 // A starting value used to check the status of a conditional command.
 // For example in a delay command the condition_start records that start time for the delay
-static long 	condition_start;
+static int32_t 	condition_start;
 // A value used in condition commands.  For example the rate at which to change altitude.
-static int 		condition_rate;
+static int16_t 		condition_rate;
 
 ////////////////////////////////////////////////////////////////////////////////
 // 3D Location vectors
@@ -572,7 +553,7 @@ static bool	home_is_set;
 static struct 	Location prev_WP;
 // The plane's current location
 static struct 	Location current_loc;
-// The location of the current/active waypoint.  Used for altitude ramp, track following and loiter calculations.
+// The location of the current/active waypoint.  Used for track following
 static struct 	Location next_WP;
 // The location of the active waypoint in Guided mode.
 static struct  	Location guided_WP;
@@ -593,30 +574,30 @@ static float G_Dt						= 0.02;
 // Performance monitoring
 ////////////////////////////////////////////////////////////////////////////////
 // Timer used to accrue data and trigger recording of the performanc monitoring log message
-static long 	perf_mon_timer;
+static int32_t 	perf_mon_timer;
 // The maximum main loop execution time recorded in the current performance monitoring interval
-static int 	G_Dt_max = 0;
+static int16_t 	G_Dt_max = 0;
 // The number of gps fixes recorded in the current performance monitoring interval
-static int 	gps_fix_count = 0;
+static int16_t 	gps_fix_count = 0;
 // A variable used by developers to track performanc metrics.
 // Currently used to record the number of GCS heartbeat messages received
-static int		pmTest1 = 0;
+static int16_t pmTest1 = 0;
 
 
 ////////////////////////////////////////////////////////////////////////////////
 // System Timers
 ////////////////////////////////////////////////////////////////////////////////
 // Time in miliseconds of start of main control loop.  Milliseconds
-static unsigned long 	fast_loopTimer;
+static uint32_t 	fast_loopTimer;
 // Time Stamp when fast loop was complete.  Milliseconds
-static unsigned long 	fast_loopTimeStamp;
+static uint32_t 	fast_loopTimeStamp;
 // Number of milliseconds used in last main loop cycle
 static uint8_t 		delta_ms_fast_loop;
 // Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
 // Time in miliseconds of start of medium control loop.  Milliseconds
-static unsigned long 	medium_loopTimer;
+static uint32_t 	medium_loopTimer;
 // Counters for branching from main control loop to slower loops
 static byte 			medium_loopCounter;	
 // Number of milliseconds used in last medium loop cycle
@@ -977,43 +958,21 @@ static void update_GPS(void)
 
 static void update_current_flight_mode(void)
 { 
-	if(control_mode == AUTO) {
-		switch (nav_command_ID) {
-				hold_course = -1;
-				calc_nav_roll();
-				calc_throttle();
-				break;
-            }
-	}else{
     switch(control_mode){
+    case AUTO:
     case RTL:
-				hold_course = -1;
         calc_nav_roll();
         calc_throttle();
         break;
 
     case LEARNING:
+    case MANUAL:
         nav_roll        = 0;
         nav_pitch       = 0;
-                       #if X_PLANE == ENABLED
-  				// servo_out is for Sim control only
-				// ---------------------------------
-				g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
-				g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
-				g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();                     
-                       #endif
-				// throttle is passthrough
-				break;
-
-			case MANUAL:
-				// servo_out is for Sim control only
-				// ---------------------------------
         g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
         g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
         g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();
         break;
-
-		}
 	}
 }
 
@@ -1028,10 +987,8 @@ static void update_navigation()
 	}else{
 
 		switch(control_mode){
-			case LOITER:
 			case RTL:        // no loitering around the wp with the rover, goes direct to the wp position
 			case GUIDED:
-				// update_loiter();
 				calc_nav_roll();
 				calc_bearing_error();
                 if(verify_RTL()) {  

APMrover2/GCS_Mavlink.pde

@@ -46,7 +46,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
         break;
     case AUTO:
     case RTL:
-    case LOITER:
     case GUIDED:
     case CIRCLE:
         base_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -215,7 +214,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
 
 static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 {
-    int16_t bearing = (hold_course==-1?nav_bearing:hold_course) / 100;
+    int16_t bearing = nav_bearing / 100;
     mavlink_msg_nav_controller_output_send(
         chan,
         nav_roll / 1.0e2,
@@ -1106,7 +1105,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             float param1 = 0, param2 = 0 , param3 = 0, param4 = 0;
 
-            // time that the mav should loiter in milliseconds
             uint8_t current = 0; // 1 (true), 0 (false)
 
 			if (packet.seq == (uint16_t)g.command_index)
@@ -1129,16 +1127,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
 			switch (tell_command.id) {				// Switch to map APM command fields inot MAVLink command fields
 
-				case MAV_CMD_NAV_LOITER_TURNS:
 				case MAV_CMD_NAV_TAKEOFF:
 				case MAV_CMD_DO_SET_HOME:
 					param1 = tell_command.p1;
 					break;
 
-				case MAV_CMD_NAV_LOITER_TIME:
-					param1 = tell_command.p1*10;    // APM loiter time is in ten second increments
-					break;
-
 				case MAV_CMD_CONDITION_CHANGE_ALT:
 					x=0;	// Clear fields loaded above that we don't want sent for this command
 					y=0;
@@ -1348,11 +1341,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
             switch (tell_command.id) {                    // Switch to map APM command fields inot MAVLink command fields
             case MAV_CMD_NAV_WAYPOINT:
-            case MAV_CMD_NAV_LOITER_UNLIM:
             case MAV_CMD_NAV_RETURN_TO_LAUNCH:
                 break;
 
-            case MAV_CMD_NAV_LOITER_TURNS:
             case MAV_CMD_NAV_TAKEOFF:
             case MAV_CMD_DO_SET_HOME:
                 tell_command.p1 = packet.param1;
@@ -1362,10 +1353,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 tell_command.lat = packet.param1;
                 break;
 
-            case MAV_CMD_NAV_LOITER_TIME:
-                tell_command.p1 = packet.param1 / 10;    // APM loiter time is in ten second increments
-                break;
-
             case MAV_CMD_CONDITION_DELAY:
             case MAV_CMD_CONDITION_DISTANCE:
                 tell_command.lat = packet.param1;
@@ -1807,8 +1794,8 @@ GCS_MAVLINK::queued_waypoint_send()
 */
 static void mavlink_delay(unsigned long t)
 {
-    unsigned long tstart;
+    uint32_t tstart;
-    static unsigned long last_1hz, last_50hz;
+    static uint32_t last_1hz, last_50hz;
 
     if (in_mavlink_delay) {
         // this should never happen, but let's not tempt fate by
@@ -1821,7 +1808,7 @@ static void mavlink_delay(unsigned long t)
 
     tstart = millis();
     do {
-        unsigned long tnow = millis();
+        uint32_t tnow = millis();
         if (tnow - last_1hz > 1000) {
             last_1hz = tnow;
             gcs_send_message(MSG_HEARTBEAT);

APMrover2/Parameters.h

@@ -161,7 +161,7 @@ public:
         k_param_command_total,
         k_param_command_index,
         k_param_waypoint_radius,
-        k_param_loiter_radius,
+        k_param_loiter_radius, // unused
         k_param_fence_action,
         k_param_fence_total,
         k_param_fence_channel,
@@ -262,7 +262,6 @@ public:
         AP_Int8     command_total;
         AP_Int8     command_index;
         AP_Int8     waypoint_radius;
-        AP_Int8     loiter_radius;
     
         // Fly-by-wire
         //

APMrover2/Parameters.pde

@@ -32,7 +32,6 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(command_total,          "CMD_TOTAL",        0),
 	GSCALAR(command_index,          "CMD_INDEX",        0),
 	GSCALAR(waypoint_radius,        "WP_RADIUS",        WP_RADIUS_DEFAULT),
-	GSCALAR(loiter_radius,          "WP_LOITER_RAD",    LOITER_RADIUS_DEFAULT),
 
 	GSCALAR(throttle_min,           "THR_MIN",          THROTTLE_MIN),
 	GSCALAR(throttle_max,           "THR_MAX",          THROTTLE_MAX),

APMrover2/commands.pde

@@ -120,21 +120,12 @@ static void set_next_WP(struct Location *wp)
         prev_WP = current_loc;
     }
 
-	// zero out our loiter vals to watch for missed waypoints
-	loiter_delta 		= 0;
-	loiter_sum 			= 0;
-	loiter_total 		= 0;
-
 	// this is handy for the groundstation
 	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing_cd(&current_loc, &next_WP);
 	nav_bearing 		= target_bearing;
 
-	// to check if we have missed the WP
-	// ----------------------------
-	old_target_bearing 	= target_bearing;
-
 	// set a new crosstrack bearing
 	// ----------------------------
 	reset_crosstrack();
@@ -155,10 +146,6 @@ static void set_guided_WP(void)
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing_cd(&current_loc, &next_WP);
 
-	// to check if we have missed the WP
-	// ----------------------------
-	old_target_bearing 	= target_bearing;
-
 	// set a new crosstrack bearing
 	// ----------------------------
 	reset_crosstrack();

APMrover2/commands_logic.pde

@@ -21,18 +21,6 @@ handle_process_nav_cmd()
 			do_nav_wp();
 			break;
 
-		case MAV_CMD_NAV_LOITER_UNLIM:	// Loiter indefinitely
-			do_loiter_unlimited();
-			break;
-
-		case MAV_CMD_NAV_LOITER_TURNS:	// Loiter N Times
-			do_loiter_turns();
-			break;
-
-		case MAV_CMD_NAV_LOITER_TIME:
-			do_loiter_time();
-			break;
-
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
 			do_RTL();
 			break;
@@ -139,32 +127,16 @@ static bool verify_nav_command()	// Returns true if command complete
 
 		case MAV_CMD_NAV_TAKEOFF:
 			return verify_takeoff();
-			break;
 
 		case MAV_CMD_NAV_WAYPOINT:
 			return verify_nav_wp();
-			break;
-
-		case MAV_CMD_NAV_LOITER_UNLIM:
-			return verify_loiter_unlim();
-			break;
-
-		case MAV_CMD_NAV_LOITER_TURNS:
-			return verify_loiter_turns();
-			break;
-
-		case MAV_CMD_NAV_LOITER_TIME:
-			return verify_loiter_time();
-			break;
 
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
 			return verify_RTL();
-			break;
 
 		default:
 			gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
 			return false;
-			break;
 	}
 }
 
@@ -220,24 +192,6 @@ static void do_nav_wp()
 	set_next_WP(&next_nav_command);
 }
 
-static void do_loiter_unlimited()
-{
-	set_next_WP(&next_nav_command);
-}
-
-static void do_loiter_turns()
-{
-	set_next_WP(&next_nav_command);
-	loiter_total = next_nav_command.p1 * 360;
-}
-
-static void do_loiter_time()
-{
-	set_next_WP(&next_nav_command);
-	loiter_time = millis();
-	loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
-}
-
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
@@ -254,7 +208,6 @@ static void calc_turn_radius(void)    // JLN update - adjut automaticaly the wp_
 
 static bool verify_nav_wp()
 {
-    hold_course = -1;
     update_crosstrack();
 
     if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
@@ -273,13 +226,7 @@ static bool verify_nav_wp()
         }
     }
 
-    // have we circled around the waypoint?
+    // have we gone past the waypoint?
-    if (loiter_sum > 300) {
-        gcs_send_text_P(SEVERITY_MEDIUM,PSTR("Missed WP"));
-        return true;
-    }
-
-    // have we flown past the waypoint?
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
         gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
                           (unsigned)nav_command_index,
@@ -290,37 +237,6 @@ static bool verify_nav_wp()
     return false;
 }
 
-static bool verify_loiter_unlim()
-{
-	update_loiter();
-	calc_bearing_error();
-	return false;
-}
-
-static bool verify_loiter_time()
-{
-	update_loiter();
-	calc_bearing_error();
-	if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) {		// scale loiter_time_max from (sec*10) to milliseconds
-		gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
-		return true;
-	}
-	return false;
-}
-
-static bool verify_loiter_turns()
-{
-	update_loiter();
-	calc_bearing_error();
-	if(loiter_sum > loiter_total) {
-		loiter_total = 0;
-		gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
-		// clear the command queue;
-		return true;
-	}
-	return false;
-}
-
 static bool verify_RTL()
 {
 	if (wp_distance <= g.waypoint_radius) {
@@ -361,7 +277,7 @@ static void do_within_distance()
 
 static bool verify_wait_delay()
 {
-	if ((unsigned)(millis() - condition_start) > condition_value){
+	if ((uint32_t)(millis() - condition_start) > (uint32_t)condition_value){
 		condition_value 	= 0;
 		return true;
 	}
@@ -390,11 +306,6 @@ static bool verify_within_distance()
 //  Do (Now) commands
 /********************************************************************************/
 
-static void do_loiter_at_location()
-{
-	next_WP = current_loc;
-}
-
 static void do_jump()
 {
 	struct Location temp;

APMrover2/config.h

@@ -649,19 +649,6 @@
 # define WP_RADIUS_DEFAULT		30
 #endif
 
-#ifndef LOITER_RADIUS_DEFAULT
-# define LOITER_RADIUS_DEFAULT 60
-#endif
-
-#ifndef ALT_HOLD_HOME
-# define ALT_HOLD_HOME 100
-#endif
-#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
-
-#ifndef USE_CURRENT_ALT
-# define USE_CURRENT_ALT FALSE
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // Developer Items
 //

APMrover2/defines.h

@@ -12,7 +12,6 @@
 #define ToDeg(x) (x*57.2957795131)	// *180/pi
 
 #define DEBUG 0
-#define LOITER_RANGE 60 // for calculating power outside of loiter radius
 #define SERVO_MAX 4500	// This value represents 45 degrees and is just an arbitrary representation of servo max travel.
 
 // failsafe
@@ -98,7 +97,6 @@
 
 #define AUTO 10
 #define RTL 11
-#define LOITER 12
 #define GUIDED 15
 #define INITIALISING 16     // in startup routines
 #define HEADALT      17   // Lock the current heading and altitude

APMrover2/events.pde

@@ -15,7 +15,6 @@ static void failsafe_short_on_event(int fstype)
 
 		case AUTO: 
 		case GUIDED: 
-		case LOITER: 
 			if(g.short_fs_action == 1) {
 				set_mode(RTL);
 			}
@@ -45,7 +44,6 @@ static void failsafe_long_on_event(int fstype)
 
 		case AUTO: 
 		case GUIDED: 
-		case LOITER: 
 			if(g.long_fs_action == 1) {
 				set_mode(RTL);
 			}

APMrover2/navigation.pde

@@ -13,23 +13,16 @@ static void navigate()
 		return;
 	}
 
-#if HIL_MODE != HIL_MODE_ATTITUDE
 	if ((next_WP.lat == 0)||(home_is_set==false)){
-#else
-	if(next_WP.lat == 0){
-#endif
 		return;
 	}
 
-         if(control_mode < INITIALISING) {
-
 	// waypoint distance from plane
 	// ----------------------------
 	wp_distance = get_distance(&current_loc, &next_WP);
 
 	if (wp_distance < 0){
 		gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
-		//cliSerial->println(wp_distance,DEC);
 		return;
 	}
 
@@ -41,34 +34,12 @@ static void navigate()
 	// ------------------------------------------
 	nav_bearing = target_bearing;
 
-	// check if we have missed the WP
-	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;
-	loiter_sum += abs(loiter_delta);
-                }    
-
 	// control mode specific updates to nav_bearing
 	// --------------------------------------------
 	update_navigation();
 }
 
 
-#if 0
-// Disabled for now
-void calc_distance_error()
-{
-	distance_estimate 	+= (float)ground_speed * .0002 * cos(radians(bearing_error * .01));
-	distance_estimate 	-= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
-	wp_distance  		= max(distance_estimate,10);
-}
-#endif
-
 static void calc_gndspeed_undershoot()
 {
     if (g_gps->status() == GPS::GPS_OK) {
@@ -97,36 +68,6 @@ static long wrap_180(long error)
 	return error;
 }
 
-static void update_loiter()
-{
-	float power;
-
-	if(wp_distance <= g.loiter_radius){
-		power = float(wp_distance) / float(g.loiter_radius);
-		power = constrain(power, 0.5, 1);
-		nav_bearing += (int)(9000.0 * (2.0 + power));
-	}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
-		power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
-		power = constrain(power, 0.5, 1);			//power = constrain(power, 0, 1);
-		nav_bearing -= power * 9000;
-
-	}else{
-		update_crosstrack();
-		loiter_time = millis();			// keep start time for loiter updating till we get within LOITER_RANGE of orbit
-
-	}
-/*
-	if (wp_distance < g.loiter_radius){
-		nav_bearing += 9000;
-	}else{
-		nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
-	}
-
-	update_crosstrack();
-*/
-	nav_bearing = wrap_360(nav_bearing);
-}
-
 static void update_crosstrack(void)
 {
 	// Crosstrack Error

APMrover2/setup.pde

@@ -228,12 +228,11 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
 				mode != CIRCLE &&
 				mode != LEARNING &&
 				mode != AUTO &&
-				mode != RTL &&
+				mode != RTL) 
-				mode != LOITER)
 			{
 				if (mode < MANUAL)
-					mode = LOITER;
+					mode = RTL;
-				else if (mode >LOITER)
+				else if (mode > RTL)
 					mode = MANUAL;
 				else
 					mode += radioInputSwitch;

APMrover2/test.pde

@@ -316,7 +316,6 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 
 	cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
 	cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
-	cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
 
 	for(byte i = 0; i <= g.command_total; i++){
 		struct Location temp = get_cmd_with_index(i);