Cleaned up PI wrapping code, still subject to testing

10 files changed, 120 insertions, 173 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c
index 18b290f99..334b95226 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c
@@ -143,10 +143,10 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	}
 
 	/* Roll (P) */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0);
+	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
 
 	/* Pitch (P) */
-	float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan;
+	float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body;
 	rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
 
 	/* Yaw (from coordinated turn constraint or lateral force) */
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h
index f631c90a1..90d717d9f 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control.h
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h
@@ -42,26 +42,7 @@
 #ifndef FIXEDWING_POS_CONTROL_H_
 #define FIXEDWING_POS_CONTROL_H_
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
 
-#endif 
-
-
-struct planned_path_segments_s {
-	bool segment_type;
-	double start_lat;			// Start of line or center of arc
-	double start_lon;
-	double end_lat;
-	double end_lon;
-	float radius;				// Radius of arc
-	float arc_start_bearing;		// Bearing from center to start of arc
-	float arc_sweep;				// Angle (radians) swept out by arc around center.  
-								// Positive for clockwise, negative for counter-clockwise
-};
 
 
 float _wrap180(float bearing);
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
index 443048913..a70b9bd30 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -91,6 +91,19 @@ struct fw_pos_control_param_handles {
 };
 
 
+struct planned_path_segments_s {
+	bool segment_type;
+	double start_lat;			// Start of line or center of arc
+	double start_lon;
+	double end_lat;
+	double end_lon;
+	float radius;				// Radius of arc
+	float arc_start_bearing;		// Bearing from center to start of arc
+	float arc_sweep;				// Angle (radians) swept out by arc around center.  
+								// Positive for clockwise, negative for counter-clockwise
+};
+
+
 /* Prototypes */
 /* Internal Prototypes */
 static int parameters_init(struct fw_pos_control_param_handles *h);
@@ -177,9 +190,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 		memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
 
 		/* publish attitude setpoint */
-		attitude_setpoint.roll_tait_bryan = 0.0f;
-		attitude_setpoint.pitch_tait_bryan = 0.0f;
-		attitude_setpoint.yaw_tait_bryan = 0.0f;
+		attitude_setpoint.roll_body = 0.0f;
+		attitude_setpoint.pitch_body = 0.0f;
+		attitude_setpoint.yaw_body = 0.0f;
 		orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
 
 		/* subscribe */
@@ -243,7 +256,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 				global_sp_updated_set_once = true;
 				psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
 						(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-				printf("psi_track: %0.4f\n", psi_track);
+				printf("psi_track: %0.4f\n", (double)psi_track);
 			}
 
 			/* Control */
@@ -265,21 +278,21 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 					float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
 
-					if(delta_psi_c > 60.0f*M_DEG_TO_RAD)
-						delta_psi_c = 60.0f*M_DEG_TO_RAD;
+					if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
+						delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
 
-					if(delta_psi_c < -60.0f*M_DEG_TO_RAD)
-						delta_psi_c = -60.0f*M_DEG_TO_RAD;
+					if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
+						delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
 
 					float psi_c = psi_track + delta_psi_c;
 
 					float psi_e = psi_c - att.yaw;
 
 					/* shift error to prevent wrapping issues */
-					psi_e = _wrapPI(psi_e);
+					psi_e = _wrap_pi(psi_e);
 
 					/* calculate roll setpoint, do this artificially around zero */
-					attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
+					attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
 
 //					if (counter % 100 == 0)
 //						printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
@@ -296,7 +309,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 			{
 
 				//TODO: take care of relative vs. ab. altitude
-				attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
+				attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
 
 			}
 			/*Publish the attitude setpoint */
@@ -378,64 +391,3 @@ int fixedwing_pos_control_main(int argc, char *argv[])
 	usage("unrecognized command");
 	exit(1);
 }
-
-
-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;
-}
-
-
-
-
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 91439d36d..10d155ffc 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -252,7 +252,7 @@ mc_thread_main(int argc, char *argv[])
 						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
 					 	    state.flag_control_manual_enabled != flag_control_manual_enabled ||
 					 	    state.flag_system_armed != flag_system_armed) {
-							att_sp.yaw_tait_bryan = att.yaw;
+							att_sp.yaw_body = att.yaw;
 						}
 
 						static bool rc_loss_first_time = true;
@@ -283,29 +283,28 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.roll_body = manual.roll;
 							att_sp.pitch_body = manual.pitch;
 
+							/* set attitude if arming */
+							if (!flag_control_attitude_enabled && state.flag_system_armed) {
+								att_sp.yaw_body = att.yaw;
+							}
+
 							/* only move setpoint if manual input is != 0 */
 
 							if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) {
-								// XXX turn into param
-								if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-									att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
-								} else if (manual.throttle <= 0.3f) {
-									att_sp.yaw_body = att.yaw;
-								}
-								control_yaw_position = true;
-							} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
 								if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
 									rates_sp.yaw = manual.yaw;
 									control_yaw_position = false;
 									first_time_after_yaw_speed_control = true;
 								} else {
-									rates_sp.yaw = 0.0f;
-									if(first_time_after_yaw_speed_control) {
-										att_sp.yaw_tait_bryan = att.yaw;
+									if (first_time_after_yaw_speed_control) {
+										att_sp.yaw_body = att.yaw;
 										first_time_after_yaw_speed_control = false;
 									}
 									control_yaw_position = true;
 								}
+							} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
+								rates_sp.yaw = manual.yaw;
+								control_yaw_position = false;
 							}
 
 							att_sp.thrust = manual.throttle;
@@ -330,9 +329,7 @@ mc_thread_main(int argc, char *argv[])
 
 				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 				if (state.flag_control_attitude_enabled) {
-				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL, control_yaw_position);
-
-
+				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
 
 				 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 87eeaced5..e94d7c55d 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators, bool control_yaw_position)
+		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -201,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 
-		//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));
-
 		/* apply parameters */
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
@@ -220,15 +218,19 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	if(control_yaw_position) {
 		/* control yaw rate */
-		//rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed);
-		yaw_error = att->yaw - att_sp->yaw_tait_bryan;
-		if ((double)yaw_error > M_PI) {
-			yaw_error -= M_PI;
-		} else if ((double)yaw_error < -M_PI) {
-			yaw_error += M_PI;
+
+		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
+		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
+
+		yaw_error = att_sp->yaw_body - att->yaw;
+
+		if (yaw_error > M_PI_F) {
+			yaw_error -= M_TWOPI_F;
+		} else if (yaw_error < -M_PI_F) {
+			yaw_error += M_TWOPI_F;
 		}
 
-		rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
+		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
 	}
 	rates_sp->thrust = att_sp->thrust;
 
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index f9003e9b4..abc94d617 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
@@ -52,6 +52,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *, bool control_yaw_position);
+	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index aa1362d3a..466284a1b 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1059,7 +1059,7 @@ Sensors::task_main()
 
 	/* advertise the manual_control topic */
 	struct manual_control_setpoint_s manual_control;
-	manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
+	manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS;
 	manual_control.roll = 0.0f;
 	manual_control.pitch = 0.0f;
 	manual_control.yaw = 0.0f;
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index 2f4b37e79..6746e8ff3 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -60,14 +60,7 @@ static double cos_phi_1;
 static double lambda_0;
 static double scale;
 
-/**
- * Initializes the map transformation.
- *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
 	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
 	phi_1 = lat_0 / 180.0 * M_PI;
@@ -105,14 +98,7 @@ __EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lo
 
 }
 
-/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y)
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
 {
 	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
 	double phi = lat / 180.0 * M_PI;
@@ -135,15 +121,7 @@ __EXPORT static void map_projection_project(double lat, double lon, float *x, fl
 //	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
 }
 
-/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon)
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
 {
 	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
 
@@ -228,7 +206,7 @@ __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));
 
-	theta = _wrapPI(theta);
+	theta = _wrap_pi(theta);
 
 	return theta;
 }
@@ -257,7 +235,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	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);
+	bearing_diff = _wrap_pi(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) {
@@ -270,10 +248,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
 
 	if (sin(bearing_diff) >= 0) {
-		crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F);
+		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
 
 	} else {
-		crosstrack_error->bearing = _wrapPI(bearing_track + M_PI_2_F);
+		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
 	}
 
 	return_value = OK;
@@ -380,22 +358,36 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	return return_value;
 }
 
-float _wrapPI(float bearing)
+__EXPORT float _wrap_pi(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing) || bearing == 0) {
+		return bearing;
+	}
 
-	while (bearing > M_PI_F) {
-		bearing = bearing - M_TWOPI_F;
+	int c = 0;
+
+	while (bearing > M_PI_F && c < 30) {
+		bearing -= M_TWOPI_F;
+		c++;
 	}
 
-	while (bearing <=  -M_PI_F) {
-		bearing = bearing + M_TWOPI_F;
+	c = 0;
+
+	while (bearing <=  -M_PI_F && c < 30) {
+		bearing += M_TWOPI_F;
+		c++;
 	}
 
 	return bearing;
 }
 
-float _wrap2PI(float bearing)
+__EXPORT float _wrap_2pi(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
 
 	while (bearing >= M_TWOPI_F) {
 		bearing = bearing - M_TWOPI_F;
@@ -408,8 +400,12 @@ float _wrap2PI(float bearing)
 	return bearing;
 }
 
-float _wrap180(float bearing)
+__EXPORT float _wrap_180(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
 
 	while (bearing > 180.0f) {
 		bearing = bearing - 360.0f;
@@ -422,8 +418,12 @@ float _wrap180(float bearing)
 	return bearing;
 }
 
-float _wrap360(float bearing)
+__EXPORT float _wrap_360(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
 
 	while (bearing >= 360.0f) {
 		bearing = bearing - 360.0f;
diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 7aad79a8c..0c0b5c533 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -54,24 +54,44 @@ struct crosstrack_error_s {
 	float bearing;		// Bearing in radians to closest point on line/arc
 } ;
 
-__EXPORT static void map_projection_init(double lat_0, double lon_0);
+/**
+ * Initializes the map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_init(double lat_0, double lon_0);
 
-__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y);
+/**
+ * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
 
-__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon);
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
 
 __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 int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, 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);
+__EXPORT float _wrap_180(float bearing);
+__EXPORT float _wrap_360(float bearing);
+__EXPORT float _wrap_pi(float bearing);
+__EXPORT float _wrap_2pi(float bearing);
diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h
index 8663846fc..a7cda34a8 100644
--- a/apps/uORB/topics/vehicle_attitude_setpoint.h
+++ b/apps/uORB/topics/vehicle_attitude_setpoint.h
@@ -56,18 +56,13 @@ struct vehicle_attitude_setpoint_s
 {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
-	float roll_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float pitch_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float yaw_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	//float tait_bryan_valid;			/**< Set to true if Tait-Bryan angles are valid */
-
 	float roll_body;				/**< body angle in NED frame		*/
 	float pitch_body;				/**< body angle in NED frame		*/
 	float yaw_body;					/**< body angle in NED frame		*/
 	//float body_valid;				/**< Set to true if body angles are valid */
 
-	//float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
-	//bool R_valid;					/**< Set to true if rotation matrix is valid */
+	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
+	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
 	float thrust;					/**< Thrust in Newton the power system should generate */