fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix issue #1972. Tested in gazebo and verified vehicle_status.arming_state on rqt_plot

Browse Source
This commit is contained in:
Bharat Tak
2015-04-03 11:14:40 +02:00
parent 368a21ddab
commit a8d4572cfa
2 changed files with 19 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/commander/commander.cpp
+15 -16
View File
@@ -53,6 +53,7 @@ Commander::Commander() :
_msg_parameter_update(), _msg_parameter_update(),
_msg_actuator_armed(), _msg_actuator_armed(),
_msg_vehicle_control_mode(), _msg_vehicle_control_mode(),
_msg_vehicle_status(),
_msg_offboard_control_mode(), _msg_offboard_control_mode(),
_got_manual_control(false) _got_manual_control(false)
{ {
@@ -64,10 +65,9 @@ Commander::Commander() :
void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
{ {
_got_manual_control = true; _got_manual_control = true;
px4::vehicle_status msg_vehicle_status;
/* fill vehicle control mode based on (faked) stick positions*/ /* 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(); _msg_vehicle_control_mode.timestamp = px4::get_time_micros();
/* fill actuator armed */ /* 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)) { if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
_msg_actuator_armed.armed = false; _msg_actuator_armed.armed = false;
_msg_vehicle_control_mode.flag_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 { } else {
@@ -87,19 +87,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
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; _msg_actuator_armed.armed = true;
_msg_vehicle_control_mode.flag_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 */ /* fill vehicle status */
msg_vehicle_status.timestamp = px4::get_time_micros(); _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.HIL_STATE_OFF;
msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
msg_vehicle_status.is_rotary_wing = true; _msg_vehicle_status.is_rotary_wing = true;
_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
_actuator_armed_pub.publish(_msg_actuator_armed); _actuator_armed_pub.publish(_msg_actuator_armed);
_vehicle_status_pub.publish(msg_vehicle_status); _vehicle_status_pub.publish(_msg_vehicle_status);
/* Fill parameter update */ /* Fill parameter update */
if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { 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) { if (!_got_manual_control) {
SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); 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.timestamp = px4::get_time_micros(); _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF;
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.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; _msg_vehicle_status.is_rotary_wing = true;
msg_vehicle_status.is_rotary_wing = true; _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED;
msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
_msg_actuator_armed.armed = true; _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); _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
_actuator_armed_pub.publish(_msg_actuator_armed); _actuator_armed_pub.publish(_msg_actuator_armed);
_vehicle_status_pub.publish(msg_vehicle_status); _vehicle_status_pub.publish(_msg_vehicle_status);
} }
} }
src/platforms/ros/nodes/commander/commander.h
+4 -3
View File
@@ -68,14 +68,14 @@ protected:
* Set control mode flags based on stick positions (equiv to code in px4 commander) * Set control mode flags based on stick positions (equiv to code in px4 commander)
*/ */
void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, void EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
px4::vehicle_status &msg_vehicle_status, px4::vehicle_status &msg_vehicle_status,
px4::vehicle_control_mode &msg_vehicle_control_mode); px4::vehicle_control_mode &msg_vehicle_control_mode);
/** /**
* Sets offboard controll flags in msg_vehicle_control_mode * Sets offboard controll flags in msg_vehicle_control_mode
*/ */
void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_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::NodeHandle _n;
ros::Subscriber _man_ctrl_sp_sub; ros::Subscriber _man_ctrl_sp_sub;
@@ -88,6 +88,7 @@ protected:
px4::parameter_update _msg_parameter_update; px4::parameter_update _msg_parameter_update;
px4::actuator_armed _msg_actuator_armed; px4::actuator_armed _msg_actuator_armed;
px4::vehicle_control_mode _msg_vehicle_control_mode; px4::vehicle_control_mode _msg_vehicle_control_mode;
px4::vehicle_status _msg_vehicle_status;
px4::offboard_control_mode _msg_offboard_control_mode; px4::offboard_control_mode _msg_offboard_control_mode;
bool _got_manual_control; bool _got_manual_control;