Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

ros: commander dummy node: fix offboard support

Browse Source
This commit is contained in:
Thomas Gubler
2015-02-15 18:31:49 +01:00
parent ae64e4e05c
commit 8b40112e9f
2 changed files with 55 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/ros/nodes/commander/commander.cpp
+47 -23
View File
@@ -53,12 +53,17 @@ Commander::Commander() :
_msg_parameter_update(),
_msg_actuator_armed(),
_msg_vehicle_control_mode(),
_msg_offboard_control_mode()
_msg_offboard_control_mode(),
_got_manual_control(false)
{
/* Default to offboard control: when no joystick is connected offboard control should just work */
}
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*/
@@ -103,6 +108,36 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
}
}
void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
px4::vehicle_control_mode &msg_vehicle_control_mode)
{
msg_vehicle_control_mode.flag_control_manual_enabled = false;
msg_vehicle_control_mode.flag_control_offboard_enabled = true;
msg_vehicle_control_mode.flag_control_auto_enabled = false;
msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate ||
!msg_offboard_control_mode.ignore_attitude ||
!msg_offboard_control_mode.ignore_position ||
!msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_acceleration_force;
msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude ||
!msg_offboard_control_mode.ignore_position ||
!msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_acceleration_force;
msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
}
void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
px4::vehicle_status &msg_vehicle_status,
px4::vehicle_control_mode &msg_vehicle_control_mode) {
@@ -110,32 +145,14 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
// needed consider a full port of the commander
if (msg->offboard_switch)
if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON)
{
msg_vehicle_control_mode.flag_control_rates_enabled = !_msg_offboard_control_mode.ignore_bodyrate ||
!_msg_offboard_control_mode.ignore_attitude ||
!_msg_offboard_control_mode.ignore_position ||
!_msg_offboard_control_mode.ignore_velocity ||
!_msg_offboard_control_mode.ignore_acceleration_force;
msg_vehicle_control_mode.flag_control_attitude_enabled = !_msg_offboard_control_mode.ignore_attitude ||
!_msg_offboard_control_mode.ignore_position ||
!_msg_offboard_control_mode.ignore_velocity ||
!_msg_offboard_control_mode.ignore_acceleration_force;
msg_vehicle_control_mode.flag_control_velocity_enabled = !_msg_offboard_control_mode.ignore_velocity ||
!_msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = !_msg_offboard_control_mode.ignore_velocity ||
!_msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_position_enabled = !_msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_altitude_enabled = !_msg_offboard_control_mode.ignore_position;
SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
return;
}
msg_vehicle_control_mode.flag_control_offboard_enabled = false;
switch (msg->mode_switch) {
case px4::manual_control_setpoint::SWITCH_POS_NONE:
ROS_WARN("Joystick button mapping error, main mode not set");
@@ -184,6 +201,13 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
{
_msg_offboard_control_mode = *msg;
/* Force system into offboard control mode */
if (!_got_manual_control) {
SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
}
}
int main(int argc, char **argv)
src/platforms/ros/nodes/commander/commander.h
+8
View File
@@ -71,6 +71,12 @@ protected:
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);
ros::NodeHandle _n;
ros::Subscriber _man_ctrl_sp_sub;
ros::Subscriber _offboard_control_mode_sub;
@@ -84,4 +90,6 @@ protected:
px4::vehicle_control_mode _msg_vehicle_control_mode;
px4::offboard_control_mode _msg_offboard_control_mode;
bool _got_manual_control;
};