fw control: moved and renamed parameters, attitude: roll and pitch working

3 files changed, 69 insertions, 65 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c
index 711a1cc31..b6196702f 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c
@@ -64,41 +64,48 @@
 
 struct fw_att_control_params {
 	float roll_p;
-	float roll_lim;
+	float rollrate_lim;
 	float pitch_p;
 	float pitch_lim;
+	float pitchrate_lim;
+	float yawrate_lim;
 };
 
-struct fw_att_control_params_handles {
+struct fw_pos_control_params_handle {
 	float roll_p;
-	float roll_lim;
+	float rollrate_lim;
 	float pitch_p;
 	float pitch_lim;
+	float pitchrate_lim;
+	float yawrate_lim;
 };
 
 
 
 /* Internal Prototypes */
-static int parameters_init(struct fw_att_control_params_handles *h);
-static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p);
+static int parameters_init(struct fw_pos_control_params_handle *h);
+static int parameters_update(const struct fw_pos_control_params_handle *h, struct fw_att_control_params *p);
 
-static int parameters_init(struct fw_att_control_params_handles *h)
+static int parameters_init(struct fw_pos_control_params_handle *h)
 {
 	/* PID parameters */
 	h->roll_p 		=	param_find("FW_ROLL_P");
-	h->roll_lim 	=	param_find("FW_ROLL_LIM");
+	h->rollrate_lim =	param_find("FW_ROLLR_LIM");
 	h->pitch_p 		=	param_find("FW_PITCH_P");
 	h->pitch_lim 	=	param_find("FW_PITCH_LIM");
+	h->pitchrate_lim =	param_find("FW_PITCHR_LIM");
+	h->yawrate_lim =	param_find("FW_YAWR_LIM");
 
 	return OK;
 }
 
-static int parameters_update(const struct fw_att_control_params_handles *h, struct fw_att_control_params *p)
+static int parameters_update(const struct fw_pos_control_params_handle *h, struct fw_att_control_params *p)
 {
 	param_get(h->roll_p, &(p->roll_p));
-	param_get(h->roll_lim, &(p->roll_lim));
+	param_get(h->rollrate_lim, &(p->rollrate_lim));
 	param_get(h->pitch_p, &(p->pitch_p));
-	param_get(h->pitch_lim, &(p->pitch_lim));
+	param_get(h->pitchrate_lim, &(p->pitchrate_lim));
+	param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	return OK;
 }
@@ -111,17 +118,18 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	static bool initialized = false;
 
 	static struct fw_att_control_params p;
-	static struct fw_att_control_params_handles h;
+	static struct fw_pos_control_params_handle h;
 
 	static PID_t roll_controller;
 	static PID_t pitch_controller;
 
+
 	if(!initialized)
 	{
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim, PID_MODE_DERIVATIV_NONE); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
 		initialized = true;
 	}
 
@@ -129,15 +137,16 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	if (counter % 100 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.roll_lim);
-		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitch_lim);
+		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.pitchrate_lim);
+		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
 	}
 
 	/* Roll (P) */
 	rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0);
 
 	/* Pitch (P) */
-	//rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0);
+
+	rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0);
 
 	/* Yaw (from coordinated turn constraint or lateral force) */
 	//TODO
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 06890ed8b..82ef0403b 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -1,7 +1,8 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
+ *   Author: 	@author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *   			@author Doug Weibel <douglas.weibel@colorado.edu>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -57,6 +58,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/debug_key_value.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/geo/geo.h>
@@ -70,26 +72,24 @@
  *
  */
 // Roll control parameters
-PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f);   // Roll rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f);   // Roll rate limit in radians/sec, applies to the roll controller
 PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f);	// Roll angle limit in radians
 
 //Pitch control parameters
-PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f);   // Pitch rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f);   // Pitch rate limit in radians/sec, applies to the pitch controller
 PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	// Pitch angle limit in radians
 
 //Yaw control parameters					//XXX TODO this is copy paste, asign correct values
-PARAM_DEFINE_FLOAT(FW_YAWRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_YAWRATE_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_YAWRATE_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_YAWRATE_LIM, 0.35f);   // Yaw rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f);   // Yaw rate limit in radians/sec
 
 /* Prototypes */
 /**
@@ -134,16 +134,20 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		memset(&att_sp, 0, sizeof(att_sp));
 		struct vehicle_rates_setpoint_s rates_sp;
 		memset(&rates_sp, 0, sizeof(rates_sp));
+//		static struct debug_key_value_s debug_output;
+//		memset(&debug_output, 0, sizeof(debug_output));
 
 		/* output structs */
 		struct actuator_controls_s actuators;
 		memset(&actuators, 0, sizeof(actuators));
 
+
 		/* publish actuator controls */
 		for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
 			actuators.control[i] = 0.0f;
 		orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-
+//		orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
+//		debug_output.key[0] = '1';
 
 
 		/* subscribe  */
@@ -162,6 +166,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			/*Get Local Copies */
 			/* get a local copy of attitude */
 			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
 
 			gyro[0] = att.rollspeed;
 			gyro[1] = att.pitchspeed;
@@ -170,14 +175,9 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			/* Control */
 
 			/* Attitude Control */
-			att_sp.roll_tait_bryan = 0.0f; //REMOVEME TODO
-			att_sp.pitch_tait_bryan = 0.0f;
-			att_sp.yaw_tait_bryan = 0.0f;
 			fixedwing_att_control_attitude(&att_sp,
 					&att,
 					&rates_sp);
-			rates_sp.pitch = 0.0f; //REMOVEME TODO
-			rates_sp.yaw = 0.0f;
 
 			/* Attitude Rate Control */
 			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
@@ -186,6 +186,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 			actuators.control[3] = 0.7f;
 
 			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+//			debug_output.value = rates_sp.pitch;
+//			orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
 		}
 
 		printf("[fixedwing_att_control] exiting, stopping all motors.\n");
@@ -198,6 +200,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
 
+
 		close(att_sub);
 		close(actuator_pub);
 
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
index 4b7e2c2ba..ec3039dd6 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -67,15 +67,12 @@ struct fw_rate_control_params {
 	float rollrate_p;
 	float rollrate_i;
 	float rollrate_awu;
-	float rollrate_lim;
 	float pitchrate_p;
 	float pitchrate_i;
 	float pitchrate_awu;
-	float pitchrate_lim;
 	float yawrate_p;
 	float yawrate_i;
 	float yawrate_awu;
-	float yawrate_lim;
 
 };
 
@@ -83,15 +80,13 @@ struct fw_rate_control_param_handles {
 	float rollrate_p;
 	float rollrate_i;
 	float rollrate_awu;
-	float rollrate_lim;
 	float pitchrate_p;
 	float pitchrate_i;
 	float pitchrate_awu;
-	float pitchrate_lim;
 	float yawrate_p;
 	float yawrate_i;
 	float yawrate_awu;
-	float yawrate_lim;
+
 };
 
 
@@ -103,18 +98,17 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
 static int parameters_init(struct fw_rate_control_param_handles *h)
 {
 	/* PID parameters */
-	h->rollrate_p 	=	param_find("FW_ROLLRATE_P");   //TODO define rate params for fixed wing
-	h->rollrate_i 	=	param_find("FW_ROLLRATE_I");
-	h->rollrate_awu =	param_find("FW_ROLLRATE_AWU");
-	h->rollrate_lim =	param_find("FW_ROLLRATE_LIM");
-	h->pitchrate_p 	=	param_find("FW_PITCHRATE_P");
-	h->pitchrate_i 	=	param_find("FW_PITCHRATE_I");
-	h->pitchrate_awu =	param_find("FW_PITCHRATE_AWU");
-	h->pitchrate_lim =	param_find("FW_PITCHRATE_LIM");
-	h->yawrate_p 	=	param_find("FW_YAWRATE_P");
-	h->yawrate_i 	=	param_find("FW_YAWRATE_I");
-	h->yawrate_awu =	param_find("FW_YAWRATE_AWU");
-	h->yawrate_lim =	param_find("FW_YAWRATE_LIM");
+	h->rollrate_p 	=	param_find("FW_ROLLR_P");   //TODO define rate params for fixed wing
+	h->rollrate_i 	=	param_find("FW_ROLLR_I");
+	h->rollrate_awu =	param_find("FW_ROLLR_AWU");
+
+	h->pitchrate_p 	=	param_find("FW_PITCHR_P");
+	h->pitchrate_i 	=	param_find("FW_PITCHR_I");
+	h->pitchrate_awu =	param_find("FW_PITCHR_AWU");
+
+	h->yawrate_p 	=	param_find("FW_YAWR_P");
+	h->yawrate_i 	=	param_find("FW_YAWR_I");
+	h->yawrate_awu =	param_find("FW_YAWR_AWU");
 
 	return OK;
 }
@@ -124,15 +118,13 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
 	param_get(h->rollrate_p, &(p->rollrate_p));
 	param_get(h->rollrate_i, &(p->rollrate_i));
 	param_get(h->rollrate_awu, &(p->rollrate_awu));
-	param_get(h->rollrate_lim, &(p->rollrate_lim));
 	param_get(h->pitchrate_p, &(p->pitchrate_p));
 	param_get(h->pitchrate_i, &(p->pitchrate_i));
 	param_get(h->pitchrate_awu, &(p->pitchrate_awu));
-	param_get(h->pitchrate_lim, &(p->pitchrate_lim));
 	param_get(h->yawrate_p, &(p->yawrate_p));
 	param_get(h->yawrate_i, &(p->yawrate_i));
 	param_get(h->yawrate_awu, &(p->yawrate_awu));
-	param_get(h->yawrate_lim, &(p->yawrate_lim));
+
 
 	return OK;
 }
@@ -159,9 +151,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	{
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
-		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
+		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
 		initialized = true;
 	}
 
@@ -169,9 +161,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (counter % 100 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, p.rollrate_lim);
-		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, p.pitchrate_lim);
-		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, p.yawrate_lim);
+		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
+		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
+		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
 	}
 
 
@@ -179,7 +171,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
 
 	//XXX TODO disabled for now
-	actuators->control[1] = 0;//pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
+	actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
 	actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT);
 
 	counter++;