mc att ctl: fix subscription handlers, fix parameters

4 files changed, 19 insertions, 12 deletions

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 906bd0a47..89da8438b 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -104,13 +104,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	/*
 	 * do subscriptions
 	 */
-	PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
-	PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
-	PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
-	PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
-	PX4_SUBSCRIBE(_n, parameter_update, 0);
-	PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
-	PX4_SUBSCRIBE(_n, actuator_armed, 0);
+	_v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
+	_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
+	_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
+	PX4_SUBSCRIBE(_n, parameter_update, MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+	_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
+	_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
 
 }
 
@@ -178,6 +178,11 @@ MulticopterAttitudeControl::parameters_update()
 	return OK;
 }
 
+void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg)
+{
+	parameters_update();
+}
+
 void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
 
 	perf_begin(_loop_perf);
@@ -250,7 +255,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 		} else {
 			/* attitude controller disabled, poll rates setpoint topic */
-			//XXX vehicle_rates_setpoint_poll();
 			_rates_sp(0) = _v_rates_sp->get().roll;
 			_rates_sp(1) = _v_rates_sp->get().pitch;
 			_rates_sp(2) = _v_rates_sp->get().yaw;
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 1da738408..76c095cc1 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -79,7 +79,9 @@ public:
 	 */
 	~MulticopterAttitudeControl();
 
-	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+	/* Callbacks for topics */
+	void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+	void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg);
 
 	void spin() { _n.spin(); }
 
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 2135b61d8..17c779796 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -90,8 +90,8 @@ protected:
 	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;  /**< manual control setpoint */
 	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
 	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
-	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) * _v_att_sp;	/**< vehicle attitude setpoint */
-	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) * _v_rates_sp;	/**< vehicle rates setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	/**< vehicle attitude setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	/**< vehicle rates setpoint */
 
 	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
 								  that gets published eventually */
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index bbbfd829b..a0b1706f2 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -40,7 +40,8 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include "mc_att_control_params.h""
+#include <px4_defines.h>
+#include "mc_att_control_params.h"
 #include <systemlib/param/param.h>
 
 /**