diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 9e49bc016f..611fea9e85 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -99,6 +99,7 @@ set(msg_files GimbalManagerStatus.msg GpsDump.msg GpsInjectData.msg + GimbalControls.msg Gripper.msg HealthReport.msg HeaterStatus.msg diff --git a/msg/GimbalControls.msg b/msg/GimbalControls.msg new file mode 100644 index 0000000000..5c4325fdc2 --- /dev/null +++ b/msg/GimbalControls.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 + +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[3] control + +float32 retract_gimbal diff --git a/src/lib/mixer_module/functions/FunctionGimbal.hpp b/src/lib/mixer_module/functions/FunctionGimbal.hpp index 513a344460..22d449dbf0 100644 --- a/src/lib/mixer_module/functions/FunctionGimbal.hpp +++ b/src/lib/mixer_module/functions/FunctionGimbal.hpp @@ -35,7 +35,7 @@ #include "FunctionProviderBase.hpp" -#include +#include /** * Functions: Gimbal_Roll .. Gimbal_Yaw @@ -48,18 +48,18 @@ public: void update() override { - actuator_controls_s actuator_controls; + gimbal_controls_s gimbal_controls; - if (_topic.update(&actuator_controls)) { - _data[0] = actuator_controls.control[0]; - _data[1] = actuator_controls.control[1]; - _data[2] = actuator_controls.control[2]; + if (_topic.update(&gimbal_controls)) { + _data[0] = gimbal_controls.control[gimbal_controls_s::INDEX_ROLL]; + _data[1] = gimbal_controls.control[gimbal_controls_s::INDEX_PITCH]; + _data[2] = gimbal_controls.control[gimbal_controls_s::INDEX_YAW]; } } float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; } private: - uORB::Subscription _topic{ORB_ID(actuator_controls_2)}; + uORB::Subscription _topic{ORB_ID(gimbal_controls)}; float _data[3] { NAN, NAN, NAN }; }; diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index bb6039ac5a..5d3c174c20 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -34,7 +34,7 @@ #include "output_rc.h" -#include +#include #include #include @@ -62,25 +62,25 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints) _stream_device_attitude_status(); - // _angle_outputs are in radians, actuator_controls are in [-1, 1] - actuator_controls_s actuator_controls{}; - actuator_controls.control[0] = constrain( - (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * - (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), - -1.f, 1.f); - actuator_controls.control[1] = constrain( - (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * - (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), - -1.f, 1.f); - actuator_controls.control[2] = constrain( - (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * - (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), - -1.f, 1.f); - actuator_controls.control[3] = constrain( - _retract_gimbal ? _parameters.mnt_ob_lock_mode : _parameters.mnt_ob_norm_mode, - -1.f, 1.f); - actuator_controls.timestamp = hrt_absolute_time(); - _actuator_controls_pub.publish(actuator_controls); + // _angle_outputs are in radians, gimbal_controls are in [-1, 1] + gimbal_controls_s gimbal_controls{}; + gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( + (_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)) * + (1.0f / (math::radians(_parameters.mnt_range_roll / 2.0f))), + -1.f, 1.f); + gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( + (_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)) * + (1.0f / (math::radians(_parameters.mnt_range_pitch / 2.0f))), + -1.f, 1.f); + gimbal_controls.control[gimbal_controls_s::INDEX_ROLL] = constrain( + (_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)) * + (1.0f / (math::radians(_parameters.mnt_range_yaw / 2.0f))), + -1.f, 1.f); + gimbal_controls.retract_gimbal = constrain( + _retract_gimbal ? _parameters.mnt_ob_lock_mode : _parameters.mnt_ob_norm_mode, + -1.f, 1.f); + gimbal_controls.timestamp = hrt_absolute_time(); + _gimbal_controls_pub.publish(gimbal_controls); _last_update = t; } diff --git a/src/modules/gimbal/output_rc.h b/src/modules/gimbal/output_rc.h index 5bb36ce2fc..0f69537981 100644 --- a/src/modules/gimbal/output_rc.h +++ b/src/modules/gimbal/output_rc.h @@ -37,7 +37,7 @@ #include "output.h" #include -#include +#include #include namespace gimbal @@ -56,7 +56,7 @@ public: private: void _stream_device_attitude_status(); - uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_2)}; + uORB::Publication _gimbal_controls_pub{ORB_ID(gimbal_controls)}; uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; bool _retract_gimbal = true; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c242b5b3a0..31ea1bad68 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -69,6 +69,7 @@ void LoggedTopics::add_default_topics() add_topic("gimbal_manager_set_attitude", 500); add_optional_topic("generator_status"); add_optional_topic("gps_dump"); + add_optional_topic("gimbal_controls", 200); add_optional_topic("gripper"); add_optional_topic("heater_status"); add_topic("home_position");