From 220fb19eb74b330bd3e97efc628aeb1bda510dcf Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 14:25:28 -0700 Subject: Removed actuator_control_mode flags...Using pre-existing flags instead --- msg/vehicle_control_mode.msg | 1 - src/modules/commander/commander.cpp | 10 ---------- src/modules/fw_att_control/fw_att_control_main.cpp | 7 ++++--- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++----------- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- src/modules/uORB/topics/offboard_control_mode.h | 1 - src/modules/vtol_att_control/vtol_att_control_main.cpp | 10 ++++++---- 7 files changed, 18 insertions(+), 31 deletions(-) diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 44f018642..153a642bb 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -12,7 +12,6 @@ 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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 024e6c18a..2c5e0aa88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2302,7 +2302,6 @@ 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: @@ -2442,7 +2441,6 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; - control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; @@ -2453,14 +2451,6 @@ 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 a7b563b0f..5daeae477 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - //vehicle_control_mode_s + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -1077,8 +1077,9 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; - /* Only publish if actuator_control mode is not enabled */ - if(!_vcontrol_mode.flag_control_offboard_actuator_control_enabled) + /* Only publish if any of the proper modes are enabled */ + if(_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { /* publish the actuator controls */ if (_actuators_0_pub > 0) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a4c6c3da4..27a6d4bca 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,8 +540,6 @@ 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(); @@ -665,10 +663,10 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints - if ((mavlink_system.sysid == set_actuator_control_target.target_system || - set_actuator_control_target.target_system == 0) && - (mavlink_system.compid == set_actuator_control_target.target_component || - set_actuator_control_target.target_component == 0)){ + if ((mavlink_system.sysid == set_actuator_control_target.target_system || + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0)) { /* ignore all since we are setting raw actuators here */ offboard_control_mode.ignore_thrust = true; @@ -678,8 +676,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.actuator_control_mode = true; - offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { @@ -690,11 +686,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m actuator_controls.timestamp = hrt_absolute_time(); - for(size_t i = 0; i < 8 ; i++){ + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { actuator_controls.control[i] = set_actuator_control_target.controls[i]; } - if (_offboard_control_mode_pub < 0) { + if (_actuator_controls_pub < 0) { _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); } else { orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); @@ -783,7 +780,6 @@ 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 099b92404..a0d76b0a6 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 || !_v_control_mode.flag_control_offboard_actuator_control_enabled) { + if (!_actuators_0_circuit_breaker_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 956c3881e..559659a1d 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -61,7 +61,6 @@ 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 53a08f97a..74e1efd6c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -782,8 +782,9 @@ void VtolAttitudeControl::task_main() fill_mc_att_control_output(); fill_mc_att_rates_sp(); - /* Only publish if actuator_control mode is not enabled */ - if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); @@ -817,8 +818,9 @@ void VtolAttitudeControl::task_main() fill_fw_att_control_output(); fill_fw_att_rates_sp(); - /* Only publish if actuator_control mode is not enabled */ - if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) { if (_actuators_0_pub > 0) { orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); -- cgit v1.2.3