Cleaned up PI wrapping code, still subject to testing

3 files changed, 25 insertions, 26 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 91439d36d..10d155ffc 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -252,7 +252,7 @@ mc_thread_main(int argc, char *argv[])
 						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
 					 	    state.flag_control_manual_enabled != flag_control_manual_enabled ||
 					 	    state.flag_system_armed != flag_system_armed) {
-							att_sp.yaw_tait_bryan = att.yaw;
+							att_sp.yaw_body = att.yaw;
 						}
 
 						static bool rc_loss_first_time = true;
@@ -283,29 +283,28 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.roll_body = manual.roll;
 							att_sp.pitch_body = manual.pitch;
 
+							/* set attitude if arming */
+							if (!flag_control_attitude_enabled && state.flag_system_armed) {
+								att_sp.yaw_body = att.yaw;
+							}
+
 							/* only move setpoint if manual input is != 0 */
 
 							if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) {
-								// XXX turn into param
-								if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-									att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
-								} else if (manual.throttle <= 0.3f) {
-									att_sp.yaw_body = att.yaw;
-								}
-								control_yaw_position = true;
-							} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
 								if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
 									rates_sp.yaw = manual.yaw;
 									control_yaw_position = false;
 									first_time_after_yaw_speed_control = true;
 								} else {
-									rates_sp.yaw = 0.0f;
-									if(first_time_after_yaw_speed_control) {
-										att_sp.yaw_tait_bryan = att.yaw;
+									if (first_time_after_yaw_speed_control) {
+										att_sp.yaw_body = att.yaw;
 										first_time_after_yaw_speed_control = false;
 									}
 									control_yaw_position = true;
 								}
+							} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
+								rates_sp.yaw = manual.yaw;
+								control_yaw_position = false;
 							}
 
 							att_sp.thrust = manual.throttle;
@@ -330,9 +329,7 @@ mc_thread_main(int argc, char *argv[])
 
 				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 				if (state.flag_control_attitude_enabled) {
-				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL, control_yaw_position);
-
-
+				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
 
 				 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 87eeaced5..e94d7c55d 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators, bool control_yaw_position)
+		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -201,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 
-		//printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
-
 		/* 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);
@@ -220,15 +218,19 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	if(control_yaw_position) {
 		/* control yaw rate */
-		//rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_tait_bryan), sinf(att->yaw - att_sp->yaw_tait_bryan)) - (p.yaw_d * att->yawspeed);
-		yaw_error = att->yaw - att_sp->yaw_tait_bryan;
-		if ((double)yaw_error > M_PI) {
-			yaw_error -= M_PI;
-		} else if ((double)yaw_error < -M_PI) {
-			yaw_error += M_PI;
+
+		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
+		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
+
+		yaw_error = att_sp->yaw_body - att->yaw;
+
+		if (yaw_error > M_PI_F) {
+			yaw_error -= M_TWOPI_F;
+		} else if (yaw_error < -M_PI_F) {
+			yaw_error += M_TWOPI_F;
 		}
 
-		rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
+		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
 	}
 	rates_sp->thrust = att_sp->thrust;
 
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index f9003e9b4..abc94d617 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
@@ -52,6 +52,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *, bool control_yaw_position);
+	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */