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/ros | |
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/ros')
-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 |
9 files changed, 84 insertions, 56 deletions
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) |