fix code style in src/platforms

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;

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;
 

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;
 }

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;

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;
 }

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;

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;
 }

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;

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)