fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 07:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander dummy node: small fix for vehicle_control_mode

Browse Source
This commit is contained in:
Thomas Gubler
2015-01-29 14:34:10 +01:00
parent 6dead1d576
commit a0ae5aeebb
2 changed files with 8 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/commander/commander.cpp
+7 -5
View File
@@ -50,18 +50,18 @@ Commander::Commander() :
_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
_msg_parameter_update(), _msg_parameter_update(),
_msg_actuator_armed() _msg_actuator_armed(),
_msg_vehicle_control_mode()
{ {
} }
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; 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 */
float arm_th = 0.95; float arm_th = 0.95;
@@ -71,6 +71,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
/* Check for disarm */ /* 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; _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;
} }
@@ -78,6 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
/* Check for arm */ /* 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; _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;
} }
} }
@@ -88,7 +90,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
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);
src/platforms/ros/nodes/commander/commander.h
+1
View File
@@ -74,5 +74,6 @@ 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;
}; };