From a8d4572cfa23af889c752d95d0512e5ced70a98a Mon Sep 17 00:00:00 2001 From: Bharat Tak Date: Fri, 3 Apr 2015 11:14:40 +0200 Subject: Fix issue #1972. Tested in gazebo and verified vehicle_status.arming_state on rqt_plot --- src/platforms/ros/nodes/commander/commander.cpp | 31 ++++++++++++------------- src/platforms/ros/nodes/commander/commander.h | 7 +++--- 2 files changed, 19 insertions(+), 19 deletions(-) (limited to 'src/platforms') diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 0c32026f3..abaa6fc60 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -53,6 +53,7 @@ Commander::Commander() : _msg_parameter_update(), _msg_actuator_armed(), _msg_vehicle_control_mode(), + _msg_vehicle_status(), _msg_offboard_control_mode(), _got_manual_control(false) { @@ -64,10 +65,9 @@ Commander::Commander() : void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { _got_manual_control = true; - px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ - EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + EvalSwitches(msg, _msg_vehicle_status, _msg_vehicle_control_mode); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); /* fill actuator armed */ @@ -79,7 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; _msg_vehicle_control_mode.flag_armed = false; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_STANDBY; } } else { @@ -87,19 +87,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; _msg_vehicle_control_mode.flag_armed = true; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; } } /* fill vehicle status */ - msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; - msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; - msg_vehicle_status.is_rotary_wing = true; + _msg_vehicle_status.timestamp = px4::get_time_micros(); + _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; + _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + _msg_vehicle_status.is_rotary_wing = true; _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); - _vehicle_status_pub.publish(msg_vehicle_status); + _vehicle_status_pub.publish(_msg_vehicle_status); /* Fill parameter update */ if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { @@ -206,12 +206,11 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - px4::vehicle_status msg_vehicle_status; - msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; - msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; - msg_vehicle_status.is_rotary_wing = true; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + _msg_vehicle_status.timestamp = px4::get_time_micros(); + _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; + _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + _msg_vehicle_status.is_rotary_wing = true; + _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; _msg_actuator_armed.armed = true; @@ -223,7 +222,7 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); - _vehicle_status_pub.publish(msg_vehicle_status); + _vehicle_status_pub.publish(_msg_vehicle_status); } } diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index fc88260ba..856144389 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ protected: * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; @@ -88,6 +88,7 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; px4::vehicle_control_mode _msg_vehicle_control_mode; + px4::vehicle_status _msg_vehicle_status; px4::offboard_control_mode _msg_offboard_control_mode; bool _got_manual_control; -- cgit v1.2.3