Convert fixed wing example to quaternions

cmake/configs/posix_sitl_default.cmake

@@ -77,6 +77,7 @@ set(config_module_list
 	examples/mc_pos_control_multiplatform
 	examples/ekf_att_pos_estimator
 	examples/attitude_estimator_ekf
+	examples/fixedwing_control
 
 	#
 	# Testing

src/examples/fixedwing_control/CMakeLists.txt

@@ -36,7 +36,7 @@ px4_add_module(
 	STACK_MAIN 1200
 	STACK_MAX 1300
 	SRCS
-		main.c
+		main.cpp
 		params.c
 	DEPENDS
 		platforms__common

src/examples/fixedwing_control/main.cpp

@@ -71,11 +71,25 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 
+#include <matrix/math.hpp>
+
 /* process-specific header files */
 #include "params.h"
 
 /* Prototypes */
 
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+extern "C" int parameters_init(struct param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+extern "C" int parameters_update(const struct param_handles *h, struct params *p);
+
 /**
  * Daemon management function.
  *
@@ -85,7 +99,7 @@
  * the command line to one particular process or the need for bg/fg
  * ^Z support by the shell.
  */
-__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
+extern "C" __EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
 
 /**
  * Mainloop of daemon.
@@ -162,13 +176,20 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	/*
 	 * Calculate roll error and apply P gain
 	 */
-	float roll_err = att->roll - att_sp->roll_body;
+
+	matrix::Quaternion<float> qa(&att->q[0]);
+	matrix::Euler<float> att_euler(qa);
+
+	matrix::Quaternion<float> qd(&att_sp->q_d[0]);
+	matrix::Euler<float> att_sp_euler(qd);
+
+	float roll_err = att_euler(0) - att_sp_euler(0);
 	actuators->control[0] = roll_err * p.roll_p;
 
 	/*
 	 * Calculate pitch error and apply P gain
 	 */
-	float pitch_err = att->pitch - att_sp->pitch_body;
+	float pitch_err = att_euler(1) - att_sp_euler(1);
 	actuators->control[1] = pitch_err * p.pitch_p;
 }
 
@@ -182,18 +203,30 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p
 
 	float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
 
+	matrix::Quaternion<float> qa(&att->q[0]);
+	matrix::Euler<float> att_euler(qa);
+
 	/* calculate heading error */
-	float yaw_err = att->yaw - bearing;
+	float yaw_err = att_euler(2) - bearing;
 	/* apply control gain */
-	att_sp->roll_body = yaw_err * p.hdng_p;
+	float roll_body = yaw_err * p.hdng_p;
 
 	/* limit output, this commonly is a tuning parameter, too */
-	if (att_sp->roll_body < -0.6f) {
+	if (roll_body < -0.6f) {
-		att_sp->roll_body = -0.6f;
+		roll_body = -0.6f;
 
 	} else if (att_sp->roll_body > 0.6f) {
-		att_sp->roll_body = 0.6f;
+		roll_body = 0.6f;
 	}
+
+	matrix::Euler<float> att_spe(roll_body, 0, bearing);
+
+	matrix::Quaternion<float> qd(att_spe);
+
+	att_sp->q_d[0] = qd(0);
+	att_sp->q_d[1] = qd(1);
+	att_sp->q_d[2] = qd(2);
+	att_sp->q_d[3] = qd(3);
 }
 
 /* Main Thread */
@@ -262,7 +295,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 
 	/* publish actuator controls with zero values */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
 		actuators.control[i] = 0.0f;
 	}
 
@@ -380,7 +413,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	thread_running = false;
 
 	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
 		actuators.control[i] = 0.0f;
 	}
 
@@ -456,6 +489,3 @@ int ex_fixedwing_control_main(int argc, char *argv[])
 	usage("unrecognized command");
 	exit(1);
 }
-
-
-

src/examples/fixedwing_control/params.c

@@ -57,6 +57,14 @@ PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);
  */
 PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);
 
+int parameters_init(struct param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct param_handles *h, struct params *p);
+
 int parameters_init(struct param_handles *h)
 {
 	/* PID parameters */
@@ -64,7 +72,7 @@ int parameters_init(struct param_handles *h)
 	h->roll_p 	=	param_find("EXFW_ROLL_P");
 	h->pitch_p 	=	param_find("EXFW_PITCH_P");
 
-	return OK;
+	return 0;
 }
 
 int parameters_update(const struct param_handles *h, struct params *p)
@@ -73,5 +81,5 @@ int parameters_update(const struct param_handles *h, struct params *p)
 	param_get(h->roll_p, &(p->roll_p));
 	param_get(h->pitch_p, &(p->pitch_p));
 
-	return OK;
+	return 0;
 }

src/examples/fixedwing_control/params.h

@@ -51,15 +51,3 @@ struct param_handles {
 	param_t roll_p;
 	param_t pitch_p;
 };
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct param_handles *h, struct params *p);