diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-02 16:11:16 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-02 16:11:28 +0100 |
commit | 530c38b6fa3bc0294d66801a0d7a36e8888e903f (patch) | |
tree | b3c49a3b87e137c9d2e40208178f235cca7d6eeb /src/platforms/ros | |
parent | 3932013777c232c8c91e34144f54f6fd2f635175 (diff) | |
download | px4-firmware-530c38b6fa3bc0294d66801a0d7a36e8888e903f.tar.gz px4-firmware-530c38b6fa3bc0294d66801a0d7a36e8888e903f.tar.bz2 px4-firmware-530c38b6fa3bc0294d66801a0d7a36e8888e903f.zip |
dummy commander and mixer: armed status
Diffstat (limited to 'src/platforms/ros')
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 24 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.h | 2 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 18 |
3 files changed, 36 insertions, 8 deletions
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 1972d8cfa..d8ff739b9 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -42,7 +42,6 @@ #include <px4/manual_control_setpoint.h> #include <px4/vehicle_control_mode.h> -#include <px4/actuator_armed.h> #include <px4/vehicle_status.h> #include <platforms/px4_middleware.h> @@ -53,14 +52,14 @@ Commander::Commander() : _actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)), _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), - _msg_parameter_update() + _msg_parameter_update(), + _msg_actuator_armed() { } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr& msg) { px4::vehicle_control_mode msg_vehicle_control_mode; - px4::actuator_armed msg_actuator_armed; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode */ @@ -71,9 +70,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon msg_vehicle_control_mode.flag_control_attitude_enabled = true; /* fill actuator armed */ - //XXX hardcoded - msg_actuator_armed.timestamp = px4::get_time_micros(); - msg_actuator_armed.armed = true; + 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)) { + _msg_actuator_armed.armed = false; + } + } else { + /* Check for arm */ + if (msg->r > arm_th && msg->z < (1-arm_th)) { + _msg_actuator_armed.armed = true; + } + } /* fill vehicle status */ //XXX hardcoded @@ -83,9 +92,10 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; 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); + _actuator_armed_pub.publish(_msg_actuator_armed); _vehicle_status_pub.publish(msg_vehicle_status); /* Fill parameter update */ diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index cd9be5135..f3c2f6f1a 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -41,6 +41,7 @@ #include "ros/ros.h" #include <px4/manual_control_setpoint.h> #include <px4/parameter_update.h> +#include <px4/actuator_armed.h> class Commander { public: @@ -62,5 +63,6 @@ protected: ros::Publisher _parameter_update_pub; px4::parameter_update _msg_parameter_update; + px4::actuator_armed _msg_actuator_armed; }; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index e8c65905f..4dc428a27 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -54,6 +54,7 @@ public: }; void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); + void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg); private: @@ -73,6 +74,9 @@ private: float control[6]; }outputs; + bool _armed; + ros::Subscriber _sub_actuator_armed; + void mix(); }; @@ -113,6 +117,7 @@ MultirotorMixer::MultirotorMixer(): if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 1500.0); } + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback,this); } void MultirotorMixer::mix() { @@ -184,7 +189,13 @@ void MultirotorMixer::mix() { } // mix - mix(); + if (_armed) { + mix(); + } else { + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = 0.0f; + } + } // publish message mav_msgs::MotorSpeed rotor_vel_msg; @@ -204,3 +215,8 @@ void MultirotorMixer::mix() { return 0; } + +void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg) +{ + _armed = msg.armed; +} |