manual merge of origin/master into fw_control

2 files changed, 20 insertions, 88 deletions

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 8fc272eb6..640edba99 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -95,6 +95,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);
@@ -185,9 +198,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 */
@@ -261,7 +274,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 */
@@ -287,7 +300,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					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 */
 					//TODO: psi rate loop incomplete
@@ -301,8 +314,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 					float psi_rate_e_scaled = psi_rate_e * sqrtf(pow(global_pos.vx,2) + pow(global_pos.vy,2)) / 9.81f; //* V_gr / g
 
-					attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
-
+					attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
 
 //					if (counter % 100 == 0)
 //						printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
@@ -319,7 +331,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 */
@@ -401,64 +413,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;
-}
-
-
-
-