From e2de72b882d7c74cdaafcfc74ab7176ef94a4455 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:43:49 -0700 Subject: Added offboard actuator controls flags to offboard control mode and vehicle control mode to disable controls in att_control apps --- src/modules/commander/commander.cpp | 9 ++++ src/modules/fw_att_control/fw_att_control_main.cpp | 28 ++++++------ src/modules/mavlink/mavlink_receiver.cpp | 7 ++- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- src/modules/uORB/topics/offboard_control_mode.h | 1 + .../vtol_att_control/vtol_att_control_main.cpp | 50 +++++++++++++--------- 6 files changed, 59 insertions(+), 38 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5a362666c..e688b8fa5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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: diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5052b41c3..a7b563b0f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -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); + } } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c506e005..48866f036 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0d76b0a6..099b92404 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h index 559659a1d..956c3881e 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -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 */ /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 288ba2ebe..53a08f97a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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); - - } 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); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + /* 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); + } + + 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); + } } } } @@ -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); + } } } } -- cgit v1.2.3