Use rollacc and pitchacc from the estimator instead of differentiating in the controller

3 files changed, 19 insertions, 12 deletions

diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index c3b2f5d2a..dd38242d3 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -394,7 +394,8 @@ mc_thread_main(int argc, char *argv[])
 				/* measure in what intervals the controller runs */
 				perf_count(mc_interval_perf);
 
-				float gyro[3];
+				float rates[3];
+				float rates_acc[3];
 
 				/* get current rate setpoint */
 				bool rates_sp_valid = false;
@@ -405,11 +406,17 @@ mc_thread_main(int argc, char *argv[])
 				}
 
 				/* apply controller */
-				gyro[0] = att.rollspeed;
-				gyro[1] = att.pitchspeed;
-				gyro[2] = att.yawspeed;
+				rates[0] = att.rollspeed;
+				rates[1] = att.pitchspeed;
+				rates[2] = att.yawspeed;
 
-				multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug);
+				rates_acc[0] = att.rollacc;
+				rates_acc[1] = att.pitchacc;
+				rates_acc[2] = att.yawacc;
+
+
+
+				multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug);
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
 				orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index a39d6f13d..e37ede3e0 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -151,7 +151,7 @@ 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,
+			      const float rates[], const float rates_acc[], struct actuator_controls_s *actuators,
 			      const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
 {
 	static uint64_t last_run = 0;
@@ -186,8 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_update(&h, &p);
 		initialized = true;
 
-		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
-		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
+		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET);
+		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET);
 
 	}
 
@@ -201,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control pitch (forward) output */
 	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-			rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
+			rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
 
 	/* control roll (left/right) output */
 	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
-			rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
+			rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
 
 	/* increase resilience to faulty control inputs */
 	if (isfinite(pitch_control)) {
@@ -225,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		warnx("rej. NaN ctrl roll");
 	}
 
-	/* control yaw rate */
+	/* control yaw rate */ //XXX use library here and use rates_acc[2]
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
 
 	/* increase resilience to faulty control inputs */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 465549540..fc741c378 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -52,7 +52,7 @@
 #include <uORB/topics/vehicle_control_debug.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators,
+			      const float rates[], const float rates_acc[], struct actuator_controls_s *actuators,
 			      const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */