diff options
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_base.cpp | 8 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 12 |
2 files changed, 16 insertions, 4 deletions
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 d7fbf3acd..2315e0a99 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -58,8 +58,8 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _publish_att_sp(false) { - memset(&_v_rates_sp, 0, sizeof(_v_att_sp_mod)); - memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod)); + memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); + memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); memset(&_actuators, 0, sizeof(_actuators)); _params.att_p.zero(); @@ -98,7 +98,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_control_mode->get().flag_control_velocity_enabled || _v_control_mode->get().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - _v_att_sp_mod = _v_att_sp->get(); + memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); } if (!_v_control_mode->get().flag_control_climb_rate_enabled) { @@ -157,7 +157,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - _v_att_sp_mod = _v_att_sp->get(); + memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8c299c748..e7e344201 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -80,6 +80,10 @@ public: * Get the last message value */ virtual const M& get() = 0; + /** + * Get void pointer to last message value + */ + virtual void * get_void_ptr() = 0; }; #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) @@ -121,6 +125,10 @@ public: * Get the last message value */ const M& get() { return _msg_current; } + /** + * Get void pointer to last message value + */ + void * get_void_ptr() { return (void*)&_msg_current; } protected: /** @@ -197,6 +205,10 @@ public: * Get the last message value */ const M& get() { return uORB::Subscription<M>::getData(); } + /** + * Get void pointer to last message value + */ + void * get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); } }; template<typename M> |