2 files changed, 28 insertions, 56 deletions

diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 76dbb36d3..d7e1a3adc 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -57,50 +57,29 @@
 PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
 PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
-
-//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f);
 
 struct mc_att_control_params {
 	float yaw_p;
 	float yaw_i;
 	float yaw_d;
-	//float yaw_awu;
-	//float yaw_lim;
 
 	float att_p;
 	float att_i;
 	float att_d;
-	//float att_awu;
-	//float att_lim;
-
-	//float att_xoff;
-	//float att_yoff;
 };
 
 struct mc_att_control_param_handles {
 	param_t yaw_p;
 	param_t yaw_i;
 	param_t yaw_d;
-	//param_t yaw_awu;
-	//param_t yaw_lim;
 
 	param_t att_p;
 	param_t att_i;
 	param_t att_d;
-	//param_t att_awu;
-	//param_t att_lim;
-
-	//param_t att_xoff;
-	//param_t att_yoff;
 };
 
 /**
@@ -122,17 +101,10 @@ static int parameters_init(struct mc_att_control_param_handles *h)
 	h->yaw_p 	=	param_find("MC_YAWPOS_P");
 	h->yaw_i 	=	param_find("MC_YAWPOS_I");
 	h->yaw_d 	=	param_find("MC_YAWPOS_D");
-	//h->yaw_awu 	=	param_find("MC_YAWPOS_AWU");
-	//h->yaw_lim 	=	param_find("MC_YAWPOS_LIM");
 
 	h->att_p 	= 	param_find("MC_ATT_P");
 	h->att_i 	= 	param_find("MC_ATT_I");
 	h->att_d 	= 	param_find("MC_ATT_D");
-	//h->att_awu 	= 	param_find("MC_ATT_AWU");
-	//h->att_lim 	= 	param_find("MC_ATT_LIM");
-
-	//h->att_xoff 	= 	param_find("MC_ATT_XOFF");
-	//h->att_yoff 	= 	param_find("MC_ATT_YOFF");
 
 	return OK;
 }
@@ -142,17 +114,10 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 	param_get(h->yaw_p, &(p->yaw_p));
 	param_get(h->yaw_i, &(p->yaw_i));
 	param_get(h->yaw_d, &(p->yaw_d));
-	//param_get(h->yaw_awu, &(p->yaw_awu));
-	//param_get(h->yaw_lim, &(p->yaw_lim));
 
 	param_get(h->att_p, &(p->att_p));
 	param_get(h->att_i, &(p->att_i));
 	param_get(h->att_d, &(p->att_d));
-	//param_get(h->att_awu, &(p->att_awu));
-	//param_get(h->att_lim, &(p->att_lim));
-
-	//param_get(h->att_xoff, &(p->att_xoff));
-	//param_get(h->att_yoff, &(p->att_yoff));
 
 	return OK;
 }
@@ -170,8 +135,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		last_input = att_sp->timestamp;
 	}
 
-	static int sensor_delay;
-	sensor_delay = hrt_absolute_time() - att->timestamp;
+//	static int sensor_delay;
+//	sensor_delay = hrt_absolute_time() - att->timestamp;
 
 	static int motor_skip_counter = 0;
 
@@ -190,10 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_init(&h);
 		parameters_update(&h, &p);
 
-		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-			 1000.0f, PID_MODE_DERIVATIV_SET);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-			 1000.0f, PID_MODE_DERIVATIV_SET);
+		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET);
+		pid_init(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET);
 
 		initialized = true;
 	}
@@ -204,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_update(&h, &p);
 
 		/* apply parameters */
-		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
-		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
+		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f);
+		pid_set_parameters(&roll_controller,  p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f);
 	}
 
 	/* reset integral if on ground */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index deba1ac03..57aea726a 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -150,13 +150,11 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 			      const float rates[], struct actuator_controls_s *actuators)
 {
-	static float roll_control_last  = 0;
-	static float pitch_control_last = 0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	static uint64_t last_input = 0;
 
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
+//	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
 
 	if (last_input != rate_sp->timestamp) {
 		last_input = rate_sp->timestamp;
@@ -166,9 +164,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static int motor_skip_counter = 0;
 
+	static PID_t pitch_rate_controller;
+	static PID_t roll_rate_controller;
+
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
+	float pitch_control_last = 0.0f;
+	float roll_control_last = 0.0f;
+
 	static bool initialized = false;
 
 	/* initialize the pid controllers when the function is called for the first time */
@@ -176,39 +180,44 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_init(&h);
 		parameters_update(&h, &p);
 		initialized = true;
+
+		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
+					 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
+		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
+					 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
 	}
 
 	/* load new parameters with lower rate */
 	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
-		// 	(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f);
+		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f);
 	}
 
-	/* calculate current control outputs */
-
 	/* control pitch (forward) output */
-	float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
+	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
+			rates[1], 0.0f, deltaT);
+
+	/* control roll (left/right) output */
+	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
+			rates[0], 0.0f, deltaT);
 
 	/* increase resilience to faulty control inputs */
 	if (isfinite(pitch_control)) {
 		pitch_control_last = pitch_control;
 
 	} else {
-		pitch_control = 0.0f;
+		pitch_control = pitch_control_last;
 		warnx("rej. NaN ctrl pitch");
 	}
 
-	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
-
 	/* increase resilience to faulty control inputs */
 	if (isfinite(roll_control)) {
 		roll_control_last = roll_control;
 
 	} else {
-		roll_control = 0.0f;
+		roll_control = roll_control_last;
 		warnx("rej. NaN ctrl roll");
 	}