diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-08 08:15:44 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-08 08:15:44 +0100 |
commit | c118d17cb576b76df3bc1b765fb38c34421c7be4 (patch) | |
tree | 5f4dd4b604207f186dfcd16fc5bc923820a7985e /src/platforms | |
parent | 34b023d0a62bef93feb0ce3131e677f7c8546b45 (diff) | |
download | px4-firmware-c118d17cb576b76df3bc1b765fb38c34421c7be4.tar.gz px4-firmware-c118d17cb576b76df3bc1b765fb38c34421c7be4.tar.bz2 px4-firmware-c118d17cb576b76df3bc1b765fb38c34421c7be4.zip |
fix code style in src/platforms
Diffstat (limited to 'src/platforms')
-rw-r--r-- | src/platforms/px4_defines.h | 6 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 27 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 24 | ||||
-rwxr-xr-x | src/platforms/ros/eigen_math.h | 3 | ||||
-rw-r--r-- | src/platforms/ros/geo.cpp | 8 | ||||
-rw-r--r-- | src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 24 | ||||
-rw-r--r-- | src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h | 7 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 16 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.h | 5 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.cpp | 15 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.h | 9 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 53 |
12 files changed, 117 insertions, 80 deletions
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 28eef7e18..6b6a03893 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 624a466fd..7b14caed9 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<typename M> - Subscriber<M> *subscribe(const char *topic, void(*fp)(const M&)) + Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &)) { SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub); - ((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, + (SubscriberROS<M> *)sub); + ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber<M> *)sub; } @@ -93,11 +94,12 @@ public: * @param fb Callback, executed on receiving a new message */ template<typename M, typename T> - Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) + Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) { SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub); - ((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, + (SubscriberROS<M> *)sub); + ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber<M> *)sub; } @@ -110,8 +112,9 @@ public: Subscriber<M> *subscribe(const char *topic) { SubscriberBase *sub = new SubscriberROS<M>(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub); - ((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, + (SubscriberROS<M> *)sub); + ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber<M> *)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<typename M> Subscriber<M> *subscribe(const struct orb_metadata *meta, - std::function<void(const M &)> callback, - unsigned interval) + std::function<void(const M &)> callback, + unsigned interval) { SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs); @@ -216,7 +221,7 @@ public: template<typename M> Subscriber<M> *subscribe(const struct orb_metadata *meta, - unsigned interval) + unsigned interval) { SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs); diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 107c60189..aaacf9e48 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<typename M> class SubscriberROS : public Subscriber<M> { -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<uORB::SubscriptionNode *> *list) : + unsigned interval, + List<uORB::SubscriptionNode *> *list) : Subscriber<M>(), uORB::Subscription<M>(meta, interval, list) {} @@ -211,7 +213,7 @@ public: /** * Get void pointer to last message value */ - void * get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); } + void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); } }; template<typename M> @@ -227,9 +229,9 @@ public: * @param list subscriber is added to this list */ SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function<void(const M &)> callback, - List<uORB::SubscriptionNode *> *list) : + unsigned interval, + std::function<void(const M &)> callback, + List<uORB::SubscriptionNode *> *list) : SubscriberUORB<M>(meta, interval, list), _callback(callback) {} diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h index 60fc9b93c..c7271c157 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 a7ee6fd84..6fad681c9 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 46c836b36..ce863d10e 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 7c09985f3..f760a39d8 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 <gazebo_msgs/ModelStates.h> #include <sensor_msgs/Imu.h> -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 d8ff739b9..b9fc296f9 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 f3c2f6f1a..bd4092b3a 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -43,7 +43,8 @@ #include <px4/parameter_update.h> #include <px4/actuator_armed.h> -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 475d0c4d2..688df50e0 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 fb516d375..93e0abe64 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 <sensor_msgs/Joy.h> -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 5db180052..e2180af41 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 <romanbapst@yahoo.de> */ - #include <ros/ros.h> - #include <px4.h> - #include <lib/mathlib/mathlib.h> - #include <mav_msgs/MotorSpeed.h> +#include <ros/ros.h> +#include <px4.h> +#include <lib/mathlib/mathlib.h> +#include <mav_msgs/MotorSpeed.h> - 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<mav_msgs::MotorSpeed>("/mixed_motor_commands",10); + _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); + _pub = _n.advertise<mav_msgs::MotorSpeed>("/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) |