Changed yaw position control to yaw speed control for multirotors, tested with ardrone

4 files changed, 44 insertions, 19 deletions

diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 3e485274e..4790a673d 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -335,6 +335,11 @@ handle_message(mavlink_message_t *msg)
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
 
+			int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+			/* get a copy first, to prevent altering values that are not sent by the mavlink command */
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
+
 			mc.timestamp = rc_hil.timestamp;
 			mc.roll = man.x / 1000.0f;
 			mc.pitch = man.y / 1000.0f;
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 337a45215..91439d36d 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -155,6 +155,9 @@ mc_thread_main(int argc, char *argv[])
 	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
 
+	/* store if we stopped a yaw movement */
+	bool first_time_after_yaw_speed_control = true;
+
 	/* prepare the handle for the failsafe throttle */
 	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
 	float failsafe_throttle = 0.0f;
@@ -249,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_body = att.yaw;
+							att_sp.yaw_tait_bryan = att.yaw;
 						}
 
 						static bool rc_loss_first_time = true;
@@ -294,16 +297,19 @@ mc_thread_main(int argc, char *argv[])
 								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 {
-									att_sp.yaw_body = 0.0f;
+									rates_sp.yaw = 0.0f;
+									if(first_time_after_yaw_speed_control) {
+										att_sp.yaw_tait_bryan = att.yaw;
+										first_time_after_yaw_speed_control = false;
+									}
 									control_yaw_position = true;
 								}
 							}
 
 							att_sp.thrust = manual.throttle;
 							att_sp.timestamp = hrt_absolute_time();
-
-							//rates_sp.yaw = manual.yaw;
 						}
 					}
 					/* STEP 2: publish the result to the vehicle actuators */
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index bf908c6dd..87eeaced5 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -181,6 +181,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	static bool initialized = false;
 
+	static float yaw_error;
+
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
 		parameters_init(&h);
@@ -218,7 +220,15 @@ 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_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
+		//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;
+		}
+
+		rates_sp->yaw = - p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
 	}
 	rates_sp->thrust = att_sp->thrust;
 
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index eea51cc1e..aa1362d3a 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -151,6 +151,7 @@ private:
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;			/**< notification of manual control updates */
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
 	orb_advert_t	_manual_control_pub;		/**< manual control signal topic */
@@ -341,6 +342,7 @@ Sensors::Sensors() :
 	_baro_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
+	_manual_control_sub(-1),
 
 	/* publications */
 	_sensor_pub(-1),
@@ -903,6 +905,9 @@ Sensors::ppm_poll()
 
 		struct manual_control_setpoint_s manual_control;
 
+		/* get a copy first, to prevent altering values */
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control);
+
 		/* require at least four channels to consider the signal valid */
 		if (rc_input.channel_count < 4)
 			return;
@@ -1023,6 +1028,7 @@ Sensors::task_main()
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -1052,20 +1058,18 @@ Sensors::task_main()
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* advertise the manual_control topic */
-	{
-		struct manual_control_setpoint_s manual_control;
-		manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
-		manual_control.roll = 0.0f;
-		manual_control.pitch = 0.0f;
-		manual_control.yaw = 0.0f;
-		manual_control.throttle = 0.0f;
-		manual_control.aux1_cam_pan_flaps = 0.0f;
-		manual_control.aux2_cam_tilt = 0.0f;
-		manual_control.aux3_cam_zoom = 0.0f;
-		manual_control.aux4_cam_roll = 0.0f;
-
-		_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
-	}
+	struct manual_control_setpoint_s manual_control;
+	manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
+	manual_control.roll = 0.0f;
+	manual_control.pitch = 0.0f;
+	manual_control.yaw = 0.0f;
+	manual_control.throttle = 0.0f;
+	manual_control.aux1_cam_pan_flaps = 0.0f;
+	manual_control.aux2_cam_tilt = 0.0f;
+	manual_control.aux3_cam_zoom = 0.0f;
+	manual_control.aux4_cam_roll = 0.0f;
+
+	_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 
 	/* advertise the rc topic */
 	{