mc_att_control_vector: use rotation matrix for attitude setpoint, move yaw setpoint with stick, WIP

1 files changed, 110 insertions, 36 deletions

diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
index 1783efc0e..0faba307d 100644
--- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
@@ -75,6 +75,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
 
 /**
  * Multicopter attitude control app start / stop handling function
@@ -83,6 +84,9 @@
  */
 extern "C" __EXPORT int mc_att_control_vector_main(int argc, char *argv[]);
 
+#define MIN_TAKEOFF_THROTTLE	0.3f
+#define YAW_DEADZONE	0.01f
+
 class MulticopterAttitudeControl
 {
 public:
@@ -116,7 +120,8 @@ private:
 	int 		_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
 
-	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
+	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_rates_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
@@ -125,7 +130,7 @@ private:
 	struct vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
-	struct vehicle_rates_setpoint_s		_rates_setpoint;		/**< vehicle rates setpoint */
+	struct vehicle_rates_setpoint_s		_rates_sp;		/**< vehicle rates setpoint */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -208,7 +213,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_arming_sub(-1),
 
 /* publications */
-	_rate_sp_pub(-1),
+	_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
 
 /* performance counters */
@@ -368,6 +373,8 @@ MulticopterAttitudeControl::task_main()
 	vehicle_manual_poll();
 	arming_status_poll();
 
+	bool reset_yaw_sp = true;
+
 	/* wakeup source(s) */
 	struct pollfd fds[2];
 
@@ -406,12 +413,12 @@ MulticopterAttitudeControl::task_main()
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
 			static uint64_t last_run = 0;
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
 
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
-				deltaT = 0.01f;
+			/* guard against too large dt's */
+			if (dt > 0.02f)
+				dt = 0.02f;
 
 			/* copy attitude topic */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
@@ -421,28 +428,93 @@ MulticopterAttitudeControl::task_main()
 			arming_status_poll();
 			vehicle_manual_poll();
 
-			float roll_sp = 0.0f;
-			float pitch_sp = 0.0f;
-			float throttle_sp = 0.0f;
-			float yaw_sp = 0.0f;
+			float yaw_sp_move_rate = 0.0f;
+			bool publish_att_sp = false;
 
-			/* decide if in auto or manual control */
+			/* define which input is the dominating control input */
 			if (_control_mode.flag_control_manual_enabled) {
-				roll_sp = _manual.roll;
-				pitch_sp = _manual.pitch;
-				yaw_sp = _manual.yaw;	// XXX move yaw setpoint
-				throttle_sp = _manual.throttle;
-
-			} else if (_control_mode.flag_control_auto_enabled) {
-				roll_sp = _att_sp.roll_body;
-				pitch_sp = _att_sp.pitch_body;
-				yaw_sp = _att_sp.yaw_body;
-				throttle_sp = _att_sp.thrust;
+				/* manual input */
+				if (!_control_mode.flag_control_climb_rate_enabled) {
+					/* pass throttle directly if not in altitude control mode */
+					_att_sp.thrust = _manual.throttle;
+				}
+
+				if (_control_mode.flag_control_attitude_enabled) {
+					/* control attitude, update attitude setpoint depending on mode */
+
+					if (_att_sp.thrust < 0.1f) {
+						// TODO
+						//if (_status.condition_landed) {
+							/* reset yaw setpoint if on ground */
+						//	reset_yaw_sp = true;
+						//}
+					} else {
+						if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
+							/* move yaw setpoint */
+							yaw_sp_move_rate = _manual.yaw;
+							_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
+							publish_att_sp = true;
+						}
+					}
+
+					/* reset yaw setpint to current position if needed */
+					if (reset_yaw_sp) {
+						reset_yaw_sp = false;
+						_att_sp.yaw_body = _att.yaw;
+						publish_att_sp = true;
+					}
+
+					if (!_control_mode.flag_control_velocity_enabled) {
+						/* update attitude setpoint if not in position control mode */
+						_att_sp.roll_body = _manual.roll;
+						_att_sp.pitch_body = _manual.pitch;
+						publish_att_sp = true;
+					}
+				} else {
+					/* manual rate inputs (ACRO) */
+					// TODO
+					/* reset yaw setpoint after ACRO */
+					reset_yaw_sp = true;
+				}
+
+			} else {
+				if (!_control_mode.flag_control_auto_enabled) {
+					/* no control, try to stay on place */
+					if (!_control_mode.flag_control_velocity_enabled) {
+						/* no velocity control, reset attitude setpoint */
+						_att_sp.roll_body = 0.0f;
+						_att_sp.pitch_body = 0.0f;
+						publish_att_sp = true;
+					}
+				}
+
+				/* reset yaw setpoint after non-manual control */
+				reset_yaw_sp = true;
+			}
+
+			if (publish_att_sp || !_att_sp.R_valid) {
+				/* controller uses rotation matrix, not euler angles, convert if necessary */
+				math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
+				math::Dcm R_sp(euler_sp);
+				for (int i = 0; i < 9; i++) {
+					_att_sp.R_body[i] = R_sp(i / 3, i % 3);
+				}
+				_att_sp.R_valid = true;
+			}
+
+			if (publish_att_sp) {
+				/* publish the attitude setpoint */
+				_att_sp.timestamp = hrt_absolute_time();
+				if (_att_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+				} else {
+					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+				}
 			}
 
-			/* construct rotation matrix from euler angles */
-			math::EulerAngles euler(roll_sp, pitch_sp, yaw_sp);
-			math::Dcm R_des(euler);
+			/* desired rotation matrix */
+			math::Dcm R_des(&_att_sp.R_body[0]);
 
 			/* rotation matrix for current state */
 			math::Dcm R(_att.R);
@@ -454,28 +526,30 @@ MulticopterAttitudeControl::task_main()
 			math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1));
 
 			/* angular rates setpoint*/
-			math::Vector3 rates_sp = K_p * e_R;
-			math::Vector3 control = K_r_p * (rates_sp - rates);
+			math::Vector3 rates_sp = _K * e_R;
+			/* feed forward yaw setpoint rate */
+			rates_sp(2) += yaw_sp_move_rate;
+			math::Vector3 control = _K_rate * (rates_sp - rates);
 
 			/* publish the attitude rates setpoint */
-			_rates_setpoint.roll = rates_sp(0);
-			_rates_setpoint.pitch = rates_sp(1);
-			_rates_setpoint.yaw = rates_sp(2);
-			_rates_setpoint.thrust = throttle_sp;
-			_rates_setpoint.timestamp = hrt_absolute_time();
+			_rates_sp.roll = rates_sp(0);
+			_rates_sp.pitch = rates_sp(1);
+			_rates_sp.yaw = rates_sp(2);
+			_rates_sp.thrust = _att_sp.thrust;
+			_rates_sp.timestamp = hrt_absolute_time();
 
-			if (_rate_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &_rates_setpoint);
+			if (_rates_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp);
 
 			} else {
-				_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_setpoint);
+				_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
 			}
 
 			/* publish the attitude controls */
 			_actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f;
 			_actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f;
 			_actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f;
-			_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+			_actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f;
 			_actuators.timestamp = hrt_absolute_time();
 
 			if (_actuators_0_pub > 0) {