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

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_init(struct fw_att_control_params_handles *h);
-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);
 
-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
+	h->roll_p 		=	param_find("FW_ROLL_P");
-
+	h->roll_lim 	=	param_find("FW_ROLL_LIM");
-
+	h->pitch_p 		=	param_find("FW_PITCH_P");
-//	if(h->attrate_i == PARAM_INVALID)
+	h->pitch_lim 	=	param_find("FW_PITCH_LIM");
-//		printf("FATAL MC_ATTRATE_I does not exist\n");
 
 	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 struct fw_att_control_params_handles h;
-
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
 
 	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++;
 

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 */
 /**

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);
 
-
+	//XXX TODO disabled for now
-	actuators->control[1] = 0;
+	actuators->control[1] = 0;//pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
-	actuators->control[2] = 0;
+	actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT);
 
 	counter++;
 

apps/fixedwing_pos_control/fixedwing_pos_control_main.c

@@ -112,7 +112,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 
 
-//		/* subscribe to attitude (for attitude rate) and rate septoint */
+//		/* subscribe */
 //
 
 		/* Setup of loop */