[work in progess] some copy paste for pitch and yaw, but not enabled yet

3 files changed, 44 insertions, 22 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c
index 7d648ccfa..b13d564d6 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c
@@ -35,7 +35,7 @@
  * @file fixedwing_att_control_rate.c
  * Implementation of a fixed wing attitude controller.
  */
-#include <fixedwing_att_control_rate.h>
+#include <fixedwing_att_control_att.h>
 
 #include <nuttx/config.h>
 #include <stdio.h>
@@ -63,30 +63,42 @@
 
 
 struct fw_att_control_params {
-	param_t roll_p;
+	float roll_p;
+	float roll_lim;
+	float pitch_p;
+	float pitch_lim;
+};
+
+struct fw_att_control_params_handles {
+	float roll_p;
+	float roll_lim;
+	float pitch_p;
+	float pitch_lim;
 };
 
 
 
 /* Internal Prototypes */
-static int parameters_init(struct fw_att_control_params *h);
-static int parameters_update(const struct fw_att_control_params *h, struct fw_att_control_params *p);
+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_att_control_params *h)
+static int parameters_init(struct fw_att_control_params_handles *h)
 {
 	/* PID parameters */
-	h->roll_p 	=	param_find("FW_ROLL_POS_P");   //TODO define rate params for fixed wing
-
-
-//	if(h->attrate_i == PARAM_INVALID)
-//		printf("FATAL MC_ATTRATE_I does not exist\n");
+	h->roll_p 		=	param_find("FW_ROLL_P");
+	h->roll_lim 	=	param_find("FW_ROLL_LIM");
+	h->pitch_p 		=	param_find("FW_PITCH_P");
+	h->pitch_lim 	=	param_find("FW_PITCH_LIM");
 
 	return OK;
 }
 
-static int parameters_update(const struct fw_att_control_params *h, struct fw_att_control_params *p)
+static int parameters_update(const struct fw_att_control_params_handles *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->pitch_p, &(p->pitch_p));
+	param_get(h->pitch_lim, &(p->pitch_lim));
 
 	return OK;
 }
@@ -99,11 +111,7 @@ 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 h;
-
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
+	static struct fw_att_control_params_handles h;
 
 	if(!initialized)
 	{
@@ -120,7 +128,17 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 
 	/* Roll (P) */
 	float roll_error = att_sp->roll_tait_bryan - att->roll;
-	rates_sp->roll = p.roll_p * roll_error;
+	//TODO convert to body frame
+	rates_sp->roll = p.roll_p * roll_error; //TODO enabled for testing only
+
+	/* Pitch (P) */
+	float pitch_error = att_sp->pitch_tait_bryan - att->pitch;
+	//TODO convert to body frame
+
+	/* Yaw (from coordinated turn constraint) */
+	//TODO
+
+	//TODO Limits
 
 	counter++;
 
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index a6dd1c9fb..0766a3d40 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -90,8 +90,6 @@ 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_YAW_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_YAW_LIM, 0.35f);	// Yaw angle limit in radians
 
 /* Prototypes */
 /**
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
index e6f5c6857..959628be9 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -152,6 +152,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	static struct fw_rate_control_param_handles h;
 
 	static PID_t roll_rate_controller;
+	static PID_t pitch_rate_controller;
+	static PID_t yaw_rate_controller;
 
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -162,6 +164,8 @@ 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_SET); // 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_SET); // 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_SET); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
 		initialized = true;
 	}
 
@@ -169,15 +173,17 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (counter % 2500 == 0) {
 		/* update parameters from storage */
 		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);
 		parameters_update(&h, &p);
 	}
 
 	/* Roll Rate (PI) */
 	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
 
-
-	actuators->control[1] = 0;
-	actuators->control[2] = 0;
+	//XXX TODO disabled for now
+	actuators->control[1] = 0;//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++;