From 2d59ead1bf3676e1f9876b533e6a56beec3af55e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 14 Sep 2018 03:56:21 +0200 Subject: [PATCH] simulator: add MAV_TYPE to module parameters (#10476) - this ensures that the MAV_TYPE parameter is always updated - previously it could happen that the simulator module was using a wrong MAV_TYPE value because it only read the parameter value at initialisation Signed-off-by: Roman --- src/modules/simulator/simulator.h | 14 +++----------- src/modules/simulator/simulator_mavlink.cpp | 2 ++ 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index fafc61baf6..994637c790 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -251,9 +251,7 @@ private: _param_sub(-1), _initialized(false), _realtime_factor(1.0), - _system_type(0) #ifndef __PX4_QURT - , _rc_channels_pub(nullptr), _attitude_pub(nullptr), _gpos_pub(nullptr), @@ -275,11 +273,6 @@ private: _vehicle_status{} #endif { - // We need to know the type for the correct mapping from - // actuator controls to the hil actuator message. - param_t param_system_type = param_find("MAV_TYPE"); - param_get(param_system_type, &_system_type); - for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) { _actuator_outputs_sub[i] = -1; @@ -347,9 +340,6 @@ private: Battery _battery; battery_status_s _battery_status{}; - // For param MAV_TYPE - int32_t _system_type; - // class methods int publish_sensor_topics(mavlink_hil_sensor_t *imu); int publish_flow_topic(mavlink_hil_optical_flow_t *flow); @@ -385,7 +375,9 @@ private: struct vehicle_status_s _vehicle_status; DEFINE_PARAMETERS( - (ParamFloat) _battery_drain_interval_s ///< battery drain interval + (ParamFloat) _battery_drain_interval_s, ///< battery drain interval + (ParamInt) _param_system_type + ) void poll_topics(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index e2ad359e5b..87d392192d 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -85,6 +85,8 @@ void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsi const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2; + int _system_type = _param_system_type.get(); + /* scale outputs depending on system type */ if (_system_type == MAV_TYPE_QUADROTOR || _system_type == MAV_TYPE_HEXAROTOR ||