From bbfe78e4f60de5414cf3594250c5ed769f0a1433 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 13:50:29 +0100 Subject: mc att ctl: fix subscription handlers, fix parameters --- src/modules/mc_att_control/mc_att_control.cpp | 20 ++++++++++++-------- src/modules/mc_att_control/mc_att_control.h | 4 +++- src/modules/mc_att_control/mc_att_control_base.h | 4 ++-- src/modules/mc_att_control/mc_att_control_params.c | 3 ++- 4 files changed, 19 insertions(+), 12 deletions(-) (limited to 'src/modules/mc_att_control') 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 */ -#include "mc_att_control_params.h"" +#include +#include "mc_att_control_params.h" #include /** -- cgit v1.2.3