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

commander dummy node: extend to support switching between modes

Browse Source
This commit is contained in:
Thomas Gubler 2015-01-29 13:38:39 +01:00
parent 3c79b2a586
commit 5237364a5a
2 changed files with 61 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/platforms/ros/nodes/commander/commander.cpp
View File

@ -40,9 +40,6 @@
#include "commander.h"
#include <px4/manual_control_setpoint.h>
#include <px4/vehicle_control_mode.h>
#include <px4/vehicle_status.h>
#include <platforms/px4_middleware.h>
Commander::Commander() :
@ -62,12 +59,9 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
px4::vehicle_control_mode msg_vehicle_control_mode;
px4::vehicle_status msg_vehicle_status;
/* fill vehicle control mode */
//XXX hardcoded
/* fill vehicle control mode based on (faked) stick positions*/
EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode);
msg_vehicle_control_mode.timestamp = px4::get_time_micros();
msg_vehicle_control_mode.flag_control_manual_enabled = true;
msg_vehicle_control_mode.flag_control_rates_enabled = true;
msg_vehicle_control_mode.flag_control_attitude_enabled = true;
/* fill actuator armed */
float arm_th = 0.95;
@ -77,21 +71,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
/* Check for disarm */
if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
_msg_actuator_armed.armed = false;
msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY;
}
} else {
/* Check for arm */
if (msg->r > arm_th && msg->z < (1 - arm_th)) {
_msg_actuator_armed.armed = true;
msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
}
}
/* fill vehicle status */
//XXX hardcoded
msg_vehicle_status.timestamp = px4::get_time_micros();
msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
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;
@ -107,6 +99,54 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
}
}
void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
px4::vehicle_status &msg_vehicle_status,
px4::vehicle_control_mode &msg_vehicle_control_mode) {
// XXX this is a minimal implementation. If more advanced functionalities are
// needed consider a full port of the commander
switch (msg->mode_switch) {
case px4::manual_control_setpoint::SWITCH_POS_NONE:
ROS_WARN("Joystick button mapping error, main mode not set");
break;
case px4::manual_control_setpoint::SWITCH_POS_OFF: // MANUAL
msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
msg_vehicle_control_mode.flag_control_manual_enabled = true;
msg_vehicle_control_mode.flag_control_rates_enabled = true;
msg_vehicle_control_mode.flag_control_attitude_enabled = true;
msg_vehicle_control_mode.flag_control_altitude_enabled = false;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = false;
msg_vehicle_control_mode.flag_control_position_enabled = false;
break;
case px4::manual_control_setpoint::SWITCH_POS_MIDDLE: // ASSIST
if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) {
msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL;
msg_vehicle_control_mode.flag_control_manual_enabled = true;
msg_vehicle_control_mode.flag_control_rates_enabled = true;
msg_vehicle_control_mode.flag_control_attitude_enabled = true;
msg_vehicle_control_mode.flag_control_altitude_enabled = true;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
msg_vehicle_control_mode.flag_control_position_enabled = true;
} else {
msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL;
msg_vehicle_control_mode.flag_control_manual_enabled = true;
msg_vehicle_control_mode.flag_control_rates_enabled = true;
msg_vehicle_control_mode.flag_control_attitude_enabled = true;
msg_vehicle_control_mode.flag_control_altitude_enabled = true;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
msg_vehicle_control_mode.flag_control_position_enabled = false;
}
break;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "commander");

9
src/platforms/ros/nodes/commander/commander.h
View File

@ -40,6 +40,8 @@
#include "ros/ros.h"
#include <px4/manual_control_setpoint.h>
#include <px4/vehicle_control_mode.h>
#include <px4/vehicle_status.h>
#include <px4/parameter_update.h>
#include <px4/actuator_armed.h>
@ -56,6 +58,13 @@ protected:
*/
void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
/**
* 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);
ros::NodeHandle _n;
ros::Subscriber _man_ctrl_sp_sub;
ros::Publisher _vehicle_control_mode_pub;