Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

mount_control.cpp: detect MOUNT_ORIENTATION stale messages #1744

Merged
merged 1 commit into from
May 27, 2022
Merged

mount_control.cpp: detect MOUNT_ORIENTATION stale messages #1744

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions mavros/launch/apm_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,6 +213,9 @@ mocap:
mount:
debounce_s: 4.0
err_threshold_deg: 10.0
negate_measured_roll: false
negate_measured_pitch: false
negate_measured_yaw: false

# odom
odometry:
Expand Down
8 changes: 8 additions & 0 deletions mavros/launch/px4_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,6 +220,14 @@ mocap:
use_tf: false # ~mocap/tf
use_pose: true # ~mocap/pose

# mount_control
mount:
debounce_s: 4.0
err_threshold_deg: 10.0
negate_measured_roll: false
negate_measured_pitch: false
negate_measured_yaw: false

# odom
odometry:
fcu:
Expand Down
67 changes: 56 additions & 11 deletions mavros_extras/src/plugins/mount_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,16 +39,17 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
public:
MountStatusDiag(const std::string &name) :
diagnostic_updater::DiagnosticTask(name),
_last_orientation_update(0, 0),
_debounce_s(NAN),
_roll_deg(NAN),
_pitch_deg(NAN),
_yaw_deg(NAN),
_setpoint_roll_deg(NAN),
_setpoint_pitch_deg(NAN),
_setpoint_yaw_deg(NAN),
_error_detected(false),
_err_threshold_deg(NAN),
_mode(255),
_debounce_s(NAN)
_error_detected(false),
_mode(255)
{ }

void set_err_threshold_deg(float threshold_deg) {
Expand All @@ -61,11 +62,12 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
_debounce_s = debounce_s;
}

void set_status(float roll_deg, float pitch_deg, float yaw_deg) {
void set_status(float roll_deg, float pitch_deg, float yaw_deg, ros::Time timestamp) {
std::lock_guard<std::mutex> lock(mutex);
_roll_deg = roll_deg;
_pitch_deg = pitch_deg;
_yaw_deg = yaw_deg;
_last_orientation_update = timestamp;
}

void set_setpoint(float roll_deg, float pitch_deg, float yaw_deg, uint8_t mode) {
Expand All @@ -82,6 +84,7 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
float pitch_err_deg;
float yaw_err_deg;
bool error_detected = false;
bool stale = false;

if (_mode != mavros_msgs::MountControl::MAV_MOUNT_MODE_MAVLINK_TARGETING) {
// Can only directly compare the MAV_CMD_DO_MOUNT_CONTROL angles with the MOUNT_ORIENTATION angles when in MAVLINK_TARGETING mode
Expand All @@ -90,6 +93,7 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
return;
}

const ros::Time now = ros::Time::now();
{
std::lock_guard<std::mutex> lock(mutex);
roll_err_deg = _setpoint_roll_deg - _roll_deg;
Expand All @@ -106,12 +110,14 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
if (fabs(yaw_err_deg) > _err_threshold_deg) {
error_detected = true;
}
if (now - _last_orientation_update > ros::Duration(5, 0)) {
stale = true;
}
// accessing the _debounce_s variable should be done inside this mutex,
// but we can treat it as an atomic variable, and save the trouble
}

// detect error state changes
const ros::Time now = ros::Time::now();
if (!_error_detected && error_detected) {
_error_started = now;
_error_detected = true;
Expand All @@ -121,7 +127,9 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
}

// debounce errors
if (_error_detected && (now - _error_started > ros::Duration(_debounce_s))) {
if (stale) {
stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "No MOUNT_ORIENTATION received in the last 5 s");
} else if (_error_detected && (now - _error_started > ros::Duration(_debounce_s))) {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "angle error too high");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal");
Expand All @@ -135,6 +143,7 @@ class MountStatusDiag : public diagnostic_updater::DiagnosticTask
private:
std::mutex mutex;
ros::Time _error_started;
ros::Time _last_orientation_update;
double _debounce_s;
float _roll_deg;
float _pitch_deg;
Expand Down Expand Up @@ -170,12 +179,33 @@ class MountControlPlugin : public plugin::PluginBase {
mount_status_pub = mount_nh.advertise<geometry_msgs::Vector3Stamped>("status", 10);
configure_srv = mount_nh.advertiseService("configure", &MountControlPlugin::mount_configure_cb, this);

// some gimbals send negated/inverted angle measurements
// these parameters correct that to obey the MAVLink frame convention
mount_nh.param<bool>("negate_measured_roll", negate_measured_roll, false);
mount_nh.param<bool>("negate_measured_pitch", negate_measured_pitch, false);
mount_nh.param<bool>("negate_measured_yaw", negate_measured_yaw, false);
if (!mount_nh.getParam("negate_measured_roll", negate_measured_roll)) {
ROS_WARN("Could not retrive negate_measured_roll parameter value, using default (%d)", negate_measured_roll);
}
if (!mount_nh.getParam("negate_measured_pitch", negate_measured_pitch)) {
ROS_WARN("Could not retrive negate_measured_pitch parameter value, using default (%d)", negate_measured_pitch);
}
if (!mount_nh.getParam("negate_measured_yaw", negate_measured_yaw)) {
ROS_WARN("Could not retrive negate_measured_yaw parameter value, using default (%d)", negate_measured_yaw);
}
vooon marked this conversation as resolved.
Show resolved Hide resolved

bool disable_diag;
if (nh.getParam("sys/disable_diag", disable_diag) && !disable_diag) {
double debounce_s;
double err_threshold_deg;
mount_nh.param("debounce_s", debounce_s, 4.0);
mount_nh.param("err_threshold_deg", err_threshold_deg, 10.0);
if (!mount_nh.getParam("debounce_s", debounce_s)) {
ROS_WARN("Could not retrive debounce_s parameter value, using default (%f)", debounce_s);
}
if (!mount_nh.getParam("err_threshold_deg", err_threshold_deg)) {
ROS_WARN("Could not retrive err_threshold_deg parameter value, using default (%f)", err_threshold_deg);
}
mount_diag.set_debounce_s(debounce_s);
mount_diag.set_err_threshold_deg(err_threshold_deg);
UAS_DIAG(m_uas).add(mount_diag);
Expand All @@ -199,6 +229,9 @@ class MountControlPlugin : public plugin::PluginBase {
ros::ServiceServer configure_srv;

MountStatusDiag mount_diag;
bool negate_measured_roll;
bool negate_measured_pitch;
bool negate_measured_yaw;

/**
* @brief Publish the mount orientation
Expand All @@ -209,13 +242,24 @@ class MountControlPlugin : public plugin::PluginBase {
*/
void handle_mount_orientation(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MOUNT_ORIENTATION &mo)
{
auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0);
const auto timestamp = ros::Time::now();
// some gimbals send negated/inverted angle measurements, correct that to obey the MAVLink frame convention
if (negate_measured_roll) {
mo.roll = -mo.roll;
}
if (negate_measured_pitch) {
mo.pitch = -mo.pitch;
}
if (negate_measured_yaw) {
mo.yaw = -mo.yaw;
mo.yaw_absolute = -mo.yaw_absolute;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This will make the frames inconsistent (oscillating between left hand coordinate or right hand coordinate systems). Shouldn't this be something that is fixed on the driver level?

Copy link
Contributor Author

@amilcarlucas amilcarlucas May 20, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Gremsy probably behaves differently from storm32. I can not test, I only have gremsy. But what I can say ist that this let's the users fix issues on their Gimbals without changing the code.

And for gremsy the code is definitely broken right now.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this is a device specific fix that needs to happen, why not handle it on the user app level? I still think mavros is the wrong place to make a workaround for a device specific problem.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Because the diagnostics code needs to handle it at this level, take a look at the diagnostics class in the mount_control.cpp file

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not sure if I understand what you mean... why does the diagnostic status influenced by the negation in the mount control handling?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The diagnostics compare the measured angle with the commanded (set-point) angle if both match the gimbal is working. If they do not, there is an issue with the gimbal.

Now due to the way some gimbals work, a set-point of 70° causes a measurement of -70°. and that triggers an error.
By configuring negate_measurement_xxx it will transform the -70° into 70° and then the diagnostic will stop failing.

Copy link
Contributor

@Jaeyoung-Lim Jaeyoung-Lim May 23, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@amilcarlucas But if the gremsy gimbal driver is implemented the opposite sign, shouldn't the user app command -70° in the first place? I am not comfortable for a device specific bug propagating into a common software.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That is the gremsy bug, you command 70 and it measures -70 !

const auto q = ftf::quaternion_from_rpy(Eigen::Vector3d(mo.roll, mo.pitch, mo.yaw) * M_PI / 180.0);
geometry_msgs::Quaternion quaternion_msg;
tf::quaternionEigenToMsg(q, quaternion_msg);
mount_orientation_pub.publish(quaternion_msg);

// pitch angle is inverted to fix a gremsy implementation bug
mount_diag.set_status(mo.roll, -mo.pitch, mo.yaw_absolute);
mount_diag.set_status(mo.roll, mo.pitch, mo.yaw_absolute, timestamp);
}

/**
Expand Down Expand Up @@ -267,7 +311,7 @@ class MountControlPlugin : public plugin::PluginBase {

UAS_FCU(m_uas)->send_message_ignore_drop(cmd);

mount_diag.set_setpoint(req->roll/100.0f, req->pitch/100.0f, req->yaw/100.0f, req->mode);
mount_diag.set_setpoint(req->roll*0.01f, req->pitch*0.01f, req->yaw*0.01f, req->mode);
}

bool mount_configure_cb(mavros_msgs::MountConfigure::Request &req,
Expand All @@ -292,7 +336,8 @@ class MountControlPlugin : public plugin::PluginBase {
cmd.request.param7 = req.yaw_input;

ROS_DEBUG_NAMED("mount", "MountConfigure: Request mode %u ", req.mode);
res.success = client.call(cmd);
client.call(cmd);
res.success = cmd.response.success;
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("mount", "MountConfigure: %s", ex.what());
Expand Down