diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 31cdd99e4d..0028127417 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -211,84 +211,6 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void -MulticopterAttitudeControl::parameter_update_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -void -MulticopterAttitudeControl::vehicle_control_mode_poll() -{ - bool updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_v_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); - } -} - -void -MulticopterAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_att_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); - } -} - -void -MulticopterAttitudeControl::vehicle_rates_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_v_rates_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); - } -} - -void -MulticopterAttitudeControl::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool updated; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - } -} - - void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { perf_begin(_loop_perf); @@ -306,15 +228,6 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi dt = 0.02f; } - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - - /* check for updates in other topics */ - parameter_update_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); - if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -373,7 +286,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); + //XXX vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 48df887714..b2ec3083f6 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -139,36 +139,5 @@ private: * Update our local parameter cache. */ int parameters_update(); - - /** - * Check for parameter update and handle it. - */ - void parameter_update_poll(); - - /** - * Check for changes in vehicle control mode. - */ - void vehicle_control_mode_poll(); - - /** - * Check for changes in manual inputs. - */ - void vehicle_manual_poll(); - - /** - * Check for attitude setpoint updates. - */ - void vehicle_attitude_setpoint_poll(); - - /** - * Check for rates setpoint updates. - */ - void vehicle_rates_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); - }; diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d0ab1bfbf8..0a05be9245 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -101,7 +101,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - vehicle_attitude_setpoint_poll(); + //XXX vehicle_attitude_setpoint_poll(); } if (!_v_control_mode.flag_control_climb_rate_enabled) { @@ -160,7 +160,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - vehicle_attitude_setpoint_poll(); + //XXX vehicle_attitude_setpoint_poll(); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; 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 54a445e0df..0783bb71ef 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -120,8 +120,6 @@ protected: bool _publish_att_sp; - virtual void vehicle_attitude_setpoint_poll() = 0; - }; #endif /* MC_ATT_CONTROL_BASE_H_ */