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

Added offboard actuator controls flags to offboard control mode and vehicle control mode to disable controls in att_control apps

Browse Source
This commit is contained in:
Matt Beall 2015-02-24 16:43:49 -07:00 committed by Thomas Gubler
parent 5e199b3984
commit e2de72b882
7 changed files with 57 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_control_mode.msg
View File

@ -12,6 +12,7 @@ bool flag_system_hil_enabled
bool flag_control_manual_enabled # true if manual input is mixed in
bool flag_control_auto_enabled # true if onboard autopilot should act
bool flag_control_offboard_enabled # true if offboard control should be used
bool flag_control_offboard_actuator_control_enabled #true if raw actuator control in offboard used
bool flag_control_rates_enabled # true if rates are stabilized
bool flag_control_attitude_enabled # true if attitude stabilization is mixed in
bool flag_control_force_enabled # true if force control is mixed in

9
src/modules/commander/commander.cpp
View File

@ -2302,6 +2302,7 @@ set_control_mode()
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
control_mode.flag_control_offboard_actuator_control_enabled = false;
switch (status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
@ -2452,6 +2453,14 @@ set_control_mode()
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust &&
offboard_control_mode.ignore_attitude &&
offboard_control_mode.ignore_bodyrate &&
offboard_control_mode.ignore_position &&
offboard_control_mode.ignore_velocity &&
offboard_control_mode.ignore_acceleration_force &&
offboard_control_mode.actuator_control_mode;
break;
default:

28
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -1077,20 +1077,24 @@ FixedwingAttitudeControl::task_main()
_actuators_airframe.timestamp = hrt_absolute_time();
_actuators_airframe.timestamp_sample = _att.timestamp;
/* publish the actuator controls */
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
/* Only publish if actuator_control mode is not enabled */
if(!_vcontrol_mode.flag_control_offboard_actuator_control_enabled)
{
/* publish the actuator controls */
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub > 0) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
if (_actuators_2_pub > 0) {
/* publish the actuator controls*/
orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
} else {
/* advertise and publish */
_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
}
}
}

7
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -540,7 +540,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
/* yawrate ignore flag mapps to ignore_bodyrate */
offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
offboard_control_mode.actuator_control_mode = false;
offboard_control_mode.timestamp = hrt_absolute_time();
@ -678,9 +678,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
offboard_control_mode.timestamp = hrt_absolute_time();
if (_offboard_control_mode_pub < 0) {
_offboard_control_mode_pub = orb_advertise(O
actuator_controls.timestamp = RB_ID(offboard_control_mode), &offboard_control_mode);
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
} else {
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
}
@ -774,6 +772,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
offboard_control_mode.ignore_attitude = ignore_attitude;
}
offboard_control_mode.actuator_control_mode = false;
offboard_control_mode.ignore_position = true;
offboard_control_mode.ignore_velocity = true;

2
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -804,7 +804,7 @@ MulticopterAttitudeControl::task_main()
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _v_att.timestamp;
if (!_actuators_0_circuit_breaker_enabled) {
if (!_actuators_0_circuit_breaker_enabled || !_v_control_mode.flag_control_offboard_actuator_control_enabled) {
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);

1
src/modules/uORB/topics/offboard_control_mode.h
View File

@ -61,6 +61,7 @@ struct offboard_control_mode_s {
bool ignore_position;
bool ignore_velocity;
bool ignore_acceleration_force;
bool actuator_control_mode;
}; /**< offboard control inputs */
/**

44
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -782,18 +782,22 @@ void VtolAttitudeControl::task_main()
fill_mc_att_control_output();
fill_mc_att_rates_sp();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
/* Only publish if actuator_control mode is not enabled */
if(!_v_control_mode.flag_control_offboard_actuator_control_enabled)
{
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
}
@ -813,18 +817,22 @@ void VtolAttitudeControl::task_main()
fill_fw_att_control_output();
fill_fw_att_rates_sp();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
/* Only publish if actuator_control mode is not enabled */
if(!_v_control_mode.flag_control_offboard_actuator_control_enabled)
{
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
}