dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix code style in src/platforms

This commit is contained in:
Thomas Gubler 2015-01-08 08:15:44 +01:00
parent 34b023d0a6
commit c118d17cb5
12 changed files with 117 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/platforms/px4_defines.h
View File

@ -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

27
src/platforms/px4_nodehandle.h
View File

@ -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);

24
src/platforms/px4_subscriber.h
View File

@ -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)
{}

3
src/platforms/ros/eigen_math.h
View File

@ -9,8 +9,7 @@
#define EIGEN_MATH_H_
struct eigen_matrix_instance
{
struct eigen_matrix_instance {
int numRows;
int numCols;
float *pData;

8
src/platforms/ros/geo.cpp
View File

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

24
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
View File

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

7
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
View File

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

16
src/platforms/ros/nodes/commander/commander.cpp
View File

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

5
src/platforms/ros/nodes/commander/commander.h
View File

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

15
src/platforms/ros/nodes/manual_input/manual_input.cpp
View File

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

9
src/platforms/ros/nodes/manual_input/manual_input.h
View File

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

53
src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
View File

@ -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)