Skip to content

Commit

Permalink
formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@PernillaWikstrom
PernillaWikstrom committed Nov 7, 2024
1 parent 916ebe0 commit de61135
Show file tree
Hide file tree
Showing 6 changed files with 90 additions and 86 deletions.
9 changes: 4 additions & 5 deletions src/modules/gimbal/input_mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand All @@ -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();
Expand Down
9 changes: 6 additions & 3 deletions src/modules/gimbal/output.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
8 changes: 3 additions & 5 deletions src/modules/mavlink/mavlink_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3087,12 +3087,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;
}

Expand Down
20 changes: 11 additions & 9 deletions src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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{};

Expand All @@ -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);
}
Expand Down
100 changes: 49 additions & 51 deletions src/modules/simulation/gz_bridge/GZGimbal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,28 +6,32 @@ 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<gz::msgs::Double>(gimbal_roll_topic);

if (!_gimbal_roll_cmd_publisher.Valid()) {
PX4_ERR("failed to advertise %s", gimbal_roll_topic.c_str());
return false;
}

const std::string gimbal_pitch_topic = "/model/" + model_name + "/command/gimbal_pitch";
_gimbal_pitch_cmd_publisher = _node.Advertise<gz::msgs::Double>(gimbal_pitch_topic);

if (!_gimbal_pitch_cmd_publisher.Valid()) {
PX4_ERR("failed to advertise %s", gimbal_pitch_topic.c_str());
return false;
}

const std::string gimbal_yaw_topic = "/model/" + model_name + "/command/gimbal_yaw";
_gimbal_yaw_cmd_publisher = _node.Advertise<gz::msgs::Double>(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;
}
Expand All @@ -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;
}

Expand All @@ -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();
Expand All @@ -95,14 +99,14 @@ 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();

matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(),
IMU_data.angular_velocity().y(),
IMU_data.angular_velocity().z()));
IMU_data.angular_velocity().y(),
IMU_data.angular_velocity().z()));

_gimbal_rate[0] = rate(0);
_gimbal_rate[1] = rate(1);
Expand All @@ -121,11 +125,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();
Expand All @@ -137,32 +140,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{};

Expand All @@ -175,7 +178,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));
Expand Down Expand Up @@ -218,8 +221,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;

Expand All @@ -231,32 +234,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;
}
Expand Down
30 changes: 17 additions & 13 deletions src/modules/simulation/gz_bridge/GZGimbal.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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]
Expand Down

0 comments on commit de61135

Please sign in to comment.