Halfway-working fixed wing waypoint control, needs more effort

1 files changed, 266 insertions, 742 deletions

diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
index 67e5d1b28..1323f2566 100644
--- a/apps/fixedwing_control/fixedwing_control.c
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -44,16 +44,12 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
-#include <debug.h>
 #include <math.h>
-#include <termios.h>
+#include <poll.h>
 #include <time.h>
 #include <arch/board/up_hrt.h>
 #include <arch/board/board.h>
-#include <drivers/drv_pwm_output.h>
-#include <nuttx/spi.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -62,6 +58,8 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <systemlib/param/param.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/geo/geo.h>
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
@@ -82,638 +80,75 @@ int fixedwing_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- * 
- * The actual stack size should be set in the call
- * to task_create().
- */
-int fixedwing_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("fixedwing_control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tfixedwing_control is running\n");
-		} else {
-			printf("\tfixedwing_control not started\n");
-		}
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-#define PID_DT 0.01f
-#define PID_SCALER 0.1f
-#define PID_DERIVMODE_CALC 0
-#define HIL_MODE 32
-#define AUTO -1000
-#define MANUAL 3000
-#define SERVO_MIN 1000
-#define SERVO_MAX 2000
-
-pthread_t control_thread;
-pthread_t nav_thread;
-pthread_t servo_thread;
-
-/**
- * Servo channels function enumerator used for
- * the servo writing part
- */
-enum SERVO_CHANNELS_FUNCTION {
-
-	AIL_1    = 0,
-	AIL_2    = 1,
-	MOT      = 2,
-	ACT_1    = 3,
-	ACT_2    = 4,
-	ACT_3    = 5,
-	ACT_4    = 6,
-	ACT_5    = 7
-};
-
-/**
- * The plane_data structure.
- *
- * The plane data structure is the local storage of all the flight information of the aircraft
- */
-typedef struct {
-	double lat;
-	double lon;
-	float alt;
-	float vx;
-	float vy;
-	float vz;
-	float yaw;
-	float hdg;
-	float pitch;
-	float roll;
-	float yawspeed;
-	float pitchspeed;
-	float rollspeed;
-	float rollb;	/* body frame angles */
-	float pitchb;
-	float yawb;
-	float p;
-	float q;
-	float r;	/* body angular rates */
-
-	/* PID parameters*/
-
-	float Kp_att;
-	float Ki_att;
-	float Kd_att;
-	float Kp_pos;
-	float Ki_pos;
-	float Kd_pos;
-	float intmax_att;
-	float intmax_pos;
-
-	/* Next waypoint*/
-
-	double wp_x;
-	double wp_y;
-	float wp_z;
-
-	/* Setpoints */
-
-	float airspeed;
-	float groundspeed;
-	float roll_setpoint;
-	float pitch_setpoint;
-	float throttle_setpoint;
-
-	/* Navigation mode*/
-	int mode;
-
-} plane_data_t;
-
-/**
- * The control_outputs structure.
- *
- * The control outputs structure contains the control outputs
- * of the aircraft
- */
-typedef struct {
-	float roll_ailerons;
-	float pitch_elevator;
-	float yaw_rudder;
-	float throttle;
-	// set the aux values to 0 per default
-	float aux1;
-	float aux2;
-	float aux3;
-	float aux4;
-	uint8_t mode;	// HIL_ENABLED: 32
-	uint8_t nav_mode;
-} control_outputs_t;
-
-/**
- * Generic PID algorithm with PD scaling
- */
-static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax);
-
 /*
- * Output calculations
- */
-
-static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed);
-static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z);
-static void calc_bodyframe_angles(float roll, float pitch, float yaw);
-static float calc_bearing(void);
-static float calc_roll_ail(void);
-static float calc_pitch_elev(void);
-static float calc_yaw_rudder(float hdg);
-static float calc_throttle(void);
-static float calc_gnd_speed(void);
-static void get_parameters(plane_data_t *plane_data);
-static float calc_roll_setpoint(void);
-static float calc_pitch_setpoint(void);
-static float calc_throttle_setpoint(void);
-static float calc_wp_distance(void);
-static void set_plane_mode(void);
-
-/****************************************************************************
- * Public Data
- ****************************************************************************/
-
-plane_data_t plane_data;
-control_outputs_t control_outputs;
-float scaler = 1; //M_PI;
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-/**
- * Calculates the PID control output given an error.
- * 
- * Incorporates PD scaling and low-pass filter for the derivative component.
- *
- * @param error the input error
- * @param error_deriv the derivative of the input error
- * @param dt time constant
- * @param scaler PD scaler
- * @param K_p P gain
- * @param K_i I gain
- * @param K_d D gain
- * @param intmax Integration limit
- *
- * @return the PID control output
- */
-
-static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax)
-{
-	float Kp = K_p;
-	float Ki = K_i;
-	float Kd = K_d;
-	float delta_time = dt;
-	float lerror;
-	float imax = intmax;
-	float integrator;
-	float derivative;
-	float lderiv;
-	int fCut = 20;		/* anything above 20 Hz is considered noise - low pass filter for the derivative */
-	float output = 0;
-
-	output += error * Kp;
-
-	if ((fabsf(Kd) > 0) && (dt > 0)) {
-
-		if (PID_DERIVMODE_CALC) {
-			derivative = (error - lerror) / delta_time;
-
-			/*
-			 * discrete low pass filter, cuts out the
-			 * high frequency noise that can drive the controller crazy
-			 */
-			float RC = 1.0f / (2.0f * M_PI_F * fCut);
-			derivative = lderiv +
-				     (delta_time / (RC + delta_time)) * (derivative - lderiv);
-
-			/* update state */
-			lerror 	= error;
-			lderiv  = derivative;
-
-		} else {
-			derivative = error_deriv;
-		}
-
-		/* add in derivative component */
-		output 	+= Kd * derivative;
-	}
-
-	//printf("PID derivative %i\n", (int)(1000*derivative));
-
-	/* scale the P and D components with the PD scaler */
-	output *= scale;
-
-	/* Compute integral component if time has elapsed */
-	if ((fabsf(Ki) > 0) && (dt > 0)) {
-		integrator 		+= (error * Ki) * scaler * delta_time;
-
-		if (integrator < -imax) {
-			integrator = -imax;
-
-		} else if (integrator > imax) {
-			integrator = imax;
-		}
-
-		output += integrator;
-	}
-
-	//printf("PID Integrator %i\n", (int)(1000*integrator));
-
-	return output;
-}
-
-PARAM_DEFINE_FLOAT(FW_ATT_P, 2.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_D, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_AWU, 5.0f);
-PARAM_DEFINE_FLOAT(FW_ATT_LIM, 10.0f);
-
-PARAM_DEFINE_FLOAT(FW_POS_P, 2.0f);
-PARAM_DEFINE_FLOAT(FW_POS_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_POS_D, 0.0f);
-PARAM_DEFINE_FLOAT(FW_POS_AWU, 5.0f);
-PARAM_DEFINE_FLOAT(FW_POS_LIM, 10.0f);
-
-/**
- * Load parameters from global storage.
- *
- * @param plane_data Fixed wing data structure
- *
- * Fetches the current parameters from the global parameter storage and writes them
- * to the plane_data structure
- */
-static void get_parameters(plane_data_t * pdata)
-{
-	static bool initialized = false;
-	static param_t att_p;
-	static param_t att_i;
-	static param_t att_d;
-	static param_t att_awu;
-	static param_t att_lim;
-
-	static param_t pos_p;
-	static param_t pos_i;
-	static param_t pos_d;
-	static param_t pos_awu;
-	static param_t pos_lim;
-
-	if (!initialized) {
-		att_p = param_find("FW_ATT_P");
-		att_i = param_find("FW_ATT_I");
-		att_d = param_find("FW_ATT_D");
-		att_awu = param_find("FW_ATT_AWU");
-		att_lim = param_find("FW_ATT_LIM");
-
-		pos_p = param_find("FW_POS_P");
-		pos_i = param_find("FW_POS_I");
-		pos_d = param_find("FW_POS_D");
-		pos_awu = param_find("FW_POS_AWU");
-		pos_lim = param_find("FW_POS_LIM");
-
-		initialized = true;
-	}
-
-	param_get(att_p, &(pdata->Kp_att));
-	param_get(att_i, &(pdata->Ki_att));
-	param_get(att_d, &(pdata->Kd_att));
-	param_get(pos_p, &(pdata->Kp_pos));
-	param_get(pos_i, &(pdata->Ki_pos));
-	param_get(pos_d, &(pdata->Kd_pos));
-	param_get(att_awu, &(pdata->intmax_att));
-	param_get(pos_awu, &(pdata->intmax_pos));
-	pdata->airspeed = 10;
-	pdata->mode = 1;
-}
-
-/**
- * Calculates the body angular rates.
- *
- * Calculates the rates of the plane using inertia matrix and
- * writes them to the plane_data structure
- *
- * @param roll
- * @param pitch
- * @param yaw
- * @param rollspeed
- * @param pitchspeed
- * @param yawspeed
- *
- */
-static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
-{
-	plane_data.p = rollspeed - sinf(pitch) * yawspeed;
-	plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed;
-	plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed;
-}
-
-/**
- *
- * Calculates the attitude angles in the body reference frame.
- *
- * Writes them to the plane data structure
- *
- * @param roll
- * @param pitch
- * @param yaw
- */
-
-static void calc_bodyframe_angles(float roll, float pitch, float yaw)
-{
-	plane_data.rollb = cosf(yaw) * cosf(pitch) * roll +
-			   (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch
-			   + (-cosf(yaw) * sinf(pitch) * cosf(roll)  + sinf(yaw) * sinf(roll)) * yaw;
-	plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll +
-			    (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch
-			    + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw;
-	plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw;
-}
-
-/**
- * calc_rotation_matrix
- *
- * Calculates the rotation matrix
- *
- * @param roll
- * @param pitch
- * @param yaw
- * @param x
- * @param y
- * @param z
- *
- */
-
-static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z)
-{
-	plane_data.rollb = cosf(yaw) * cosf(pitch) * x +
-			   (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y
-			   + (-cosf(yaw) * sinf(pitch) * cosf(roll)  + sinf(yaw) * sinf(roll)) * z;
-	plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x +
-			    (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y
-			    + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z;
-	plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z;
-}
-
-/**
- * calc_bearing
- *
- * Calculates the bearing error of the plane compared to the waypoints
- *
- * @return bearing Bearing error
- *
- */
-static float calc_bearing()
-{
-	float bearing = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
-
-	if (bearing < 0.0f) {
-		bearing += 2*M_PI_F;
-	}
-
-	return bearing;
-}
-
-/**
- * calc_roll_ail
- *
- * Calculates the roll ailerons control output
- *
- * @return Roll ailerons control output (-1,1)
- */
-
-static float calc_roll_ail()
-{
-	float ret = pid((plane_data.roll_setpoint - plane_data.roll), plane_data.rollspeed, PID_DT, PID_SCALER,
-			plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att);
-
-	if (ret < -1)
-		return -1;
-
-	if (ret > 1)
-		return 1;
-
-	return ret;
-}
-
-/**
- * calc_pitch_elev
- *
- * Calculates the pitch elevators control output
+ * Controller parameters, accessible via MAVLink
  *
- * @return Pitch elevators control output (-1,1)
  */
-static float calc_pitch_elev()
-{
-	float ret = pid((plane_data.pitch_setpoint - plane_data.pitch), plane_data.pitchspeed, PID_DT, PID_SCALER,
-			plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att);
-
-	if (ret < -1)
-		return -1;
-
-	if (ret > 1)
-		return 1;
-
-	return ret;
-}
+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);
+
+struct fw_att_control_params {
+	float roll_pos_p;
+	float roll_pos_i;
+	float roll_pos_awu;
+	float roll_pos_lim;
+};
 
-/**
- * calc_yaw_rudder
- *
- * Calculates the yaw rudder control output (only if yaw rudder exists on the model)
- *
- * @return Yaw rudder control output (-1,1)
- *
- */
-static float calc_yaw_rudder(float hdg)
-{
-	float ret = pid((plane_data.yaw - abs(hdg)), plane_data.yawspeed, PID_DT, PID_SCALER,
-			plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
+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;
+};
 
-	if (ret < -1)
-		return -1;
 
-	if (ret > 1)
-		return 1;
+// XXX outsource position control to a separate app some time
 
-	return ret;
-}
+PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
+PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f);
 
-/**
- * calc_throttle
- *
- * Calculates the throttle control output
- *
- * @return Throttle control output (0,1)
- */
-
-static float calc_throttle()
-{
-	float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER,
-			plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
-
-	if (ret < 0.2f)
-		return 0.2f;
-
-	if (ret > 1.0f)
-		return 1.0f;
+struct fw_pos_control_params {
+	float heading_p;
+	float heading_lim;
+};
 
-	return ret;
-}
+struct fw_pos_control_param_handles {
+	param_t heading_p;
+	param_t heading_lim;
+};
 
 /**
- * calc_gnd_speed
- *
- * Calculates the ground speed using the x and y components
- *
- * Input: none (operation on global data)
- *
- * Output: Ground speed of the plane
+ * Initialize all parameter handles and values
  *
  */
-
-static float calc_gnd_speed()
-{
-	float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy);
-	return gnd_speed;
-}
+static int att_parameters_init(struct fw_att_control_param_handles *h);
 
 /**
- * calc_wp_distance
- *
- * Calculates the distance to the next waypoint
- *
- * @return the distance to the next waypoint
+ * Update all parameters
  *
  */
-
-static float calc_wp_distance()
-{
-	return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) +
-		     (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x));
-}
+static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p);
 
 /**
- * calc_roll_setpoint
- *
- * Calculates the offset angle for the roll plane,
- * saturates at +- 35 deg.
- *
- * @return setpoint on which attitude control should stabilize while changing heading
+ * Initialize all parameter handles and values
  *
  */
-
-static float calc_roll_setpoint()
-{
-	float setpoint = 0;
-
-	setpoint = calc_bearing() - plane_data.yaw;
-
-	if (setpoint < (-35.0f/180.0f)*M_PI_F)
-		return (-35.0f/180.0f)*M_PI_F;
-
-	if (setpoint > (35/180.0f)*M_PI_F)
-		return (-35.0f/180.0f)*M_PI_F;
-
-	return setpoint;
-}
+static int pos_parameters_init(struct fw_pos_control_param_handles *h);
 
 /**
- * calc_pitch_setpoint
- *
- * Calculates the offset angle for the pitch plane
- * saturates at +- 35 deg.
- *
- * @return setpoint on which attitude control should stabilize while changing altitude
+ * Update all parameters
  *
  */
+static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
 
-static float calc_pitch_setpoint()
-{
-	float setpoint = 0.0f;
-
-	// if (plane_data.mode == TAKEOFF) {
-	// 	setpoint = ((35/180.0f)*M_PI_F);
-
-	// } else {
-		setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
-
-		if (setpoint < (-35.0f/180.0f)*M_PI_F)
-			return (-35.0f/180.0f)*M_PI_F;
-
-		if (setpoint > (35/180.0f)*M_PI_F)
-			return (-35.0f/180.0f)*M_PI_F;
-	// }
-
-	return setpoint;
-}
 
 /**
+ * The fixed wing control main loop.
  *
- * Calculates the throttle setpoint for different flight modes
- *
- * @return throttle output setpoint
- *
- */
-static float calc_throttle_setpoint()
-{
-	float setpoint = 0;
-
-	// // if TAKEOFF full throttle
-	// if (plane_data.mode == TAKEOFF) {
-	// 	setpoint = 60.0f;
-	// }
-
-	// // if CRUISE - parameter value
-	// if (plane_data.mode == CRUISE) {
-		setpoint = plane_data.airspeed;
-	// }
-
-	// // if LAND no throttle
-	// if (plane_data.mode == LAND) {
-	// 	setpoint = 0.0f;
-	// }
-
-	return setpoint;
-}
-
-/**
+ * This loop executes continously and calculates the control
+ * response.
  *
  * @param argc number of arguments
  * @param argv argument array
@@ -723,27 +158,14 @@ static float calc_throttle_setpoint()
  */
 int fixedwing_control_thread_main(int argc, char *argv[])
 {
-	/* default values for arguments */
-	char *fixedwing_uart_name = "/dev/ttyS1";
-	char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";
+	// /* read arguments */
+	// bool verbose = false;
 
-	/* read arguments */
-	bool verbose = false;
-
-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
-			if (argc > i + 1) {
-				fixedwing_uart_name = argv[i + 1];
-
-			} else {
-				printf(commandline_usage, argv[0]);
-				return 0;
-			}
-		}
-		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-			verbose = true;
-		}
-	}
+	// for (int i = 1; i < argc; i++) {
+	// 	if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+	// 		verbose = true;
+	// 	}
+	// }
 
 	/* welcome user */
 	printf("[fixedwing control] started\n");
@@ -760,10 +182,6 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
 	/* Subscribe to global position, attitude and rc */
-	struct vehicle_global_position_s global_pos;
-	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	struct vehicle_global_position_setpoint_s global_setpoint;
-	int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	/* declare and safely initialize all structs */
 	struct vehicle_status_s state;
 	memset(&state, 0, sizeof(state));
@@ -773,6 +191,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	memset(&manual, 0, sizeof(manual));
 
 	/* subscribe to attitude, motor setpoints and system state */
+	struct vehicle_global_position_s global_pos;
+	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	struct vehicle_global_position_setpoint_s global_setpoint;
+	int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -780,139 +202,241 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 
 	/* Mainloop setup */
 	unsigned int loopcounter = 0;
-	unsigned int failcounter = 0;
-
-	/* Control constants */
-	control_outputs.mode = HIL_MODE;
-	control_outputs.nav_mode = 0;
-
-	/* Servo setup */
-
-	/*
-	 * Main control, navigation and servo routine
-	 */
-
-	while(1) {
-		/*
-		 * DATA Handling
-		 * Fetch current flight data
-		 */
-
-		/* get position, attitude and rc inputs */
-		// XXX add error checking
-		orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-		orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
-		
-		/* get a local copy of system state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of attitude setpoint */
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
-
-		/* scaling factors are defined by the data from the APM Planner
-		 * TODO: ifdef for other parameters (HIL/Real world switch)
-		 */
-
-		/* position values*/
-		plane_data.lat = global_pos.lat / 10000000.0;
-		plane_data.lon = global_pos.lon / 10000000.0;
-		plane_data.alt = global_pos.alt / 1000.0f;
-		plane_data.vx = global_pos.vx / 100.0f;
-		plane_data.vy = global_pos.vy / 100.0f;
-		plane_data.vz = global_pos.vz / 100.0f;
-
-		/* attitude values*/
-		plane_data.roll = att.roll;
-		plane_data.pitch = att.pitch;
-		plane_data.yaw = att.yaw;
-		plane_data.rollspeed = att.rollspeed;
-		plane_data.pitchspeed = att.pitchspeed;
-		plane_data.yawspeed = att.yawspeed;
-
-		plane_data.wp_x =  global_setpoint.lat / (double)1e7;
-		plane_data.wp_y =  global_setpoint.lon / (double)1e7;
-		plane_data.wp_z =  global_setpoint.altitude;
-
-		/* parameter values */
-		if (loopcounter % 20 == 0) {
-			get_parameters(&plane_data);
-		}
 
-		/* Attitude control part */
+	uint64_t last_run = 0;
+	uint64_t last_run_pos = 0;
 
-		if (verbose && loopcounter % 20 == 0) {
-			/******************************** DEBUG OUTPUT ************************************************************/
+	bool global_sp_updated_set_once = false;
 
-			printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
-					(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
-					(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
-					(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
+	struct fw_att_control_params p;
+	struct fw_att_control_param_handles h;
 
-			printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
-			printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
-			printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*calc_throttle_setpoint()));
+	struct fw_pos_control_params ppos;
+	struct fw_pos_control_param_handles hpos;
 
-	//		printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
+	/* initialize the pid controllers */
+	att_parameters_init(&h);
+	att_parameters_update(&h, &p);
 
-	//		printf("Current Altitude: %i\n\n", (int)plane_data.alt);
+	pos_parameters_init(&hpos);
+	pos_parameters_update(&hpos, &ppos);
 
-			printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
-					(int)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F),
-					(int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F);
+	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, 155);
 
-	//		printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
-	//				(int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
+	PID_t heading_controller;
+	pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
+			PID_MODE_DERIVATIV_SET, 155);
 
-			printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n",
-					att_sp.roll_body, att_sp.pitch_body,
-					att_sp.yaw_body, att_sp.thrust);
+	while(!thread_should_exit) {
 
-			/************************************************************************************************************/
-		}
+		struct pollfd fds[1] = {
+			{ .fd = att_sub,   .events = POLLIN },
+		};
+		int ret = poll(fds, 1, 1000);
 
-		/* Calculate the P,Q,R body rates of the aircraft */
-		//calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw,
-		//		plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* XXX this means no sensor data - should be critical or emergency */
+			printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n");
+		} else {
 
-		/* Calculate the body frame angles of the aircraft */
-		//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
+			// FIXME SUBSCRIBE
+			if (loopcounter % 20 == 0) {
+				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);
+			}
 
-		// if (manual.override_mode_switch < XXX) {
+			/* if position updated, run position control loop */
+			bool pos_updated;
+			orb_check(global_pos_sub, &pos_updated);
+			bool global_sp_updated;
+			orb_check(global_setpoint_sub, &global_sp_updated);
+			if (global_sp_updated) {
+				global_sp_updated_set_once = true;
+			}
+			/* checking has to happen before the read, as the read clears the changed flag */
+
+			/* get a local copy of system state */
+			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+			/* get a local copy of manual setpoint */
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			/* get a local copy of attitude */
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			/* get a local copy of attitude setpoint */
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+
+			if (pos_updated) {
+
+				/* get position */
+				orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+				
+				if (global_sp_updated_set_once) {
+					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
+				
+			
+					/* calculate delta T */
+					const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+					last_run = hrt_absolute_time();
+
+					/* calculate bearing error */
+					float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d,
+						global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
+
+					/* shift error to prevent wrapping issues */
+					float bearing_error = att.yaw - target_bearing;
+
+					if (bearing_error < M_PI_F) {
+						bearing_error += 2.0f * M_PI_F;
+					}
+
+					if (bearing_error > M_PI_F) {
+						bearing_error -= 2.0f * M_PI_F;
+					}
+
+					/* calculate roll setpoint, do this artificially around zero */
+					att_sp.roll_body = pid_calculate(&heading_controller, bearing_error,
+					0.0f, att.yawspeed, deltaT);
+
+					/* limit roll angle output */
+					if (att_sp.roll_body > ppos.heading_lim) {
+						att_sp.roll_body = ppos.heading_lim;
+						heading_controller.saturated = 1;
+					}
+
+					if (att_sp.roll_body < -ppos.heading_lim) {
+						att_sp.roll_body = -ppos.heading_lim;
+						heading_controller.saturated = 1;
+					}
+
+					att_sp.roll_body = att_sp.roll_body;
+					att_sp.pitch_body = 0.0f;
+					att_sp.yaw_body = 0.0f;
+
+				} else {
+					/* no setpoint, maintain level flight */
+					att_sp.roll_body = 0.0f;
+					att_sp.pitch_body = 0.0f;
+					att_sp.yaw_body = 0.0f;
+				}
+
+				att_sp.thrust = 0.7f;
+			}
 
+			/* calculate delta T */
+			const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f;
+			last_run_pos = hrt_absolute_time();
 
-		/* Navigation part */
+			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;
+			actuators.control[3] = att_sp.thrust;
 
-		// Get Waypoint
+			/* publish attitude setpoint (for MAVLink) */
+			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
-		// XXX FIXME
+			/* publish actuator setpoints (for mixer) */
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-		//else if (manual.override_mode_switch > MANUAL) {	// AUTO mode
-		
-		/* calculate setpoint based on current position and waypoint */
-		att_sp.roll_body = calc_roll_setpoint();
-		att_sp.pitch_body = calc_pitch_setpoint();
-		att_sp.thrust = calc_throttle_setpoint();
+			loopcounter++;
 
-		/* calculate the control outputs based on roll / pitch / yaw setpoints */
-		actuators.control[0] = calc_roll_ail();
-		actuators.control[1] = calc_pitch_elev();
-		actuators.control[2] = calc_yaw_rudder(att.yaw);
-		actuators.control[3] = calc_throttle();
+		}
+	}
 
-		/* publish attitude setpoint (for MAVLink) */
-		orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+	printf("[fixedwing_control] exiting.\n");
+	thread_running = false;
 
-		/* publish actuator setpoints (for mixer) */
-		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+	return 0;
+}
 
-		loopcounter++;
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int fixedwing_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
 
-		/* 20Hz loop*/
-		usleep(50000);
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("fixedwing_control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		thread_running = true;
+		exit(0);
 	}
 
-	return 0;
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tfixedwing_control is running\n");
+		} else {
+			printf("\tfixedwing_control not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+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");
+
+	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));
+
+	return OK;
+}
+
+static int pos_parameters_init(struct fw_pos_control_param_handles *h)
+{
+	/* PID parameters */
+	h->heading_p 	=	param_find("FW_HEADING_P");
+	h->heading_lim 	=	param_find("FW_HEADING_LIM");
+
+	return OK;
+}
+
+static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
+{
+	param_get(h->heading_p, &(p->heading_p));
+	param_get(h->heading_lim, &(p->heading_lim));
+
+	return OK;
 }