diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 28eef7e187..6b6a03893b 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -84,6 +84,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) if (!ros::param::has(name)) { ros::param::set(name, value); } + return (px4_param_t)name; }; static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) @@ -91,6 +92,7 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) if (!ros::param::has(name)) { ros::param::set(name, value); } + return (px4_param_t)name; }; @@ -179,8 +181,8 @@ typedef param_t px4_param_t; /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) #define isspace(c) \ - ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ - (c) == '\r' || (c) == '\f' || c== '\v') + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') #endif #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 624a466fdf..7b14caed97 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -78,11 +78,12 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(const M&)) + Subscriber *subscribe(const char *topic, void(*fp)(const M &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); - ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; } @@ -93,11 +94,12 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); - ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; } @@ -110,8 +112,9 @@ public: Subscriber *subscribe(const char *topic) { SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); - ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; } @@ -160,6 +163,7 @@ public: /* Empty subscriptions list */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; + while (sub != nullptr) { if (count++ > kMaxSubscriptions) { PX4_WARN("exceeded max subscriptions"); @@ -174,6 +178,7 @@ public: /* Empty publications list */ uORB::PublicationNode *pub = _pubs.getHead(); count = 0; + while (pub != nullptr) { if (count++ > kMaxPublications) { PX4_WARN("exceeded max publications"); @@ -195,8 +200,8 @@ public: template Subscriber *subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) + std::function callback, + unsigned interval) { SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); @@ -216,7 +221,7 @@ public: template Subscriber *subscribe(const struct orb_metadata *meta, - unsigned interval) + unsigned interval) { SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 107c60189e..aaacf9e484 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -86,7 +86,7 @@ public: /** * Get void pointer to last message value */ - virtual void * get_void_ptr() = 0; + virtual void *get_void_ptr() = 0; }; #if defined(__PX4_ROS) @@ -97,7 +97,7 @@ template class SubscriberROS : public Subscriber { -friend class NodeHandle; + friend class NodeHandle; public: /** @@ -131,13 +131,14 @@ public: /** * Get void pointer to last message value */ - void * get_void_ptr() { return (void*)&_msg_current; } + void *get_void_ptr() { return (void *)&_msg_current; } protected: /** * Called on topic update, saves the current message and then calls the provided callback function */ - void callback(const M &msg) { + void callback(const M &msg) + { /* Store data */ _msg_current = msg; @@ -151,7 +152,8 @@ protected: /** * Saves the ros subscriber to keep ros subscription alive */ - void set_ros_sub(ros::Subscriber ros_sub) { + void set_ros_sub(ros::Subscriber ros_sub) + { _ros_sub = ros_sub; } @@ -180,8 +182,8 @@ public: * @param list subscriber is added to this list */ SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List *list) : + unsigned interval, + List *list) : Subscriber(), uORB::Subscription(meta, interval, list) {} @@ -211,7 +213,7 @@ public: /** * Get void pointer to last message value */ - void * get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } + void *get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } }; template @@ -227,9 +229,9 @@ public: * @param list subscriber is added to this list */ SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List *list) : + unsigned interval, + std::function callback, + List *list) : SubscriberUORB(meta, interval, list), _callback(callback) {} diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h index 60fc9b93cd..c7271c1575 100755 --- a/src/platforms/ros/eigen_math.h +++ b/src/platforms/ros/eigen_math.h @@ -9,8 +9,7 @@ #define EIGEN_MATH_H_ -struct eigen_matrix_instance -{ +struct eigen_matrix_instance { int numRows; int numCols; float *pData; diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index a7ee6fd844..6fad681c99 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -60,6 +60,7 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; @@ -69,6 +70,7 @@ __EXPORT float _wrap_pi(float bearing) } c = 0; + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; @@ -88,6 +90,7 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; @@ -97,6 +100,7 @@ __EXPORT float _wrap_2pi(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += M_TWOPI_F; @@ -116,6 +120,7 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; + while (bearing >= 180.0f) { bearing -= 360.0f; @@ -125,6 +130,7 @@ __EXPORT float _wrap_180(float bearing) } c = 0; + while (bearing < -180.0f) { bearing += 360.0f; @@ -144,6 +150,7 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; + while (bearing >= 360.0f) { bearing -= 360.0f; @@ -153,6 +160,7 @@ __EXPORT float _wrap_360(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += 360.0f; diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index 46c836b36b..ce863d10e3 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -52,7 +52,7 @@ AttitudeEstimator::AttitudeEstimator() : { } -void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg) +void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) { px4::vehicle_attitude msg_v_att; @@ -64,8 +64,8 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP int index = 1; quat(0) = (float)msg->pose[index].orientation.w; quat(1) = (float)msg->pose[index].orientation.x; - quat(2) = (float)-msg->pose[index].orientation.y; - quat(3) = (float)-msg->pose[index].orientation.z; + quat(2) = (float) - msg->pose[index].orientation.y; + quat(3) = (float) - msg->pose[index].orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); @@ -73,11 +73,13 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP msg_v_att.q[3] = quat(3); math::Matrix<3, 3> rot = quat.to_dcm(); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { PX4_R(msg_v_att.R, i, j) = rot(i, j); } } + msg_v_att.R_valid = true; math::Vector<3> euler = rot.to_euler(); @@ -93,7 +95,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP _vehicle_attitude_pub.publish(msg_v_att); } -void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) +void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) { px4::vehicle_attitude msg_v_att; @@ -105,8 +107,8 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) int index = 1; quat(0) = (float)msg->orientation.w; quat(1) = (float)msg->orientation.x; - quat(2) = (float)-msg->orientation.y; - quat(3) = (float)-msg->orientation.z; + quat(2) = (float) - msg->orientation.y; + quat(3) = (float) - msg->orientation.z; msg_v_att.q[0] = quat(0); msg_v_att.q[1] = quat(1); @@ -114,11 +116,13 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) msg_v_att.q[3] = quat(3); math::Matrix<3, 3> rot = quat.to_dcm(); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { PX4_R(msg_v_att.R, i, j) = rot(i, j); } } + msg_v_att.R_valid = true; math::Vector<3> euler = rot.to_euler(); @@ -136,10 +140,10 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr& msg) int main(int argc, char **argv) { - ros::init(argc, argv, "attitude_estimator"); - AttitudeEstimator m; + ros::init(argc, argv, "attitude_estimator"); + AttitudeEstimator m; - ros::spin(); + ros::spin(); - return 0; + return 0; } diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h index 7c09985f34..f760a39d8b 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h @@ -42,15 +42,16 @@ #include #include -class AttitudeEstimator { +class AttitudeEstimator +{ public: AttitudeEstimator(); ~AttitudeEstimator() {} protected: - void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr& msg); - void ImuCallback(const sensor_msgs::ImuConstPtr& msg); + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); + void ImuCallback(const sensor_msgs::ImuConstPtr &msg); ros::NodeHandle _n; ros::Subscriber _sub_modelstates; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index d8ff739b9e..b9fc296f9d 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -57,7 +57,7 @@ Commander::Commander() : { } -void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg) +void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; @@ -72,14 +72,16 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* fill actuator armed */ float arm_th = 0.95; _msg_actuator_armed.timestamp = px4::get_time_micros(); + if (_msg_actuator_armed.armed) { /* Check for disarm */ - if (msg->r < -arm_th && msg->z < (1-arm_th)) { + if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; } + } else { /* Check for arm */ - if (msg->r > arm_th && msg->z < (1-arm_th)) { + if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; } } @@ -107,8 +109,8 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon int main(int argc, char **argv) { - ros::init(argc, argv, "commander"); - Commander m; - ros::spin(); - return 0; + ros::init(argc, argv, "commander"); + Commander m; + ros::spin(); + return 0; } diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index f3c2f6f1a9..bd4092b3a2 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -43,7 +43,8 @@ #include #include -class Commander { +class Commander +{ public: Commander(); @@ -53,7 +54,7 @@ protected: /** * Based on manual control input the status will be set */ - void ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg); + void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 475d0c4d23..688df50e0c 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -72,7 +72,7 @@ ManualInput::ManualInput() : } -void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) +void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) { px4::manual_control_setpoint msg_out; @@ -99,10 +99,11 @@ void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr& msg) _man_ctrl_sp_pub.publish(msg_out); } -void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, - double deadzone, float &out) +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, + double deadzone, float &out) { double val = msg->axes[map_index]; + if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; } @@ -110,8 +111,8 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, do int main(int argc, char **argv) { - ros::init(argc, argv, "manual_input"); - ManualInput m; - ros::spin(); - return 0; + ros::init(argc, argv, "manual_input"); + ManualInput m; + ros::spin(); + return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index fb516d3751..93e0abe64d 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -41,7 +41,8 @@ #include "ros/ros.h" #include -class ManualInput { +class ManualInput +{ public: ManualInput(); @@ -51,13 +52,13 @@ protected: /** * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic */ - void JoyCallback(const sensor_msgs::JoyConstPtr& msg); + void JoyCallback(const sensor_msgs::JoyConstPtr &msg); /** * Helper function to map and scale joystick input */ - void MapAxis(const sensor_msgs::JoyConstPtr& msg, int map_index, double scale, double offset, double deadzone, - float &out); + void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, + float &out); ros::NodeHandle _n; ros::Subscriber _joy_sub; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 5db1800525..e2180af41b 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -37,12 +37,13 @@ * * @author Roman Bapst */ - #include - #include - #include - #include +#include +#include +#include +#include - class MultirotorMixer { +class MultirotorMixer +{ public: MultirotorMixer(); @@ -68,11 +69,11 @@ private: struct { float control[6]; - }inputs; + } inputs; struct { float control[6]; - }outputs; + } outputs; bool _armed; ros::Subscriber _sub_actuator_armed; @@ -112,18 +113,22 @@ MultirotorMixer::MultirotorMixer(): _rotor_count(4), _rotors(_config_index[2]) //XXX + eurocconfig hardcoded { - _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback,this); - _pub = _n.advertise("/mixed_motor_commands",10); + _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); + _pub = _n.advertise("/mixed_motor_commands", 10); + if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 150.0); } + if (!_n.hasParam("motor_offset_radps")) { _n.setParam("motor_offset_radps", 600.0); } - _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this); + + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this); } -void MultirotorMixer::mix() { +void MultirotorMixer::mix() +{ float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); @@ -134,7 +139,7 @@ void MultirotorMixer::mix() { /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale - + pitch * _rotors[i].pitch_scale + thrust; + + pitch * _rotors[i].pitch_scale + thrust; /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { @@ -145,12 +150,14 @@ void MultirotorMixer::mix() { if (out < min_out) { min_out = out; } + if (out > max_out) { max_out = out; } outputs.control[i] = out; } + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); @@ -158,8 +165,8 @@ void MultirotorMixer::mix() { /* mix again with adjusted controls */ for (unsigned i = 0; i < _rotor_count; i++) { outputs.control[i] = scale_in - * (roll * _rotors[i].roll_scale - + pitch * _rotors[i].pitch_scale) + thrust; + * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) + thrust; } } else { @@ -171,6 +178,7 @@ void MultirotorMixer::mix() { /* scale down all outputs if some outputs are too large, reduce total thrust */ float scale_out; + if (max_out > 1.0f) { scale_out = 1.0f / max_out; @@ -184,10 +192,10 @@ void MultirotorMixer::mix() { } } - void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) { // read message - for(int i = 0;i < msg.NUM_ACTUATOR_CONTROLS;i++) { + for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) { inputs.control[i] = msg.control[i]; } @@ -200,25 +208,28 @@ void MultirotorMixer::mix() { double offset; _n.getParamCached("motor_scaling_radps", scaling); _n.getParamCached("motor_offset_radps", offset); + if (_armed) { for (int i = 0; i < _rotor_count; i++) { rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset); } + } else { for (int i = 0; i < _rotor_count; i++) { rotor_vel_msg.motor_speed.push_back(0.0); } } + _pub.publish(rotor_vel_msg); } - int main(int argc, char **argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "mc_mixer"); - MultirotorMixer mixer; - ros::spin(); + ros::init(argc, argv, "mc_mixer"); + MultirotorMixer mixer; + ros::spin(); - return 0; + return 0; } void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)