working offboard

1 files changed, 7 insertions, 2 deletions

diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 372b378d1..7532dffa2 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -144,6 +144,8 @@ 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;
 	last_run = hrt_absolute_time();
@@ -172,10 +174,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	/* calculate current control outputs */
 	
 	/* control pitch (forward) output */
-	float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]);
+
+	float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
+	pitch_control_last=pitch_control;
 	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0]  );
 
+	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
+	roll_control_last=roll_control;
 	/* control yaw rate */
 	float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2]  );