From 0f5cb756926b1d0156dfee5abe5adb5415a3ede2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 19 May 2015 10:41:15 -0700 Subject: [PATCH] ROS: Fixes for ROS build The ROS build included some files that used isfinite vs PX4_ISFINITE. The AppState class also needed to be supported for ROS. Signed-off-by: Mark Charlebois --- src/examples/subscriber/subscriber_example.cpp | 10 +++++----- .../mc_att_control_multiplatform/mc_att_control.cpp | 8 ++++---- .../mc_pos_control_multiplatform/mc_pos_control.cpp | 8 ++++---- src/platforms/px4_defines.h | 2 ++ src/platforms/px4_nodehandle.h | 9 ++++++--- src/platforms/ros/geo.cpp | 8 ++++---- 6 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index ae58f634ce5..c4af0757b5a 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -45,7 +45,7 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg) { - PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); + PX4_INFO("I heard: [%lu]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -84,21 +84,21 @@ SubscriberExample::SubscriberExample() : */ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { - PX4_INFO("rc_channels_callback (method): [%llu]", + PX4_INFO("rc_channels_callback (method): [%lu]", msg.data().timestamp_last_valid); - PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%lu]", _sub_rc_chan->data().timestamp_last_valid); } void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { - PX4_INFO("vehicle_attitude_callback (method): [%llu]", + PX4_INFO("vehicle_attitude_callback (method): [%lu]", msg.data().timestamp); } void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { - PX4_INFO("parameter_update_callback (method): [%llu]", + PX4_INFO("parameter_update_callback (method): [%lu]", msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 2ad89e606cb..f5341155b8d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -236,10 +236,10 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti control_attitude_rates(dt); /* publish actuator controls */ - _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.data().timestamp = px4::get_time_micros(); if (!_actuators_0_circuit_breaker_enabled) { diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index b5962263366..c14a4b76273 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -537,7 +537,7 @@ MulticopterPositionControl::control_auto(float dt) _pos_sp = pos_sp_s.edivide(scale); /* update yaw setpoint if needed */ - if (isfinite(_pos_sp_triplet->data().current.yaw)) { + if (PX4_ISFINITE(_pos_sp_triplet->data().current.yaw)) { _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; } @@ -554,9 +554,9 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) { /* Make sure that the position setpoint is valid */ - if (!isfinite(_pos_sp_triplet->data().current.lat) || - !isfinite(_pos_sp_triplet->data().current.lon) || - !isfinite(_pos_sp_triplet->data().current.alt)) { + if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) || + !PX4_ISFINITE(_pos_sp_triplet->data().current.lon) || + !PX4_ISFINITE(_pos_sp_triplet->data().current.alt)) { _pos_sp_triplet->data().current.valid = false; } } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c17b39e4c5b..f350a26326d 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -66,6 +66,8 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) +#define PX4_ISFINITE(x) std::isfinite(x) + #elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) /* * Building for NuttX or POSIX diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 259dab80057..540cab5231a 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -42,7 +42,6 @@ #include "px4_subscriber.h" #include "px4_publisher.h" #include "px4_middleware.h" -#include "px4_posix.h" #include "px4_app.h" #if defined(__PX4_ROS) @@ -64,10 +63,11 @@ class NodeHandle : private ros::NodeHandle { public: - NodeHandle() : + NodeHandle(AppState &a) : ros::NodeHandle(), _subs(), - _pubs() + _pubs(), + _appState(a) {} ~NodeHandle() @@ -138,6 +138,9 @@ public: protected: std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ + + AppState &_appState; + }; #else //Building for NuttX class __EXPORT NodeHandle diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 04094de8bb6..2f277d7f167 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -55,7 +55,7 @@ __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } @@ -85,7 +85,7 @@ __EXPORT float _wrap_pi(float bearing) __EXPORT float _wrap_2pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } @@ -115,7 +115,7 @@ __EXPORT float _wrap_2pi(float bearing) __EXPORT float _wrap_180(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; } @@ -145,7 +145,7 @@ __EXPORT float _wrap_180(float bearing) __EXPORT float _wrap_360(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing)) { + if (!PX4_ISFINITE(bearing)) { return bearing; }