From 2fff2ab9ac0d1357cf48c588bf358c520552b9db Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Oct 2016 18:12:09 +0200 Subject: [PATCH] Add switch for landing gear, pass it to actuators --- msg/manual_control_setpoint.msg | 1 + msg/rc_channels.msg | 3 +- msg/vehicle_attitude_setpoint.msg | 2 + .../fw_att_control/fw_att_control_main.cpp | 1 + .../mc_att_control/mc_att_control_main.cpp | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 29 +++++++++++- src/modules/sensors/sensor_params.c | 45 +++++++++++++++++++ src/modules/sensors/sensors.cpp | 17 +++++++ 8 files changed, 97 insertions(+), 2 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 599e63d5a906..2b5285b80313 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -52,4 +52,5 @@ uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD uint8 kill_switch # throttle kill: _NORMAL_, KILL uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT +uint8 gear_switch # landing gear switch: _DOWN_, UP int8 mode_slot # the slot a specific model selector is in diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 52d89df5fb9d..e62070dd91f1 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -21,11 +21,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 uint8 RC_CHANNELS_FUNCTION_TRANSITION=21 +uint8 RC_CHANNELS_FUNCTION_GEAR=22 uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[22] function # Functions mapping +int8[23] function # Functions mapping uint8 rssi # Receive signal strength indicator (0-100) bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 22e5b446923a..cb2f62576ed7 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -30,5 +30,7 @@ bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane m bool apply_flaps +float32 landing_gear + # WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint # TOPICS vehicle_attitude_setpoint 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 983f59944ace..80d7f880e6f6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1156,6 +1156,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; _actuators.control[5] = _manual.aux1; _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; + // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ 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 b5f3dc3b2686..b152f369533c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -976,6 +976,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.control[7] = _v_att_sp.landing_gear; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 1d2fd6fae431..c6c913b1f4fa 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -66,7 +66,6 @@ #include #include -#include #include #include #include @@ -1195,6 +1194,22 @@ void MulticopterPositionControl::control_auto(float dt) _reset_alt_sp = true; } + // During a mission or in loiter it's safe to retract the landing gear. + if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) && + !_vehicle_land_detected.landed) { + _att_sp.landing_gear = 1.0f; + + // During takeoff and landing, we better put it down again. + + } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF || + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _att_sp.landing_gear = -1.0f; + + } else { + // For the rest of the setpoint types, just leave it as is. + } + } else { /* no waypoint, do nothing, setpoint was already reset */ } @@ -1240,6 +1255,9 @@ MulticopterPositionControl::task_main() math::Vector<3> thrust_int; thrust_int.zero(); + // Let's be safe and have the landing gear down by default + _att_sp.landing_gear = -1.0f; + matrix::Dcmf R; R.identity(); @@ -2064,6 +2082,15 @@ MulticopterPositionControl::task_main() _att_sp.q_d_valid = true; } + if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && + !_vehicle_land_detected.landed) { + _att_sp.landing_gear = 1.0f; + + } else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + _att_sp.landing_gear = -1.0f; + } + + _att_sp.timestamp = hrt_absolute_time(); } else { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 90cbc3dfb073..3d9d543496bb 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2544,6 +2544,33 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); */ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0); +/** + * Landing gear switch channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0); /** * AUX1 Passthrough RC Channel * @@ -2976,6 +3003,24 @@ PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f); */ PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.25f); +/** + * Threshold for the landing gear switch + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel