diff options
Diffstat (limited to 'src/modules/mc_att_control')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control.cpp | 32 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control.h | 4 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_base.cpp | 2 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_base.h | 10 |
4 files changed, 39 insertions, 9 deletions
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp index 89da8438b..90a6c90fe 100644 --- a/src/modules/mc_att_control/mc_att_control.cpp +++ b/src/modules/mc_att_control/mc_att_control.cpp @@ -97,9 +97,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); + _params_handles.autostart_id = PX4_PARAM_INIT(SYS_AUTOSTART); /* fetch initial parameter values */ parameters_update(); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _is_vtol = true; + } + else { + _is_vtol = false; + } /* * do subscriptions @@ -108,9 +116,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _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); + _parameter_update = 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); + _v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0); } @@ -204,7 +214,7 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi control_attitude(dt); /* publish the attitude setpoint if needed */ - if (_publish_att_sp) { + if (_publish_att_sp && _v_status->get().is_rotary_wing) { _v_att_sp_mod.timestamp = px4::get_time_micros(); if (_att_sp_pub != nullptr) { @@ -225,7 +235,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + if (_is_vtol) { + _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + } else { + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + } } } else { @@ -250,7 +264,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + if (_is_vtol) { + _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + } else { + _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + } } } else { @@ -277,7 +295,11 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi _actuators_0_pub->publish(_actuators); } else { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + if (_is_vtol) { + _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); + } else { + _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + } } } } diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h index 76c095cc1..33552c269 100644 --- a/src/modules/mc_att_control/mc_att_control.h +++ b/src/modules/mc_att_control/mc_att_control.h @@ -93,6 +93,8 @@ private: px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + bool _is_vtol; /**< true if vehicle is vtol, to be replaced with global API */ + px4::NodeHandle _n; struct { @@ -117,6 +119,8 @@ private: px4_param_t acro_roll_max; px4_param_t acro_pitch_max; px4_param_t acro_yaw_max; + + px4_param_t autostart_id; } _params_handles; /**< handles for interesting parameters */ perf_counter_t _loop_perf; /**< loop performance counter */ 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 871f93ab8..d7fbf3acd 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -266,7 +266,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed->get().armed) { + if (!_armed->get().armed || !_v_status->get().is_rotary_wing) { _rates_int.zero(); } 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 17c779796..cf4c726a7 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -87,11 +87,13 @@ public: protected: px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */ - px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control 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::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ + px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */ + px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ 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_status) *_v_status; /**< vehicle status */ PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint that gets published eventually */ @@ -121,6 +123,8 @@ protected: float man_pitch_max; float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + int32_t autostart_id; } _params; bool _publish_att_sp; |