Skip to content

Commit

Permalink
Merge branch 'master' into coverity_scan
Browse files Browse the repository at this point in the history 
* master:
  extras: mocap fix #352: use new helper for quaternion.
  plugin: setpoint_attitude #352: use new helper.
  plugin: sys: Fix cppcheck and YouCompleteMe warnings
  plugin: ftp: Fix cppcheck errors.
  • Loading branch information
@vooon
vooon committed Jul 29, 2015
2 parents 399d168 + 231d77e commit 924a8ce
Show file tree
Hide file tree
Showing 5 changed files with 26 additions and 35 deletions.
4 changes: 2 additions & 2 deletions mavros/src/plugins/ftp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -567,7 +567,7 @@ class FTPPlugin : public MavRosPlugin {

void send_reset() {
ROS_DEBUG_NAMED("ftp", "FTP:m: kCmdResetSessions");
if (session_file_map.size() > 0) {
if (!session_file_map.empty()) {
ROS_WARN_NAMED("ftp", "FTP: Reset closes %zu sessons",
session_file_map.size());
session_file_map.clear();
Expand All @@ -579,7 +579,7 @@ class FTPPlugin : public MavRosPlugin {
}

/// Send any command with string payload (usually file/dir path)
inline void send_any_path_command(FTPRequest::Opcode op, const std::string debug_msg, std::string &path, uint32_t offset) {
inline void send_any_path_command(FTPRequest::Opcode op, const std::string &debug_msg, std::string &path, uint32_t offset) {
ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: " << debug_msg << path << " off: " << offset);
FTPRequest req(op);
req.header()->offset = offset;
Expand Down
20 changes: 7 additions & 13 deletions mavros/src/plugins/setpoint_attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,9 @@ class SetpointAttitudePlugin : public MavRosPlugin,
sp_nh.param("tf/rate_limit", tf_rate, 10.0);

if (tf_listen) {
ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << tf_frame_id
<< " -> " << tf_child_frame_id);
ROS_INFO_STREAM_NAMED("attitude",
"Listen to desired attitude transform "
<< tf_frame_id << " -> " << tf_child_frame_id);
tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb);
}
else {
Expand Down Expand Up @@ -116,20 +117,13 @@ class SetpointAttitudePlugin : public MavRosPlugin,
const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
float q[4];

// Eigen use same convention as mavlink: w x y z
Eigen::Map<Eigen::Quaternionf> q_out(q);
q_out = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())).cast<float>();

float q_transformed[4];

q_transformed[0] = q_out.w();
q_transformed[1] = q_out.x();
q_transformed[2] = q_out.y();
q_transformed[3] = q_out.z();
UAS::quaternion_to_mavlink(
UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())),
q);

set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_q,
q_transformed,
q,
0.0, 0.0, 0.0,
0.0);
}
Expand Down
17 changes: 8 additions & 9 deletions mavros/src/plugins/sys_status.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,14 @@ namespace mavplugin {
class HeartbeatStatus : public diagnostic_updater::DiagnosticTask
{
public:
HeartbeatStatus(const std::string name, size_t win_size) :
HeartbeatStatus(const std::string &name, size_t win_size) :
diagnostic_updater::DiagnosticTask(name),
times_(win_size),
seq_nums_(win_size),
window_size_(win_size),
min_freq_(0.2),
max_freq_(100),
tolerance_(0.1),
times_(win_size),
seq_nums_(win_size),
autopilot(MAV_AUTOPILOT_GENERIC),
type(MAV_TYPE_GENERIC),
system_status(MAV_STATE_UNINIT)
Expand All @@ -52,8 +52,7 @@ class HeartbeatStatus : public diagnostic_updater::DiagnosticTask
ros::Time curtime = ros::Time::now();
count_ = 0;

for (int i = 0; i < window_size_; i++)
{
for (size_t i = 0; i < window_size_; i++) {
times_[i] = curtime;
seq_nums_[i] = count_;
}
Expand Down Expand Up @@ -128,7 +127,7 @@ class HeartbeatStatus : public diagnostic_updater::DiagnosticTask
class SystemStatusDiag : public diagnostic_updater::DiagnosticTask
{
public:
SystemStatusDiag(const std::string name) :
SystemStatusDiag(const std::string &name) :
diagnostic_updater::DiagnosticTask(name),
last_st {}
{ };
Expand Down Expand Up @@ -203,7 +202,7 @@ class SystemStatusDiag : public diagnostic_updater::DiagnosticTask
class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask
{
public:
BatteryStatusDiag(const std::string name) :
BatteryStatusDiag(const std::string &name) :
diagnostic_updater::DiagnosticTask(name),
voltage(-1.0),
current(0.0),
Expand Down Expand Up @@ -253,7 +252,7 @@ class BatteryStatusDiag : public diagnostic_updater::DiagnosticTask
class MemInfo : public diagnostic_updater::DiagnosticTask
{
public:
MemInfo(const std::string name) :
MemInfo(const std::string &name) :
diagnostic_updater::DiagnosticTask(name),
freemem(-1),
brkval(0)
Expand Down Expand Up @@ -291,7 +290,7 @@ class MemInfo : public diagnostic_updater::DiagnosticTask
class HwStatus : public diagnostic_updater::DiagnosticTask
{
public:
HwStatus(const std::string name) :
HwStatus(const std::string &name) :
diagnostic_updater::DiagnosticTask(name),
vcc(-1.0),
i2cerr(0),
Expand Down
2 changes: 1 addition & 1 deletion mavros/src/plugins/sys_time.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ namespace mavplugin {
class TimeSyncStatus : public diagnostic_updater::DiagnosticTask
{
public:
TimeSyncStatus(const std::string name, size_t win_size) :
TimeSyncStatus(const std::string &name, size_t win_size) :
diagnostic_updater::DiagnosticTask(name),
window_size_(win_size),
min_freq_(0.01),
Expand Down
18 changes: 8 additions & 10 deletions mavros_extras/src/plugins/mocap_pose_estimate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,14 +91,13 @@ class MocapPoseEstimatePlugin : public MavRosPlugin
/* -*- mid-level helpers -*- */
void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &pose)
{
float q[4];

// Eigen has same order as mavlink: w x y z
Eigen::Quaterniond q_enu;
Eigen::Map<Eigen::Quaternionf> q_out(q);
float q[4];

tf::quaternionMsgToEigen(pose->pose.orientation, q_enu);
q_out = UAS::transform_frame_enu_ned(q_enu).cast<float>();
UAS::quaternion_to_mavlink(
UAS::transform_frame_enu_ned(q_enu),
q);

auto position = UAS::transform_frame_enu_ned(
Eigen::Vector3d(
Expand All @@ -116,14 +115,13 @@ class MocapPoseEstimatePlugin : public MavRosPlugin
/* -*- callbacks -*- */
void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans)
{
float q[4];

// Eigen has same order as mavlink: w x y z
Eigen::Quaterniond q_enu;
Eigen::Map<Eigen::Quaternionf> q_out(q);
float q[4];

tf::quaternionMsgToEigen(trans->transform.rotation, q_enu);
q_out = UAS::transform_frame_enu_ned(q_enu).cast<float>();
UAS::quaternion_to_mavlink(
UAS::transform_frame_enu_ned(q_enu),
q);

auto position = UAS::transform_frame_enu_ned(
Eigen::Vector3d(
Expand Down

0 comments on commit 924a8ce

Please sign in to comment.