diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index fa3414c0d7..2dbce5672e 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -54,8 +54,6 @@ #include #include -using matrix::wrap_pi; - namespace vmount { @@ -121,6 +119,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ _control_data.type = ControlData::Type::Neutral; *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { _control_data.type = ControlData::Type::LonLat; @@ -132,17 +131,17 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { //TODO is this even suported? } - - _cur_roi_mode = vehicle_roi.mode; } // check whether the position setpoint got updated @@ -371,17 +370,14 @@ void InputMavlinkCmdMount::print_status() PX4_INFO("Input: Mavlink (CMD_MOUNT)"); } -InputMavlinkGimbalV2::InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, +InputMavlinkGimbalV2::InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw) : _mav_sys_id(mav_sys_id), _mav_comp_id(mav_comp_id), _mnt_rate_pitch(mnt_rate_pitch), _mnt_rate_yaw(mnt_rate_yaw) { - if (!has_v2_gimbal_device) { - /* dumb gimbal or MAVLink v1 protocol gimbal: fake GIMBAL_DEVICE_INFORMATION */ - _stream_gimbal_manager_information(); - } + _stream_gimbal_manager_information(); } InputMavlinkGimbalV2::~InputMavlinkGimbalV2() @@ -564,29 +560,24 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con _control_data.type = ControlData::Type::Neutral; *control_data = &_control_data; - _is_roi_set = false; _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { - double lat, lon, alt = 0.; - _read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt); + _control_data.type = ControlData::Type::LonLat; + _read_control_data_from_position_setpoint_sub(); _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; - _transform_lon_lat_to_angle(lon, lat, alt); - *control_data = &_control_data; - _is_roi_set = true; _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); - _transform_lon_lat_to_angle(vehicle_roi.lon, vehicle_roi.lat, (double)vehicle_roi.alt); *control_data = &_control_data; - _is_roi_set = true; _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { @@ -601,9 +592,7 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con // check whether the position setpoint got updated if (polls[2].revents & POLLIN) { if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { - double lat, lon, alt = 0.; - _read_lat_lon_alt_from_position_setpoint_sub(lon, lat, alt); - _transform_lon_lat_to_angle(lon, lat, alt); + _read_control_data_from_position_setpoint_sub(); *control_data = &_control_data; } else { // must do an orb_copy() in *every* case @@ -908,71 +897,6 @@ int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **con return 0; } -void InputMavlinkGimbalV2::_transform_lon_lat_to_angle(const double roi_lon, const double roi_lat, - const double roi_alt) -{ - vehicle_global_position_s vehicle_global_position; - _vehicle_global_position_sub.copy(&vehicle_global_position); - const double &vlat = vehicle_global_position.lat; - const double &vlon = vehicle_global_position.lon; - - _control_data.type = ControlData::Type::Angle; - _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - - const float roll = 0.0f; - float pitch = 0.0f; - float yaw = 0.0f; - - // interface: use fixed pitch value > -pi otherwise consider ROI altitude - if (_control_data.type_data.lonlat.pitch_fixed_angle >= -M_PI_F) { - pitch = _control_data.type_data.lonlat.pitch_fixed_angle; - - } else { - pitch = _calculate_pitch(roi_lon, roi_lat, roi_alt, vehicle_global_position); - } - - yaw = get_bearing_to_next_waypoint(vlat, vlon, roi_lat, roi_lon) - vehicle_local_position.yaw; - - // add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET - pitch += _control_data.type_data.lonlat.pitch_angle_offset; - yaw += _control_data.type_data.lonlat.yaw_angle_offset; - - // make sure yaw is wrapped correctly for the output - yaw = wrap_pi(yaw); - - matrix::Eulerf euler(roll, pitch, yaw); - matrix::Quatf q(euler); - q.copyTo(_control_data.type_data.angle.q); -} - -float InputMavlinkGimbalV2::_calculate_pitch(double lon, double lat, float altitude, - const vehicle_global_position_s &global_position) -{ - if (!map_projection_initialized(&_projection_reference)) { - map_projection_init(&_projection_reference, global_position.lat, global_position.lon); - } - - float x1, y1, x2, y2; - map_projection_project(&_projection_reference, lat, lon, &x1, &y1); - map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2); - float dx = x1 - x2, dy = y1 - y2; - float target_distance = sqrtf(dx * dx + dy * dy); - float z = altitude - global_position.alt; - - return atan2f(z, target_distance); -} - -void InputMavlinkGimbalV2::_read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp) -{ - position_setpoint_triplet_s position_setpoint_triplet; - orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); - lon_sp = position_setpoint_triplet.current.lon; - lat_sp = position_setpoint_triplet.current.lat; - alt_sp = (double)position_setpoint_triplet.current.alt; -} - void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, const matrix::Vector3f &angular_velocity) { @@ -1022,4 +946,13 @@ void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t cmd_ack_pub.publish(vehicle_command_ack); } +void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub() +{ + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + _control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + _control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + _control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; +} + } /* namespace vmount */ diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index a6367ba534..dfe491d792 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -109,8 +109,7 @@ private: class InputMavlinkGimbalV2 : public InputBase { public: - InputMavlinkGimbalV2(bool has_v2_gimbal_device, uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, - float &mnt_rate_yaw); + InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw); virtual ~InputMavlinkGimbalV2(); virtual void print_status(); @@ -120,16 +119,12 @@ protected: virtual int initialize(); private: - - void _transform_lon_lat_to_angle(const double roi_lon, const double roi_lat, const double roi_alt); - float _calculate_pitch(double lon, double lat, float altitude, - const vehicle_global_position_s &global_position); - void _read_lat_lon_alt_from_position_setpoint_sub(double &lon_sp, double &lat_sp, double &alt_sp); void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, const matrix::Vector3f &angular_velocity); void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result); void _stream_gimbal_manager_information(); void _stream_gimbal_manager_status(); + void _read_control_data_from_position_setpoint_sub(); int _vehicle_roi_sub = -1; int _gimbal_manager_set_attitude_sub = -1; @@ -146,13 +141,10 @@ private: float &_mnt_rate_pitch; float &_mnt_rate_yaw; - bool _is_roi_set{false}; - uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; - map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m] uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; }; diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index 9fd7fa4373..b2541d94d7 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -51,8 +51,6 @@ #include #include -using matrix::wrap_pi; - namespace vmount { @@ -102,17 +100,10 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) { _cur_control_data = control_data; - for (int i = 0; i < 3; ++i) { - _angle_speeds[i] = 0.f; - } - switch (control_data->type) { case ControlData::Type::Angle: { - matrix::Quatf q(control_data->type_data.angle.q); - matrix::Eulerf euler(q); - for (int i = 0; i < 3; ++i) { switch (control_data->type_data.angle.frames[i]) { case ControlData::TypeData::TypeAngle::Frame::AngularRate: @@ -127,8 +118,11 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) break; } - _angle_speeds[i] = control_data->type_data.angle.angular_velocity[i]; - _angle_setpoints[i] = euler(i); + _angle_velocity[i] = control_data->type_data.angle.angular_velocity[i]; + } + + for (int i = 0; i < 4; ++i) { + _q_setpoint[i] = control_data->type_data.angle.q[i]; } } @@ -139,9 +133,13 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) break; case ControlData::Type::Neutral: - _angle_setpoints[0] = 0.f; - _angle_setpoints[1] = 0.f; - _angle_setpoints[2] = 0.f; + _q_setpoint[0] = 1.f; + _q_setpoint[1] = 0.f; + _q_setpoint[2] = 0.f; + _q_setpoint[3] = 0.f; + _angle_velocity[0] = NAN; + _angle_velocity[1] = NAN; + _angle_velocity[2] = NAN; break; } } @@ -176,31 +174,31 @@ void OutputBase::_handle_position_update(bool force_update) const double &lon = _cur_control_data->type_data.lonlat.lon; const float &alt = _cur_control_data->type_data.lonlat.altitude; - _angle_setpoints[0] = _cur_control_data->type_data.lonlat.roll_angle; + float roll = _cur_control_data->type_data.lonlat.roll_angle; // interface: use fixed pitch value > -pi otherwise consider ROI altitude - if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) { - _angle_setpoints[1] = _cur_control_data->type_data.lonlat.pitch_fixed_angle; + float pitch = (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ? + _cur_control_data->type_data.lonlat.pitch_fixed_angle : + _calculate_pitch(lon, lat, alt, vehicle_global_position); - } else { - _angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position); - } - - _angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading; + float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading; // add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET - _angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset; - _angle_setpoints[2] += _cur_control_data->type_data.lonlat.yaw_angle_offset; + pitch += _cur_control_data->type_data.lonlat.pitch_angle_offset; + yaw += _cur_control_data->type_data.lonlat.yaw_angle_offset; - // make sure yaw is wrapped correctly for the output - _angle_setpoints[2] = wrap_pi(_angle_setpoints[2]); + matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint); + + _angle_velocity[0] = NAN; + _angle_velocity[1] = NAN; + _angle_velocity[2] = NAN; } -void OutputBase::_calculate_output_angles(const hrt_abstime &t) +void OutputBase::_calculate_angle_output(const hrt_abstime &t) { //get the output angles and stabilize if necessary vehicle_attitude_s vehicle_attitude{}; - matrix::Eulerf euler; + matrix::Eulerf euler_vehicle; // We only need to apply additional compensation if the required angle is // absolute (world frame) as well as the gimbal is not capable of doing that @@ -214,29 +212,31 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t) if (compensate[0] || compensate[1] || compensate[2]) { _vehicle_attitude_sub.copy(&vehicle_attitude); - euler = matrix::Quatf(vehicle_attitude.q); + euler_vehicle = matrix::Quatf(vehicle_attitude.q); } float dt = (t - _last_update) / 1.e6f; + matrix::Eulerf euler_gimbal = matrix::Quatf(_q_setpoint); + for (int i = 0; i < 3; ++i) { - if (PX4_ISFINITE(_angle_setpoints[i])) { - _angle_outputs[i] = _angle_setpoints[i]; + if (PX4_ISFINITE(euler_gimbal(i))) { + _angle_outputs[i] = euler_gimbal(i); } - if (PX4_ISFINITE(_angle_speeds[i])) { - _angle_outputs[i] += dt * _angle_speeds[i]; + if (PX4_ISFINITE(_angle_velocity[i])) { + _angle_outputs[i] += dt * _angle_velocity[i]; } if (compensate[i]) { - _angle_outputs[i] -= euler(i); + _angle_outputs[i] -= euler_vehicle(i); } - //bring angles into proper range [-pi, pi] - while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; } - - while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; } + if (PX4_ISFINITE(_angle_outputs[i])) { + //bring angles into proper range [-pi, pi] + _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); + } } } diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h index e62e427095..1c13edd16e 100644 --- a/src/modules/vmount/output.h +++ b/src/modules/vmount/output.h @@ -114,8 +114,10 @@ protected: void _handle_position_update(bool force_update = false); const ControlData *_cur_control_data = nullptr; - float _angle_setpoints[3] = { 0.f, 0.f, 0.f }; ///< [rad], can be NAN if not specifically set - float _angle_speeds[3] = { 0.f, 0.f, 0.f }; ///< [rad/s], can be NAN if not specifically set + + float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set + float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set + bool _stabilize[3] = { false, false, false }; // Pitch and role are by default aligned with the horizon. @@ -123,7 +125,7 @@ protected: bool _absolute_angle[3] = {true, true, false }; /** calculate the _angle_outputs (with speed) and stabilize if needed */ - void _calculate_output_angles(const hrt_abstime &t); + void _calculate_angle_output(const hrt_abstime &t); float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad] hrt_abstime _last_update; diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 9cda74650f..8a82b378f3 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -94,7 +94,7 @@ int OutputMavlinkV1::update(const ControlData *control_data) _handle_position_update(); hrt_abstime t = hrt_absolute_time(); - _calculate_output_angles(t); + _calculate_angle_output(t); vehicle_command.timestamp = t; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; @@ -136,17 +136,13 @@ int OutputMavlinkV2::update(const ControlData *control_data) _last_gimbal_device_checked = t; } else { - // Only start sending attitude setpoints once a device has been discovered. - if (control_data) { //got new command _set_angle_setpoints(control_data); - _publish_gimbal_device_set_attitude(control_data); } _handle_position_update(); - - _calculate_output_angles(t); + _publish_gimbal_device_set_attitude(); _last_update = t; } @@ -185,51 +181,31 @@ void OutputMavlinkV2::print_status() PX4_INFO("Output: MAVLink gimbal protocol v2"); } -void OutputMavlinkV2::_publish_gimbal_device_set_attitude(const ControlData *control_data) +void OutputMavlinkV2::_publish_gimbal_device_set_attitude() { gimbal_device_set_attitude_s set_attitude{}; set_attitude.timestamp = hrt_absolute_time(); set_attitude.target_system = (uint8_t)_mav_sys_id; set_attitude.target_component = _gimbal_device_compid; - switch (control_data->type) { - case ControlData::Type::Neutral: - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_NEUTRAL; - set_attitude.angular_velocity_x = NAN; - set_attitude.angular_velocity_y = NAN; - set_attitude.angular_velocity_z = NAN; - set_attitude.q[0] = NAN; - set_attitude.q[1] = NAN; - set_attitude.q[2] = NAN; - set_attitude.q[3] = NAN; - break; + set_attitude.angular_velocity_x = _angle_velocity[0]; + set_attitude.angular_velocity_y = _angle_velocity[1]; + set_attitude.angular_velocity_z = _angle_velocity[2]; + set_attitude.q[0] = _q_setpoint[0]; + set_attitude.q[1] = _q_setpoint[1]; + set_attitude.q[2] = _q_setpoint[2]; + set_attitude.q[3] = _q_setpoint[3]; - case ControlData::Type::Angle: - set_attitude.angular_velocity_x = control_data->type_data.angle.angular_velocity[0]; - set_attitude.angular_velocity_y = control_data->type_data.angle.angular_velocity[1]; - set_attitude.angular_velocity_z = control_data->type_data.angle.angular_velocity[2]; - set_attitude.q[0] = control_data->type_data.angle.q[0]; - set_attitude.q[1] = control_data->type_data.angle.q[1]; - set_attitude.q[2] = control_data->type_data.angle.q[2]; - set_attitude.q[3] = control_data->type_data.angle.q[3]; + if (_absolute_angle[0]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK; + } - if (control_data->type_data.angle.frames[0] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK; - } + if (_absolute_angle[1]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + } - if (control_data->type_data.angle.frames[1] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK; - } - - if (control_data->type_data.angle.frames[2] == ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame) { - set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; - } - - break; - - case ControlData::Type::LonLat: - // FIXME: needs conversion to angle. - break; + if (_absolute_angle[2]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; } _gimbal_device_set_attitude_pub.publish(set_attitude); diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 934be1a19a..5cea1c953d 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -78,7 +78,7 @@ public: virtual void print_status(); private: - void _publish_gimbal_device_set_attitude(const ControlData *control_data); + void _publish_gimbal_device_set_attitude(); void _request_gimbal_device_information(); void _check_for_gimbal_device_information(); diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp index 6aaa5b76f8..04d2df39f1 100644 --- a/src/modules/vmount/output_rc.cpp +++ b/src/modules/vmount/output_rc.cpp @@ -63,7 +63,7 @@ int OutputRC::update(const ControlData *control_data) _handle_position_update(); hrt_abstime t = hrt_absolute_time(); - _calculate_output_angles(t); + _calculate_angle_output(t); actuator_controls_s actuator_controls{}; actuator_controls.timestamp = hrt_absolute_time(); diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index 28b0889dbf..2fe8956091 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -281,11 +281,11 @@ static int vmount_thread_main(int argc, char *argv[]) break; case 4: //MAVLINK_V2 - thread_data.input_objs[0] = new InputMavlinkGimbalV2(params.mnt_mode_out == 2, - params.mav_sys_id, - params.mav_comp_id, - params.mnt_rate_pitch, - params.mnt_rate_yaw); + thread_data.input_objs[0] = new InputMavlinkGimbalV2( + params.mav_sys_id, + params.mav_comp_id, + params.mnt_rate_pitch, + params.mnt_rate_yaw); break; default: