Better yaw position control, but not quite there yet

2 files changed, 8 insertions, 4 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index b77281dc2..77adaf217 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -139,6 +139,7 @@ mc_thread_main(int argc, char *argv[])
 	/* store last control mode to detect mode switches */
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
+	bool flag_system_armed = false;
 
 	while (!thread_should_exit) {
 
@@ -208,15 +209,16 @@ mc_thread_main(int argc, char *argv[])
 
 				/* initialize to current yaw if switching to manual or att control */
 				if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-			 	    state.flag_control_manual_enabled != flag_control_manual_enabled) {
+			 	    state.flag_control_manual_enabled != flag_control_manual_enabled ||
+			 	    state.flag_system_armed != flag_system_armed) {
 					att_sp.yaw_body = att.yaw;
 				}
 
 				att_sp.roll_body = manual.roll;
 				att_sp.pitch_body = manual.pitch;
 				/* only move setpoint if manual input is != 0 */
-				if (-3.0f * FLT_EPSILON < manual.yaw || manual.yaw > 3.0f * FLT_EPSILON) {
-					att_sp.yaw_body = att.yaw + manual.yaw * -2.0f;
+				if (manual.yaw < -0.02f || 0.02f < manual.yaw) {
+					att_sp.yaw_body = att_sp.yaw_body + manual.yaw * -0.0025f;
 				}
 				att_sp.thrust = manual.throttle;
 				att_sp.timestamp = hrt_absolute_time();
@@ -264,8 +266,10 @@ mc_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 		}
 
+		/* update state */
 		flag_control_attitude_enabled = state.flag_control_attitude_enabled;
 		flag_control_manual_enabled = state.flag_control_manual_enabled;
+		flag_system_armed = state.flag_system_armed;
 
 		perf_end(mc_loop_perf);
 	}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index eb94cca8a..1258c739e 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -217,7 +217,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 			att->roll, att->rollspeed, deltaT);
 
 	/* control yaw rate */
-	rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body));
+	rates_sp->yaw = p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
 
 	rates_sp->thrust = att_sp->thrust;