diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 660a9da667d..cf88f56ea08 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -73,12 +73,14 @@ static px4::atomic thread_should_exit {false}; static px4::atomic thread_running {false}; static constexpr int input_objs_len_max = 3; +static constexpr int output_objs_len_max = 2; struct ThreadData { InputBase *input_objs[input_objs_len_max] = {nullptr, nullptr, nullptr}; int input_objs_len = 0; int last_input_active = -1; - OutputBase *output_obj = nullptr; + OutputBase *output_objs[output_objs_len_max] = {nullptr, nullptr}; + int output_objs_len = 0; InputTest *test_input = nullptr; ControlData control_data {}; }; @@ -88,10 +90,29 @@ static ThreadData *g_thread_data = nullptr; static void usage(); static void update_params(ParameterHandles ¶m_handles, Parameters ¶ms); static bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms); +static OutputBase *create_output(int32_t mode, const Parameters ¶ms); static int gimbal_thread_main(int argc, char *argv[]); extern "C" __EXPORT int gimbal_main(int argc, char *argv[]); +static OutputBase *create_output(int32_t mode, const Parameters ¶ms) +{ + switch (mode) { + case MNT_MODE_OUT_AUX: + return new OutputRC(params); + + case MNT_MODE_OUT_MAVLINK_V1: + return new OutputMavlinkV1(params); + + case MNT_MODE_OUT_MAVLINK_V2: + return new OutputMavlinkV2(params); + + default: + PX4_ERR("invalid output mode %" PRId32, mode); + return nullptr; + } +} + static int gimbal_thread_main(int argc, char *argv[]) { ParameterHandles param_handles; @@ -169,40 +190,40 @@ static int gimbal_thread_main(int argc, char *argv[]) } } - switch (params.mnt_mode_out) { - case MNT_MODE_OUT_AUX: //AUX - thread_data.output_obj = new OutputRC(params); + const int32_t out_modes[output_objs_len_max] = {params.mnt_mode_out, params.mnt_mode_out2}; - if (!thread_data.output_obj) { alloc_failed = true; } - - break; - - case MNT_MODE_OUT_MAVLINK_V1: //MAVLink gimbal v1 protocol - thread_data.output_obj = new OutputMavlinkV1(params); - - if (!thread_data.output_obj) { alloc_failed = true; } - - break; - - case MNT_MODE_OUT_MAVLINK_V2: //MAVLink gimbal v2 protocol - thread_data.output_obj = new OutputMavlinkV2(params); - - if (!thread_data.output_obj) { alloc_failed = true; } - - break; - - default: - PX4_ERR("invalid output mode %" PRId32, params.mnt_mode_out); - thread_should_exit.store(true); - break; - } - - if (alloc_failed) { - PX4_ERR("output memory allocation failed"); + // The two outputs must use different modes: two instances of the same output + // would fight over the same device/topic (e.g. two AUX outputs both driving + // gimbal_controls). + if (params.mnt_mode_out2 != MNT_MODE_OUT_DISABLED && params.mnt_mode_out2 == params.mnt_mode_out) { + PX4_ERR("MNT_MODE_OUT2 must differ from MNT_MODE_OUT"); thread_should_exit.store(true); } - const unsigned int poll_timeout_ms = params.mnt_mode_out == MNT_MODE_OUT_AUX ? 10 : 20; + for (int i = 0; i < output_objs_len_max && !thread_should_exit.load(); ++i) { + if (out_modes[i] == MNT_MODE_OUT_DISABLED) { + continue; + } + + OutputBase *output = create_output(out_modes[i], params); + + if (!output) { + // Invalid mode already logged by create_output(). + thread_should_exit.store(true); + break; + } + + thread_data.output_objs[thread_data.output_objs_len++] = output; + } + + if (thread_data.output_objs_len == 0) { + PX4_ERR("no output configured"); + thread_should_exit.store(true); + } + + const bool any_aux_output = + (params.mnt_mode_out == MNT_MODE_OUT_AUX) || (params.mnt_mode_out2 == MNT_MODE_OUT_AUX); + const unsigned int poll_timeout_ms = any_aux_output ? 10 : 20; while (!thread_should_exit.load()) { @@ -261,42 +282,49 @@ static int gimbal_thread_main(int argc, char *argv[]) } } + bool stabilize[3]; + switch (params.mnt_do_stab) { - case MntDoStabilize::ALL_AXES: { - thread_data.output_obj->set_stabilize(true, true, true); - break; - } + case MntDoStabilize::ALL_AXES: + stabilize[0] = true; stabilize[1] = true; stabilize[2] = true; + break; - case MntDoStabilize::YAW_LOCK: { - thread_data.output_obj->set_stabilize(false, false, true); - break; - } + case MntDoStabilize::YAW_LOCK: + stabilize[0] = false; stabilize[1] = false; stabilize[2] = true; + break; - case MntDoStabilize::PITCH_LOCK: { - thread_data.output_obj->set_stabilize(false, true, false); - break; - } + case MntDoStabilize::PITCH_LOCK: + stabilize[0] = false; stabilize[1] = true; stabilize[2] = false; + break; - default: { - thread_data.output_obj->set_stabilize(false, false, false); - break; - } + default: + stabilize[0] = false; stabilize[1] = false; stabilize[2] = false; + break; } - if (thread_data.output_obj->check_and_handle_setpoint_timeout(thread_data.control_data, hrt_absolute_time())) { + // The setpoint timeout operates on control_data and is independent of the + // output, so it only needs to run once for the shared control_data. + if (thread_data.output_objs[0]->check_and_handle_setpoint_timeout( + thread_data.control_data, hrt_absolute_time())) { // Without flagging an update the changes are not processed in the output update_result = InputBase::UpdateResult::UpdatedActive; } - // Update output - thread_data.output_obj->update( - thread_data.control_data, - update_result != InputBase::UpdateResult::NoUpdate, thread_data.control_data.device_compid); + const bool new_setpoints = (update_result != InputBase::UpdateResult::NoUpdate); - // Only publish the mount orientation if the mode is not mavlink v1 or v2 - // If the gimbal speaks mavlink it publishes its own orientation. - if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2) { - thread_data.output_obj->publish(); + // Fan the same setpoints out to all configured outputs. + for (int i = 0; i < thread_data.output_objs_len; ++i) { + OutputBase *output = thread_data.output_objs[i]; + + output->set_stabilize(stabilize[0], stabilize[1], stabilize[2]); + + output->update(thread_data.control_data, new_setpoints, thread_data.control_data.device_compid); + + // Only publish the mount orientation for outputs that don't report it + // themselves. A MAVLink gimbal publishes its own orientation. + if (output->publishes_mount_orientation()) { + output->publish(); + } } } else { @@ -316,11 +344,15 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.input_objs_len = 0; - if (thread_data.output_obj) { - delete (thread_data.output_obj); - thread_data.output_obj = nullptr; + for (int i = 0; i < output_objs_len_max; ++i) { + if (thread_data.output_objs[i]) { + delete (thread_data.output_objs[i]); + thread_data.output_objs[i] = nullptr; + } } + thread_data.output_objs_len = 0; + thread_running.store(false); return 0; } @@ -523,8 +555,10 @@ int gimbal_main(int argc, char *argv[]) } - if (g_thread_data->output_obj) { - g_thread_data->output_obj->print_status(); + if (g_thread_data->output_objs_len > 0) { + for (int i = 0; i < g_thread_data->output_objs_len; ++i) { + g_thread_data->output_objs[i]->print_status(); + } } else { PX4_INFO("Output: None"); @@ -546,6 +580,7 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) { param_get(param_handles.mnt_mode_in, ¶ms.mnt_mode_in); param_get(param_handles.mnt_mode_out, ¶ms.mnt_mode_out); + param_get(param_handles.mnt_mode_out2, ¶ms.mnt_mode_out2); param_get(param_handles.mnt_mav_sysid_v1, ¶ms.mnt_mav_sysid_v1); param_get(param_handles.mnt_mav_compid_v1, ¶ms.mnt_mav_compid_v1); param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch); @@ -571,6 +606,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) { param_handles.mnt_mode_in = param_find("MNT_MODE_IN"); param_handles.mnt_mode_out = param_find("MNT_MODE_OUT"); + param_handles.mnt_mode_out2 = param_find("MNT_MODE_OUT2"); param_handles.mnt_mav_sysid_v1 = param_find("MNT_MAV_SYSID"); param_handles.mnt_mav_compid_v1 = param_find("MNT_MAV_COMPID"); param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH"); @@ -593,6 +629,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) if (param_handles.mnt_mode_in == PARAM_INVALID || param_handles.mnt_mode_out == PARAM_INVALID || + param_handles.mnt_mode_out2 == PARAM_INVALID || param_handles.mnt_mav_sysid_v1 == PARAM_INVALID || param_handles.mnt_mav_compid_v1 == PARAM_INVALID || param_handles.mnt_man_pitch == PARAM_INVALID || diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index 6b00b9e74ad..243daff9788 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -51,6 +51,7 @@ enum MntModeIn { }; enum MntModeOut { + MNT_MODE_OUT_DISABLED = -1, // Only valid for the secondary output (MNT_MODE_OUT2) MNT_MODE_OUT_AUX = 0, MNT_MODE_OUT_MAVLINK_V1, // MAVLink gimbal protocol v1 (to be deprecated) MNT_MODE_OUT_MAVLINK_V2 // MAVLink gimbal protocol v2 @@ -59,6 +60,7 @@ enum MntModeOut { struct Parameters { int32_t mnt_mode_in; int32_t mnt_mode_out; + int32_t mnt_mode_out2; int32_t mnt_mav_sysid_v1; int32_t mnt_mav_compid_v1; int32_t mnt_man_pitch; @@ -83,6 +85,7 @@ struct Parameters { struct ParameterHandles { param_t mnt_mode_in; param_t mnt_mode_out; + param_t mnt_mode_out2; param_t mnt_mav_sysid_v1; param_t mnt_mav_compid_v1; param_t mnt_man_pitch; diff --git a/src/modules/gimbal/gimbal_params.yaml b/src/modules/gimbal/gimbal_params.yaml index 7a3ed6937af..05ac9e94b61 100644 --- a/src/modules/gimbal/gimbal_params.yaml +++ b/src/modules/gimbal/gimbal_params.yaml @@ -44,6 +44,25 @@ parameters: min: 0 max: 2 reboot_required: true + MNT_MODE_OUT2: + description: + short: Second mount output mode + long: |- + Optional second output, driven in parallel to MNT_MODE_OUT with the + same setpoints. This allows the gimbal driver to talk to a second + gimbal in addition to the primary one. + + Set to Disabled if not used. The two outputs must use different modes. + type: enum + values: + -1: Disabled + 0: AUX + 1: MAVLink gimbal protocol v1 + 2: MAVLink gimbal protocol v2 + default: -1 + min: -1 + max: 2 + reboot_required: true MNT_MAV_SYSID: description: short: Mavlink System ID of the mount diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index 9d77fc1cb14..68db22574f0 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -61,6 +61,13 @@ public: virtual void print_status() const = 0; + /** + * Whether this output publishes the estimated mount orientation itself. + * MAVLink gimbals report their own orientation, so only non-MAVLink + * outputs (e.g. AUX) need the driver to publish it. + */ + virtual bool publishes_mount_orientation() const { return false; } + void publish(); void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize); diff --git a/src/modules/gimbal/output_rc.h b/src/modules/gimbal/output_rc.h index 750395f33a6..43b11ca750f 100644 --- a/src/modules/gimbal/output_rc.h +++ b/src/modules/gimbal/output_rc.h @@ -53,6 +53,9 @@ public: virtual void update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id); virtual void print_status() const; + // AUX gimbals cannot report their own orientation, so the driver publishes it. + bool publishes_mount_orientation() const override { return true; } + private: void _stream_device_attitude_status(); float anglesMappedToOutput(const uint8_t index);