fw control: attitude, added pid elements

2 files changed, 14 insertions, 16 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c
index c691c6938..711a1cc31 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c
@@ -113,10 +113,15 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	static struct fw_att_control_params p;
 	static struct fw_att_control_params_handles 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
 		initialized = true;
 	}
 
@@ -124,22 +129,19 @@ 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);
 	}
 
 	/* Roll (P) */
-	float roll_error = att_sp->roll_tait_bryan - att->roll;
-	//TODO convert to body frame
-	rates_sp->roll = p.roll_p * roll_error; //TODO enabled for testing only
+	rates_sp->roll = pid_calculate(&roll_controller,att_sp->roll_tait_bryan, att->roll, 0, 0);
 
 	/* Pitch (P) */
-	float pitch_error = att_sp->pitch_tait_bryan - att->pitch;
-	//TODO convert to body frame
+	//rates_sp->pitch = pid_calculate(&pitch_controller,att_sp->pitch_tait_bryan, att->pitch, 0, 0);
 
-	/* Yaw (from coordinated turn constraint) */
+	/* Yaw (from coordinated turn constraint or lateral force) */
 	//TODO
 
-	//TODO Limits
-
 	counter++;
 
 	return 0;
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
index 24dce199f..4b7e2c2ba 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -116,10 +116,6 @@ static int parameters_init(struct fw_rate_control_param_handles *h)
 	h->yawrate_awu =	param_find("FW_YAWRATE_AWU");
 	h->yawrate_lim =	param_find("FW_YAWRATE_LIM");
 
-
-//	if(h->attrate_i == PARAM_INVALID)
-//		printf("FATAL MC_ATTRATE_I does not exist\n");
-
 	return OK;
 }
 
@@ -163,19 +159,19 @@ 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
+		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
 		initialized = true;
 	}
 
 	/* load new parameters with lower rate */
 	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);
-		parameters_update(&h, &p);
 	}