Merged with fixed-wing stabilization work, multirotor control tested

7 files changed, 385 insertions, 143 deletions

diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index ad08247e1..8629c2f11 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
+ *   Modifications: Doug Weibel <douglas.weibel@colorado.edu>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,7 +32,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-// Workflow test comment - DEW
+
 /**
  * @file fixedwing_control.c
  * Implementation of a fixed wing attitude and position controller.
@@ -86,27 +87,61 @@ static void usage(const char *reason);
  * Controller parameters, accessible via MAVLink
  *
  */
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f);
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f);
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f);
+// Roll control parameters
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
+// Need to add functionality to suppress integrator windup while on the ground
+// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f);   // Roll rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f);	// Roll angle limit in radians
+
+//Pitch control parameters
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
+// Need to add functionality to suppress integrator windup while on the ground
+// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f);   // Pitch rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	// Pitch angle limit in radians
 
 struct fw_att_control_params {
-	float roll_pos_p;
-	float roll_pos_i;
-	float roll_pos_awu;
-	float roll_pos_lim;
+	float rollrate_p;
+	float rollrate_i;
+	float rollrate_awu;
+	float rollrate_lim;
+	float roll_p;
+	float roll_lim;
+	float pitchrate_p;
+	float pitchrate_i;
+	float pitchrate_awu;
+	float pitchrate_lim;
+	float pitch_p;
+	float pitch_lim;
 };
 
 struct fw_att_control_param_handles {
-	param_t roll_pos_p;
-	param_t roll_pos_i;
-	param_t roll_pos_awu;
-	param_t roll_pos_lim;
+	param_t rollrate_p;
+	param_t rollrate_i;
+	param_t rollrate_awu;
+	param_t rollrate_lim;
+	param_t roll_p;
+	param_t roll_lim;
+	param_t pitchrate_p;
+	param_t pitchrate_i;
+	param_t pitchrate_awu;
+	param_t pitchrate_lim;
+	param_t pitch_p;
+	param_t pitch_lim;
 };
 
 
-// XXX outsource position control to a separate app some time
+// TO_DO - Navigation control will be moved to a separate app
+// Attitude control will just handle the inner angle and rate loops
+// to control pitch and roll, and turn coordination via rudder and 
+// possibly throttle compensation for battery voltage sag.
 
 PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
 PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f);
@@ -147,9 +182,9 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s
 
 
 /**
- * The fixed wing control main loop.
+ * The fixed wing control main thread.
  *
- * This loop executes continously and calculates the control
+ * The main loop executes continously and calculates the control
  * response.
  *
  * @param argc number of arguments
@@ -223,19 +258,31 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	pos_parameters_init(&hpos);
 	pos_parameters_update(&hpos, &ppos);
 
-	PID_t roll_pos_controller;
-	pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu,
-			PID_MODE_DERIVATIV_SET);
+// TO_DO  Fix output limit functionallity of PID controller or add that function elsewhere
+	PID_t roll_rate_controller;
+	pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu,
+			p.rollrate_lim,PID_MODE_DERIVATIV_NONE);
+	PID_t roll_angle_controller;
+	pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f,
+			p.roll_lim,PID_MODE_DERIVATIV_NONE);
+			
+	PID_t pitch_rate_controller;
+	pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu,
+			p.pitchrate_lim,PID_MODE_DERIVATIV_NONE);
+	PID_t pitch_angle_controller;
+	pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f,
+			p.pitch_lim,PID_MODE_DERIVATIV_NONE);
 
 	PID_t heading_controller;
 	pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
-			PID_MODE_DERIVATIV_SET);
+			100.0f,PID_MODE_DERIVATIV_SET);		// Temporary arbitrarily large limit
 
 	// XXX remove in production
 	/* advertise debug value */
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 
+	// This is the top of the main loop
 	while(!thread_should_exit) {
 
 		struct pollfd fds[1] = {
@@ -251,11 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 		} else {
 
 			// FIXME SUBSCRIBE
-			if (loopcounter % 20 == 0) {
+			if (loopcounter % 100 == 0) {
 				att_parameters_update(&h, &p);
 				pos_parameters_update(&hpos, &ppos);
-				pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu);
-				pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f);
+				pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, 
+									p.rollrate_awu, p.rollrate_lim);
+				pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 
+									0.0f, p.roll_lim);
+				pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, 
+									p.pitchrate_awu, p.pitchrate_lim);
+				pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 
+									0.0f, p.pitch_lim);
+				pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f);
+//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f  \n",
+//p.rollrate_p, p.rollrate_i, p.rollrate_lim);
 			}
 
 			/* if position updated, run position control loop */
@@ -359,14 +415,25 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
 			}
 
-			// actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
-			// 		att.roll, att.rollspeed, deltaTpos);
-						actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
-					att.roll, att.rollspeed, deltaTpos);
-			actuators.control[1] = 0;
-			actuators.control[2] = 0;
+			// actuator control[0] is aileron (or elevon roll control)
+			// Commanded roll rate from P controller on roll angle
+			float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body,
+					att.roll, 0.0f, deltaTpos);
+			// actuator control from PI controller on roll rate
+			actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command,
+					att.rollspeed, 0.0f, deltaTpos);
+					
+			// actuator control[1] is elevator (or elevon pitch control)
+			// Commanded pitch rate from P controller on pitch angle
+			float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body,
+					att.pitch, 0.0f, deltaTpos);
+			// actuator control from PI controller on pitch rate
+			actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command,
+					att.pitchspeed, 0.0f, deltaTpos);
+					
+			// actuator control[3] is throttle
 			actuators.control[3] = att_sp.thrust;
-
+					
 			/* publish attitude setpoint (for MAVLink) */
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
@@ -446,21 +513,38 @@ int fixedwing_control_main(int argc, char *argv[])
 static int att_parameters_init(struct fw_att_control_param_handles *h)
 {
 	/* PID parameters */
-	h->roll_pos_p 	=	param_find("FW_ROLL_POS_P");
-	h->roll_pos_i 	=	param_find("FW_ROLL_POS_I");
-	h->roll_pos_lim =	param_find("FW_ROLL_POS_LIM");
-	h->roll_pos_awu =	param_find("FW_ROLL_POS_AWU");
-
+	
+	h->rollrate_p 			=	param_find("FW_ROLLRATE_P");
+	h->rollrate_i 			=	param_find("FW_ROLLRATE_I");
+	h->rollrate_awu 		=	param_find("FW_ROLLRATE_AWU");
+	h->rollrate_lim 		=	param_find("FW_ROLLRATE_LIM");
+	h->roll_p 				=	param_find("FW_ROLL_P");
+	h->roll_lim 			=	param_find("FW_ROLL_LIM");
+	h->pitchrate_p 			=	param_find("FW_PITCHRATE_P");
+	h->pitchrate_i 			=	param_find("FW_PITCHRATE_I");
+	h->pitchrate_awu 		=	param_find("FW_PITCHRATE_AWU");
+	h->pitchrate_lim 		=	param_find("FW_PITCHRATE_LIM");
+	h->pitch_p 				=	param_find("FW_PITCH_P");
+	h->pitch_lim 			=	param_find("FW_PITCH_LIM");
+	
 	return OK;
 }
 
 static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p)
 {
-	param_get(h->roll_pos_p, &(p->roll_pos_p));
-	param_get(h->roll_pos_i, &(p->roll_pos_i));
-	param_get(h->roll_pos_lim, &(p->roll_pos_lim));
-	param_get(h->roll_pos_awu, &(p->roll_pos_awu));
-
+	param_get(h->rollrate_p, &(p->rollrate_p));
+	param_get(h->rollrate_i, &(p->rollrate_i));
+	param_get(h->rollrate_awu, &(p->rollrate_awu));
+	param_get(h->rollrate_lim, &(p->rollrate_lim));
+	param_get(h->roll_p, &(p->roll_p));
+	param_get(h->roll_lim, &(p->roll_lim));
+	param_get(h->pitchrate_p, &(p->pitchrate_p));
+	param_get(h->pitchrate_i, &(p->pitchrate_i));
+	param_get(h->pitchrate_awu, &(p->pitchrate_awu));
+	param_get(h->pitchrate_lim, &(p->pitchrate_lim));
+	param_get(h->pitch_p, &(p->pitch_p));
+	param_get(h->pitch_lim, &(p->pitch_lim));
+	
 	return OK;
 }
 
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index a08ee5e46..8c0e10fc3 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -60,11 +60,9 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
-
-
-PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); /* 0.025 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
 PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
 PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
 
@@ -78,12 +76,6 @@ struct mc_att_control_params {
 	float yaw_awu;
 	float yaw_lim;
 
-	//	float yawrate_p;
-	//	float yawrate_i;
-	//	float yawrate_d;
-	//	float yawrate_awu;
-	//	float yawrate_lim;
-
 	float att_p;
 	float att_i;
 	float att_d;
@@ -101,12 +93,6 @@ struct mc_att_control_param_handles {
 	param_t yaw_awu;
 	param_t yaw_lim;
 
-	//	param_t yawrate_p;
-	//	param_t yawrate_i;
-	//	param_t yawrate_d;
-	//	param_t yawrate_awu;
-	//	param_t yawrate_lim;
-
 	param_t att_p;
 	param_t att_i;
 	param_t att_d;
@@ -139,12 +125,6 @@ static int parameters_init(struct mc_att_control_param_handles *h)
 	h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
 	h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
 
-	//	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
-	//	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
-	//	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-	//	h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
-	//	h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
-
 	h->att_p 	= 	param_find("MC_ATT_P");
 	h->att_i 	= 	param_find("MC_ATT_I");
 	h->att_d 	= 	param_find("MC_ATT_D");
@@ -165,12 +145,6 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 	param_get(h->yaw_awu, &(p->yaw_awu));
 	param_get(h->yaw_lim, &(p->yaw_lim));
 
-	//	param_get(h->yawrate_p, &(p->yawrate_p));
-	//	param_get(h->yawrate_i, &(p->yawrate_i));
-	//	param_get(h->yawrate_d, &(p->yawrate_d));
-	//	param_get(h->yawrate_awu, &(p->yawrate_awu));
-	//	param_get(h->yawrate_lim, &(p->yawrate_lim));
-
 	param_get(h->att_p, &(p->att_p));
 	param_get(h->att_i, &(p->att_i));
 	param_get(h->att_d, &(p->att_d));
@@ -199,8 +173,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	static int motor_skip_counter = 0;
 
-	// static PID_t yaw_pos_controller;
-//	static PID_t yaw_speed_controller;
 	static PID_t pitch_controller;
 	static PID_t roll_controller;
 
@@ -214,14 +186,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_init(&h);
 		parameters_update(&h, &p);
 
-		// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
-		// 	PID_MODE_DERIVATIV_SET, 154);
-		//		pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
-		//			PID_MODE_DERIVATIV_SET);
 		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
-				PID_MODE_DERIVATIV_SET);
+			p.att_lim, PID_MODE_DERIVATIV_SET);
 		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
-				PID_MODE_DERIVATIV_SET);
+			p.att_lim, PID_MODE_DERIVATIV_SET);
 
 		initialized = true;
 	}
@@ -230,12 +198,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	if (motor_skip_counter % 1000 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
+
 		/* apply parameters */
-		// pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
-		//		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
-		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
 		printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+
+		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim);
+		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim);
 	}
 
 	/* calculate current control outputs */
@@ -251,10 +219,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	/* control yaw rate */
 	rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body));
 
-
-
 	rates_sp->thrust = att_sp->thrust;
 
-
 	motor_skip_counter++;
 }
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index ce46d01cc..bc467bfa3 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -68,10 +68,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
-	double lat_now_rad = lat_now / 180.0 * M_PI;
-	double lon_now_rad = lon_now / 180.0 * M_PI;
-	double lat_next_rad = lat_next / 180.0 * M_PI;
-	double lon_next_rad = lon_next / 180.0 * M_PI;
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+	double lat_next_rad = lat_next * M_DEG_TO_RAD;
+	double lon_next_rad = lon_next * M_DEG_TO_RAD;
 
 	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
@@ -79,13 +79,195 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
 	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
 
-	if (theta < M_PI_F) {
-		theta += 2.0f * M_PI_F;
+	theta = _wrapPI(theta);
+
+	return theta;
+}
+
+// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
+
+__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
+{
+// This function returns the distance to the nearest point on the track line.  Distance is positive if current
+// position is right of the track and negative if left of the track as seen from a point on the track line 
+// headed towards the end point.
+
+	crosstrack_error_s return_var;
+	float dist_to_end;
+	float bearing_end;
+	float bearing_track;
+	float bearing_diff;
+	
+	return_var.error = true;		// Set error flag, cleared when valid result calculated.
+	return_var.past_end = false;
+	return_var.distance = 0.0f;
+	return_var.bearing = 0.0f;
+	
+	// Return error if arguments are bad
+	if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var;
+	
+	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
+	bearing_diff = bearing_track - bearing_end;
+	bearing_diff = _wrapPI(bearing_diff);
+	
+	// Return past_end = true if past end point of line
+	if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
+		return_var.past_end = true;
+		return_var.error = false;
+		return return_var;
 	}
+	
+	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+	return_var.distance = (dist_to_end)*sin(bearing_diff);
+	if(sin(bearing_diff) >=0 ) {
+		return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
+	} else {
+		return_var.bearing = _wrapPI(bearing_track + M_PI_2_F);
+	}
+	return_var.error = false;
+	
+	return return_var;
+
+}
+
+
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, 
+									float radius, float arc_start_bearing, float arc_sweep)
+{
+	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
+	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
+	// headed towards the end point.
+	crosstrack_error_s return_var;
 
-	if (theta > M_PI_F) {
-		theta -= 2.0f * M_PI_F;
+	// Determine if the current position is inside or outside the sector between the line from the center
+	// to the arc start and the line from the center to the arc end
+	float	bearing_sector_start;
+	float	bearing_sector_end;
+	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
+	bool	in_sector;
+	
+	return_var.error = true;		// Set error flag, cleared when valid result calculated.
+	return_var.past_end = false;
+	return_var.distance = 0.0f;
+	return_var.bearing = 0.0f;
+	
+	// Return error if arguments are bad
+	if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var;
+	
+	
+	if(arc_sweep >= 0) {
+		bearing_sector_start = arc_start_bearing;
+		bearing_sector_end = arc_start_bearing + arc_sweep;
+		if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F;
+	} else {
+		bearing_sector_end = arc_start_bearing;
+		bearing_sector_start = arc_start_bearing - arc_sweep;
+		if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F;
 	}
+	in_sector = false;
+	
+	// Case where sector does not span zero
+	if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+	
+	// Case where sector does span zero
+	if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true;
+	
+	// If in the sector then calculate distance and bearing to closest point
+	if(in_sector) {
+		return_var.past_end = false;
+		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
+		
+		if(dist_to_center <= radius) {
+			return_var.distance = radius - dist_to_center;
+			return_var.bearing = bearing_now + M_PI_F;
+		} else {
+			return_var.distance = dist_to_center - radius;
+			return_var.bearing = bearing_now;
+		}
+		
+	// If out of the sector then calculate dist and bearing to start or end point
+	} else {
+	
+	// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude) 
+	// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
+	// calculate the position of the start and end points.  We should not be doing this often
+	// as this function generally will not be called repeatedly when we are out of the sector.
+	
+	// TO DO - this is messed up and won't compile
+		float start_disp_x = radius * sin(arc_start_bearing);
+		float start_disp_y = radius * cos(arc_start_bearing);
+		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep));
+		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep));
+		float lon_start = lon_now + start_disp_x/111111.0d;
+		float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d;
+		float lon_end = lon_now + end_disp_x/111111.0d;
+		float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d;
+		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+		if(dist_to_start < dist_to_end) {
+			return_var.distance = dist_to_start;
+			return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
+		} else {
+			return_var.past_end = true;
+			return_var.distance = dist_to_end;
+			return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+		}
 
-	return theta;
-}
\ No newline at end of file
+	}
+	
+	return_var.bearing = _wrapPI(return_var.bearing);
+	return_var.error = false;
+	return return_var;	
+}
+
+float _wrapPI(float bearing)
+{
+
+	while (bearing > M_PI_F) {
+		bearing = bearing - M_TWOPI_F;
+	}
+	while (bearing <=  -M_PI_F) {
+		bearing = bearing + M_TWOPI_F;
+	}
+	return bearing;
+}
+		
+float _wrap2PI(float bearing)
+{
+
+	while (bearing >= M_TWOPI_F) {
+		bearing = bearing - M_TWOPI_F;
+	}
+	while (bearing <  0.0f) {
+		bearing = bearing + M_TWOPI_F;
+	}
+	return bearing;
+}
+
+float _wrap180(float bearing)
+{
+
+	while (bearing > 180.0f) {
+		bearing = bearing - 360.0f;
+	}
+	while (bearing <=  -180.0f) {
+		bearing = bearing + 360.0f;
+	}
+	return bearing;
+}
+		
+float _wrap360(float bearing)
+{
+
+	while (bearing >= 360.0f) {
+		bearing = bearing - 360.0f;
+	}
+	while (bearing <  0.0f) {
+		bearing = bearing + 360.0f;
+	}
+	return bearing;
+}
+		
+
+	
\ No newline at end of file
diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 0e86b3599..205afc79e 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -42,8 +42,31 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
+ 
+ 
+#include <stdbool.h>
+
+typedef struct {
+	bool error;			// Flag that the calculation failed
+	bool past_end;		// Flag indicating we are past the end of the line/arc segment
+	float distance;		// Distance in meters to closest point on line/arc
+	float bearing;		// Bearing in radians to closest point on line/arc
+} crosstrack_error_s;
 
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+
+// 
+
+__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
+
+__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, 
+									 float radius, float arc_start_bearing, float arc_sweep);
+									 
+float _wrap180(float bearing);
+float _wrap360(float bearing);	 
+float _wrapPI(float bearing);
+float _wrap2PI(float bearing);
\ No newline at end of file
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index cff5e6bbe..f9b50d030 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -43,12 +43,13 @@
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-	      uint8_t mode)
+	      float limit, uint8_t mode)
 {
 	pid->kp = kp;
 	pid->ki = ki;
 	pid->kd = kd;
 	pid->intmax = intmax;
+	pid->limit = limit;
 	pid->mode = mode;
 	pid->count = 0;
 	pid->saturated = 0;
@@ -58,7 +59,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->error_previous = 0;
 	pid->integral = 0;
 }
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
 {
 	int ret = 0;
 
@@ -85,8 +86,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
 	}  else {
 		ret = 1;
 	}
+	
+	if (isfinite(limit)) {
+		pid->limit = limit;
+	}  else {
+		ret = 1;
+	}
 
-	// pid->limit = limit;
 	return ret;
 }
 
@@ -122,72 +128,48 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 	float i, d;
 	pid->sp = sp;
+	
+	// Calculated current error value
 	float error = pid->sp - val;
-
-	if (pid->saturated && (pid->integral * error > 0)) {
-		//Output is saturated and the integral would get bigger (positive or negative)
-		i = pid->integral;
-
-		//Reset saturation. If we are still saturated this will be set again at output limit check.
-		pid->saturated = 0;
-
-	} else {
-		i = pid->integral + (error * dt);
+	
+	if (isfinite(error)) {				// Why is this necessary?  DEW
+		pid->error_previous = error;
 	}
 
-	// Anti-Windup. Needed if we don't use the saturation above.
-	if (pid->intmax != 0.0f) {
-		if (i > pid->intmax) {
-			pid->integral = pid->intmax;
-
-		} else if (i < -pid->intmax) {
-
-			pid->integral = -pid->intmax;
-
-		} else {
-			pid->integral = i;
-		}
-	}
+	// Calculate or measured current error derivative
 
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
 		d = (error - pid->error_previous) / dt;
-
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
-
 	} else {
 		d = 0.0f;
 	}
 
-	if (pid->kd == 0.0f) {
-		d = 0.0f;
-	}
-
-	if (pid->ki == 0.0f) {
-		i = 0;
-	}
-
-	float p;
-
-	if (pid->kp == 0.0f) {
-		p = 0.0f;
+	// Calculate the error integral and check for saturation
+	i = pid->integral + (error * dt);
+	if( fabs((error * pid->kp) + (i * pid->ki) +  (d * pid->kd)) > pid->limit ||
+		fabs(i) > pid->intmax )
+	{
+		i = pid->integral;		// If saturated then do not update integral value
+		pid->saturated = 1;
 	} else {
-		p = error;
-	}
-
-	if (isfinite(error)) {
-		pid->error_previous = error;
+		if (!isfinite(i)) {
+			i = 0;
+		}
+		pid->integral = i;
+		pid->saturated = 0;
 	}
 
+	// Calculate the output.  Limit output magnitude to pid->limit
 	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	if (output > pid->limit) output = pid->limit;
+	if (output < -pid->limit) output = -pid->limit;
 
 	if (isfinite(output)) {
 		pid->last_output = output;
 	}
 
-	if (!isfinite(pid->integral)) {
-		pid->integral = 0;
-	}
 
 	return pid->last_output;
 }
diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h
index 098b2bef8..b51afef9e 100644
--- a/apps/systemlib/pid/pid.h
+++ b/apps/systemlib/pid/pid.h
@@ -49,6 +49,8 @@
 #define PID_MODE_DERIVATIV_CALC	0
 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
 #define PID_MODE_DERIVATIV_SET	1
+// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
+#define PID_MODE_DERIVATIV_NONE 9
 
 typedef struct {
 	float kp;
@@ -65,8 +67,8 @@ typedef struct {
 	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
 
diff --git a/nuttx/arch/arm/include/math.h b/nuttx/arch/arm/include/math.h
index ef0869243..9de10c58b 100644
--- a/nuttx/arch/arm/include/math.h
+++ b/nuttx/arch/arm/include/math.h
@@ -544,6 +544,8 @@ extern int matherr _PARAMS((struct exception *e));
 #define M_1_PI		0.31830988618379067154
 #define M_2_PI		0.63661977236758134308
 #define M_2_SQRTPI	1.12837916709551257390
+#define M_DEG_TO_RAD 	0.01745329251994
+#define M_RAD_TO_DEG 	57.2957795130823
 #define M_SQRT2		1.41421356237309504880
 #define M_SQRT1_2	0.70710678118654752440
 #define M_LN2LO         1.9082149292705877000E-10
@@ -568,6 +570,8 @@ extern int matherr _PARAMS((struct exception *e));
 #define M_1_PI_F		0.31830988618379067154f
 #define M_2_PI_F		0.63661977236758134308f
 #define M_2_SQRTPI_F	1.12837916709551257390f
+#define M_DEG_TO_RAD_F 	0.01745329251994f
+#define M_RAD_TO_DEG_F 	57.2957795130823f
 #define M_SQRT2_F		1.41421356237309504880f
 #define M_SQRT1_2_F		0.70710678118654752440f
 #define M_LN2LO_F       1.9082149292705877000E-10f