Fix build breakage in FW control example

1 files changed, 22 insertions, 131 deletions

diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 067d77364..6a5cbcd30 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,6 +30,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 /**
  * @file main.c
  *
@@ -55,7 +55,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -106,11 +106,9 @@ static void usage(const char *reason);
  *
  * @param att_sp The current attitude setpoint - the values the system would like to reach.
  * @param att The current attitude. The controller should make the attitude match the setpoint
- * @param speed_body The velocity of the system. Currently unused.
  * @param rates_sp The angular rate setpoint. This is the output of the controller.
  */
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, 
+void 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);
 
 /**
@@ -125,7 +123,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
  * @param att The current attitude
  * @param att_sp The attitude setpoint. This is the output of the controller
  */
-void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
 	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
 
 /* Variables */
@@ -135,8 +133,7 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 static struct params p;
 static struct param_handles ph;
 
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, 
+void 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)
 {
 
@@ -173,7 +170,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	actuators->control[1] = pitch_err * p.pitch_p;
 }
 
-void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
 	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
 {
 
@@ -186,7 +183,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
 	/* calculate heading error */
 	float yaw_err = att->yaw - bearing;
 	/* apply control gain */
-	float roll_command = yaw_err * p.hdng_p;
+	att_sp->roll_body = yaw_err * p.hdng_p;
 
 	/* limit output, this commonly is a tuning parameter, too */
 	if (att_sp->roll_body < -0.6f) {
@@ -253,7 +250,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	memset(&manual_sp, 0, sizeof(manual_sp));
 	struct vehicle_status_s vstatus;
 	memset(&vstatus, 0, sizeof(vstatus));
-	struct vehicle_global_position_setpoint_s global_sp;
+	struct position_setpoint_s global_sp;
 	memset(&global_sp, 0, sizeof(global_sp));
 
 	/* output structs - this is what is sent to the mixer */
@@ -275,17 +272,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 	/* subscribe to topics. */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 
 	/* Setup of loop */
-	float speed_body[3] = {0.0f, 0.0f, 0.0f};
-	/* RC failsafe check */
-	bool throttle_half_once = false;
+
 	struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
 				{ .fd = att_sub, .events = POLLIN }};
 
@@ -339,25 +333,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				/* get a local copy of attitude */
 				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 
-				if (global_sp_updated)
-					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
-
-				/* currently speed in body frame is not used, but here for reference */
-				if (pos_updated) {
-					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-
-					if (att.R_valid) {
-						speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
-						speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
-						speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
-
-					} else {
-						speed_body[0] = 0;
-						speed_body[1] = 0;
-						speed_body[2] = 0;
-
-						warnx("Did not get a valid R\n");
-					}
+				if (global_sp_updated) {
+					struct position_setpoint_triplet_s triplet;
+					orb_copy(ORB_ID(position_setpoint_triplet), global_sp_sub, &triplet);
+					memcpy(&global_sp, &triplet.current, sizeof(global_sp));
 				}
 
 				if (manual_sp_updated)
@@ -365,106 +344,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
 
 				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
-				if (isfinite(manual_sp.throttle) &&
-							    (manual_sp.throttle >= 0.6f) &&
-							    (manual_sp.throttle <= 1.0f)) {
-					throttle_half_once = true;
+				if (isfinite(manual_sp.z) &&
+							    (manual_sp.z >= 0.6f) &&
+							    (manual_sp.z <= 1.0f)) {
 				}
 
 				/* get the system status and the flight mode we're in */
 				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 
-				/* control */
-
-#warning fix this
-#if 0
-				if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
-				    vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
-
-				    	/* simple heading control */
-				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
-
-				    	/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
-				    	actuators.control[1] = 0.0f;
-				    	actuators.control[2] = 0.0f;
-					
-					/* simple attitude control */
-					control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
-
-					/* pass through throttle */
-					actuators.control[3] = att_sp.thrust;
-
-					/* set flaps to zero */
-					actuators.control[4] = 0.0f;
-
-				} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
-				/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
-				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (vstatus.rc_signal_lost && throttle_half_once) {
-
-							/* put plane into loiter */
-							att_sp.roll_body = 0.3f;
-							att_sp.pitch_body = 0.0f;
-
-							/* limit throttle to 60 % of last value if sane */
-							if (isfinite(manual_sp.throttle) &&
-							    (manual_sp.throttle >= 0.0f) &&
-							    (manual_sp.throttle <= 1.0f)) {
-								att_sp.thrust = 0.6f * manual_sp.throttle;
-
-							} else {
-								att_sp.thrust = 0.0f;
-							}
-
-							att_sp.yaw_body = 0;
-
-							// XXX disable yaw control, loiter
-
-						} else {
-
-							att_sp.roll_body = manual_sp.roll;
-							att_sp.pitch_body = manual_sp.pitch;
-							att_sp.yaw_body = 0;
-							att_sp.thrust = manual_sp.throttle;
-						}
-
-						att_sp.timestamp = hrt_absolute_time();
-
-						/* attitude control */
-						control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
-
-						/* pass through throttle */
-						actuators.control[3] = att_sp.thrust;
-
-						/* pass through flaps */
-						if (isfinite(manual_sp.flaps)) {
-							actuators.control[4] = manual_sp.flaps;
-
-						} else {
-							actuators.control[4] = 0.0f;
-						}
-
-					} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-						/* directly pass through values */
-						actuators.control[0] = manual_sp.roll;
-						/* positive pitch means negative actuator -> pull up */
-						actuators.control[1] = manual_sp.pitch;
-						actuators.control[2] = manual_sp.yaw;
-						actuators.control[3] = manual_sp.throttle;
-
-						if (isfinite(manual_sp.flaps)) {
-							actuators.control[4] = manual_sp.flaps;
-
-						} else {
-							actuators.control[4] = 0.0f;
-						}
-					}
-				}
-#endif
-
 				/* publish rates */
 				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
 
@@ -474,6 +361,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				    isfinite(actuators.control[2]) &&
 				    isfinite(actuators.control[3])) {
 					orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+					if (verbose) {
+						warnx("published");
+					}
 				}
 			}
 		}