mc_att_control: implemented anti windup

3 files changed, 49 insertions, 2 deletions

diff --git a/msg/mc_att_ctrl_status.msg b/msg/mc_att_ctrl_status.msg
new file mode 100644
index 000000000..114e843b0
--- /dev/null
+++ b/msg/mc_att_ctrl_status.msg
@@ -0,0 +1,5 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll_rate_integ	    # roll rate inegrator
+float32 pitch_rate_integ	# pitch rate integrator
+float32 yaw_rate_integ	    # yaw rate integrator
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index d47731622..b8c143782 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -77,6 +77,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/multirotor_motor_limits.h>
+#include <uORB/topics/mc_att_ctrl_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
@@ -129,10 +131,12 @@ private:
 	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
 	int		_armed_sub;				/**< arming status subscription */
 	int		_vehicle_status_sub;	/**< vehicle status subscription */
+	int 	_motor_limits_sub;		/**< motor limits subscription */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
+	orb_advert_t	_controller_status_pub;	/**< controller status publication */
 
 	orb_id_t _rates_sp_id;	/**< pointer to correct rates setpoint uORB metadata structure */
 	orb_id_t _actuators_id;	/**< pointer to correct actuator controls0 uORB metadata structure */
@@ -147,6 +151,8 @@ private:
 	struct actuator_controls_s			_actuators;			/**< actuator controls */
 	struct actuator_armed_s				_armed;				/**< actuator arming status */
 	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+	struct multirotor_motor_limits_s	_motor_limits;		/**< motor limits */
+	struct mc_att_ctrl_status_s 		_controller_status; /**< controller status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	perf_counter_t	_controller_latency_perf;
@@ -262,6 +268,11 @@ private:
 	void		vehicle_status_poll();
 
 	/**
+	 * Check for vehicle motor limits status.
+	 */
+	void		vehicle_motor_limits_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -302,6 +313,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
+	_controller_status_pub(-1),
 	_rates_sp_id(0),
 	_actuators_id(0),
 
@@ -320,6 +332,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	memset(&_actuators, 0, sizeof(_actuators));
 	memset(&_armed, 0, sizeof(_armed));
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
+	memset(&_motor_limits, 0, sizeof(_motor_limits));
+	memset(&_controller_status,0,sizeof(_controller_status));
 	_vehicle_status.is_rotary_wing = true;
 
 	_params.att_p.zero();
@@ -570,6 +584,18 @@ MulticopterAttitudeControl::vehicle_status_poll()
 	}
 }
 
+void
+MulticopterAttitudeControl::vehicle_motor_limits_poll()
+{
+	/* check if there is a new message */
+	bool updated;
+	orb_check(_motor_limits_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
+	}
+}
+
 /*
  * Attitude controller.
  * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -689,8 +715,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp);
 	_rates_prev = rates;
 
-	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+	/* update integral only if not saturated on low limit and if motor commands are not saturated */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) {
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
@@ -725,6 +751,7 @@ MulticopterAttitudeControl::task_main()
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
 
 	/* initialize parameters cache */
 	parameters_update();
@@ -777,6 +804,7 @@ MulticopterAttitudeControl::task_main()
 			arming_status_poll();
 			vehicle_manual_poll();
 			vehicle_status_poll();
+			vehicle_motor_limits_poll();
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -837,6 +865,11 @@ MulticopterAttitudeControl::task_main()
 				_actuators.timestamp = hrt_absolute_time();
 				_actuators.timestamp_sample = _v_att.timestamp;
 
+				_controller_status.roll_rate_integ = _rates_int(0);
+				_controller_status.pitch_rate_integ = _rates_int(1);
+				_controller_status.yaw_rate_integ = _rates_int(2);
+				_controller_status.timestamp = hrt_absolute_time();
+
 				if (!_actuators_0_circuit_breaker_enabled) {
 					if (_actuators_0_pub > 0) {
 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
@@ -847,6 +880,13 @@ MulticopterAttitudeControl::task_main()
 					}
 
 				}
+
+				/* publish controller status */
+				if(_controller_status_pub > 0) {
+					orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
+				} else {
+					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
+				}
 			}
 		}
 
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 75977ffd1..1a38b981e 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -254,3 +254,5 @@ ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s);
 #include "topics/time_offset.h"
 ORB_DEFINE(time_offset, struct time_offset_s);
 
+#include "topics/mc_att_ctrl_status.h"
+ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s);