diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index 1d6ba9c6dca7..05dec9bbd337 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -474,8 +474,8 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data) { gimbal_device_information_s gimbal_device_info; - if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == 2) - { + + if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == 2) { gimbal_manager_information_s gimbal_manager_info; gimbal_manager_info.timestamp = hrt_absolute_time(); @@ -491,9 +491,8 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData gimbal_manager_info.gimbal_device_id = control_data.device_compid; _gimbal_manager_info_pub.publish(gimbal_manager_info); - } - else if (_parameters.mnt_mode_out == 0) - { + + } else if (_parameters.mnt_mode_out == 0) { // since we have a non-Mavlink gimbal device, the gimbal manager itself has to act as the gimbal device gimbal_manager_information_s gimbal_manager_info; gimbal_manager_info.timestamp = hrt_absolute_time(); diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index 5f4d02e626b3..ea474bf33ec5 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -265,9 +265,12 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) } // constrain angle outputs to [-range/2, range/2] - _angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll/2), math::radians(_parameters.mnt_range_roll/2)); - _angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch/2), math::radians(_parameters.mnt_range_pitch/2)); - _angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw/2), math::radians(_parameters.mnt_range_yaw/2)); + _angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll / 2), + math::radians(_parameters.mnt_range_roll / 2)); + _angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch / 2), + math::radians(_parameters.mnt_range_pitch / 2)); + _angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw / 2), + math::radians(_parameters.mnt_range_yaw / 2)); // constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed if (_landed) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d594bb6d3df6..6c87142c30ed 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3063,12 +3063,10 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - if (gimbal_device_info_msg.gimbal_device_id == 0) - { + if (gimbal_device_info_msg.gimbal_device_id == 0) { gimbal_information.gimbal_device_compid = msg->compid; - } - else - { + + } else { gimbal_information.gimbal_device_compid = gimbal_device_info_msg.gimbal_device_id; } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 46c6e41f4978..2708e97bc6d1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -74,8 +74,7 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream return false; } - if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) - { + if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) { // A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id mavlink_gimbal_device_attitude_status_t msg{}; @@ -99,15 +98,18 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); - } - else - { + + } else { // We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID mavlink_message_t message; - mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, _mavlink->get_channel(), &message, - gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, gimbal_device_attitude_status.timestamp / 1000, - gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, gimbal_device_attitude_status.angular_velocity_x, - gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, gimbal_device_attitude_status.failure_flags, + mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, + _mavlink->get_channel(), &message, + gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, + gimbal_device_attitude_status.timestamp / 1000, + gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, + gimbal_device_attitude_status.angular_velocity_x, + gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, + gimbal_device_attitude_status.failure_flags, 0, 0, 0); _mavlink->forward_message(&message, _mavlink); } diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 023826468d80..a9e35f51b8a3 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -6,6 +6,7 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name // Gazebo communication const std::string gimbal_roll_topic = "/model/" + model_name + "/command/gimbal_roll"; _gimbal_roll_cmd_publisher = _node.Advertise(gimbal_roll_topic); + if (!_gimbal_roll_cmd_publisher.Valid()) { PX4_ERR("failed to advertise %s", gimbal_roll_topic.c_str()); return false; @@ -13,6 +14,7 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name const std::string gimbal_pitch_topic = "/model/" + model_name + "/command/gimbal_pitch"; _gimbal_pitch_cmd_publisher = _node.Advertise(gimbal_pitch_topic); + if (!_gimbal_pitch_cmd_publisher.Valid()) { PX4_ERR("failed to advertise %s", gimbal_pitch_topic.c_str()); return false; @@ -20,14 +22,16 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name const std::string gimbal_yaw_topic = "/model/" + model_name + "/command/gimbal_yaw"; _gimbal_yaw_cmd_publisher = _node.Advertise(gimbal_yaw_topic); + if (!_gimbal_yaw_cmd_publisher.Valid()) { PX4_ERR("failed to advertise %s", gimbal_yaw_topic.c_str()); return false; } - const std::string gimbal_imu_topic = "/world/" + world_name + "/model/" + model_name + "/link/camera_link/sensor/camera_imu/imu"; - if (!_node.Subscribe(gimbal_imu_topic, &GZGimbal::gimbalIMUCallback, this)) - { + const std::string gimbal_imu_topic = "/world/" + world_name + "/model/" + model_name + + "/link/camera_link/sensor/camera_imu/imu"; + + if (!_node.Subscribe(gimbal_imu_topic, &GZGimbal::gimbalIMUCallback, this)) { PX4_ERR("failed to subscribe to %s", gimbal_imu_topic.c_str()); return false; } @@ -37,20 +41,20 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name _mnt_range_pitch_handle = param_find("MNT_RANGE_PITCH"); _mnt_range_yaw_handle = param_find("MNT_RANGE_YAW"); _mnt_mode_out_handle = param_find("MNT_MODE_OUT"); + if (_mnt_range_roll_handle == PARAM_INVALID || _mnt_range_pitch_handle == PARAM_INVALID || _mnt_range_yaw_handle == PARAM_INVALID || - _mnt_mode_out_handle == PARAM_INVALID) - { + _mnt_mode_out_handle == PARAM_INVALID) { return false; } + updateParameters(); ScheduleOnInterval(200_ms); // @5Hz // Schedule on vehicle command messages - if (!_vehicle_command_sub.registerCallback()) - { + if (!_vehicle_command_sub.registerCallback()) { return false; } @@ -66,16 +70,16 @@ void GZGimbal::Run() _last_time_update = now; updateParameters(); - if (pollSetpoint()) - { + + if (pollSetpoint()) { //TODO handle device flags publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _roll_min, _roll_max, dt); - publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max, dt); + publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max, + dt); publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt); } - if (_mnt_mode_out == 2) - { + if (_mnt_mode_out == 2) { // We have a Mavlink gimbal capable of sending messages publishDeviceInfo(); publishDeviceAttitude(); @@ -95,9 +99,9 @@ void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f); const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(), - IMU_data.orientation().x(), - IMU_data.orientation().y(), - IMU_data.orientation().z()); + IMU_data.orientation().x(), + IMU_data.orientation().y(), + IMU_data.orientation().z()); _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); pthread_mutex_unlock(&_node_mutex); @@ -113,11 +117,10 @@ void GZGimbal::updateParameters() bool GZGimbal::pollSetpoint() { - if(_gimbal_device_set_attitude_sub.updated()) - { + if (_gimbal_device_set_attitude_sub.updated()) { gimbal_device_set_attitude_s msg; - if (_gimbal_device_set_attitude_sub.copy(&msg)) - { + + if (_gimbal_device_set_attitude_sub.copy(&msg)) { const matrix::Eulerf gimbal_att_stp(matrix::Quatf(msg.q)); _roll_stp = gimbal_att_stp.phi(); _pitch_stp = gimbal_att_stp.theta(); @@ -129,32 +132,32 @@ bool GZGimbal::pollSetpoint() return true; } - } - else if (_gimbal_controls_sub.updated()) - { + + } else if (_gimbal_controls_sub.updated()) { gimbal_controls_s msg; - if (_gimbal_controls_sub.copy(&msg)) - { + + if (_gimbal_controls_sub.copy(&msg)) { // map control inputs from [-1;1] to [min_angle; max_angle] using the range parameters - _roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll/2), _roll_min, _roll_max); - _pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch/2), _pitch_min, _pitch_max); - _yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw/2), _yaw_min, _yaw_max); + _roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll / 2), _roll_min, _roll_max); + _pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch / 2), _pitch_min, + _pitch_max); + _yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max); return true; } } + return false; } void GZGimbal::publishDeviceInfo() { - if (_vehicle_command_sub.updated()) - { + if (_vehicle_command_sub.updated()) { vehicle_command_s cmd; _vehicle_command_sub.copy(&cmd); + if (cmd.command == vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE && - (uint16_t)cmd.param1 == vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION) - { + (uint16_t)cmd.param1 == vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION) { // Acknowledge the command vehicle_command_ack_s command_ack{}; @@ -167,7 +170,7 @@ void GZGimbal::publishDeviceInfo() _vehicle_command_ack_pub.publish(command_ack); // Send the requested message - gimbal_device_information_s device_info{}; + gimbal_device_information_s device_info{}; memcpy(device_info.vendor_name, _vendor_name, sizeof(_vendor_name)); memcpy(device_info.model_name, _model_name, sizeof(_model_name)); @@ -210,8 +213,8 @@ void GZGimbal::publishDeviceAttitude() _gimbal_device_attitude_status_pub.publish(gimbal_att); } -void GZGimbal::publishJointCommand(gz::transport::Node::Publisher& publisher, const float att_stp, const float rate_stp, - float &last_stp, const float min_stp, const float max_stp, const float dt) +void GZGimbal::publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp, + float &last_stp, const float min_stp, const float max_stp, const float dt) { gz::msgs::Double msg; @@ -223,32 +226,27 @@ void GZGimbal::publishJointCommand(gz::transport::Node::Publisher& publisher, co publisher.Publish(msg); } -float GZGimbal::computeJointSetpoint(const float att_stp, const float rate_stp,const float last_stp, const float dt) +float GZGimbal::computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt) { - if (PX4_ISFINITE(rate_stp)) - { + if (PX4_ISFINITE(rate_stp)) { const float rate_diff = dt * rate_stp; - const float stp_from_rate = last_stp + rate_diff; + const float stp_from_rate = last_stp + rate_diff; - if (PX4_ISFINITE(att_stp)) - { + if (PX4_ISFINITE(att_stp)) { // We use the attitude rate setpoint but we constrain it by the desired angle return rate_diff > 0 ? math::min(att_stp, stp_from_rate) : math::max(att_stp, stp_from_rate); - } - else - { + + } else { // The rate setpoint is valid while the angle one is not return stp_from_rate; } - } - else if (PX4_ISFINITE(att_stp)) - { + + } else if (PX4_ISFINITE(att_stp)) { // Only the angle setpoint is valid return att_stp; - } - else - { + + } else { // Neither setpoint is valid so we steer the gimbal to the default position (roll = pitch = yaw = 0) return 0.0f; } diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index b001a76904a8..1396b4354d1e 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -85,21 +85,24 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams const char _custom_name[32] = ""; const uint8_t _firmware_dev_version = 0; const uint8_t _firmware_patch_version = 0; - const uint8_t _firmware_minor_version = 0; - const uint8_t _firmware_major_version = 1; - const uint32_t _firmware_version = (_firmware_dev_version & 0xff) << 24 | (_firmware_patch_version & 0xff) << 16 | - (_firmware_minor_version & 0xff) << 8 | (_firmware_major_version & 0xff); + const uint8_t _firmware_minor_version = 0; + const uint8_t _firmware_major_version = 1; + const uint32_t _firmware_version = (_firmware_dev_version & 0xff) << 24 | (_firmware_patch_version & 0xff) << 16 | + (_firmware_minor_version & 0xff) << 8 | (_firmware_major_version & 0xff); const uint8_t _hardware_dev_version = 0; - const uint8_t _hardware_patch_version = 0; - const uint8_t _hardware_minor_version = 0; - const uint8_t _hardware_major_version = 1; + const uint8_t _hardware_patch_version = 0; + const uint8_t _hardware_minor_version = 0; + const uint8_t _hardware_major_version = 1; const uint32_t _hardware_version = (_hardware_dev_version & 0xff) << 24 | (_hardware_patch_version & 0xff) << 16 | - (_hardware_minor_version & 0xff) << 8 | (_hardware_major_version & 0xff); + (_hardware_minor_version & 0xff) << 8 | (_hardware_major_version & 0xff); const uint64_t _uid = 0x9a77a55b3c10971f ; const uint16_t _cap_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL | - gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS | gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW | - gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW | - gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS | gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW | gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW; const uint16_t _custom_cap_flags = 0; const float _roll_min = -0.785398f; @@ -131,8 +134,9 @@ class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams /// @param min_stp minimum joint attitude [rad] /// @param max_stp maximum joint attitude [rad] /// @param dt time interval since the last computation [s] - static void publishJointCommand(gz::transport::Node::Publisher& publisher, const float att_stp, const float rate_stp, float &last_stp, - const float min_stp, const float max_stp, const float dt); + static void publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp, + float &last_stp, + const float min_stp, const float max_stp, const float dt); /// @brief Compute joint position setpoint taking into account both desired position and velocity. /// @param att_stp desired joint attitude [rad] /// @param rate_stp desired joint attitude rate [rad/s]