From a0e213473818daff3263a1747e7c7c006d7a1d66 Mon Sep 17 00:00:00 2001 From: rien3271 Date: Mon, 21 Sep 2020 14:21:42 +0200 Subject: [PATCH 1/3] =?UTF-8?q?Impl=C3=A9mentation=20du=20switch?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Tools/sitl_gazebo | 2 +- msg/commander_state.msg | 1 + msg/manual_control_setpoint.msg | 1 + msg/vehicle_control_mode.msg | 1 + msg/vehicle_status.msg | 1 + src/modules/commander/Commander.cpp | 24 +++++++++++++++++++ src/modules/commander/px4_custom_mode.h | 3 ++- .../commander/state_machine_helper.cpp | 7 ++++++ src/modules/mc_att_control/mc_att_control.hpp | 5 ++++ .../mc_att_control/mc_att_control_main.cpp | 8 ++++++- src/modules/vtol_att_control/tiltrotor.cpp | 10 ++++++-- src/modules/vtol_att_control/tiltrotor.h | 3 +++ 12 files changed, 61 insertions(+), 5 deletions(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 9d2b19784cb3..c747e1f4930e 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 9d2b19784cb34bf8f397ab2e2f368f682f2d5d5a +Subproject commit c747e1f4930ed181b3afbc6687719e934048befa diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 38c94931f13c..5aeea70dfe48 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -17,5 +17,6 @@ uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 uint8 MAIN_STATE_AUTO_PRECLAND = 13 uint8 MAIN_STATE_ORBIT = 14 uint8 MAIN_STATE_MAX = 15 +uint8 MAIN_STATE_TILT = 16 //Tilt des rotors uint8 main_state # main state machine diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index febe5edc4eb5..c5e10190d165 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -65,3 +65,4 @@ uint8 mode_slot # the slot a specific model selector is in uint8 data_source # where this input is coming from uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL +bool tilt_switch # tilt enabled 2 position switch (optional): _MANUAL, TILT diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 08b8c43da8a9..89f9cf336eb8 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -18,3 +18,4 @@ bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled bool flag_control_fixed_hdg_enabled # true if using a fixed heading for user control +bool flag_control_tilt_enabled # true if the mode tilt rotor is enabled diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 85b45cf43e6a..139f5feb872a 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -44,6 +44,7 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle uint8 NAVIGATION_STATE_MAX = 22 +uint8 NAVIGATION_STATE_TILT= 23 # Tilt rotor mode uint8 RC_IN_MODE_DEFAULT = 0 uint8 RC_IN_MODE_OFF = 1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d6fd3c6b90ab..53e4f9823e4c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -403,6 +403,9 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND); + } else if (!strcmp(argv[2], "tilt")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_TILT); + } else { PX4_ERR("argument %s unsupported.", argv[2]); } @@ -673,6 +676,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ /* OFFBOARD */ main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_TILT) { + /* MANUAL */ + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_TILT, status_flags, &_internal_state); } } else { @@ -3196,7 +3203,24 @@ Commander::update_control_mode() && !status.is_vtol); switch (status.nav_state) { + + case vehicle_status_s::NAVIGATION_STATE_TILT: + control_mode.flag_control_tilt_enabled = true; + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_rattitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_acceleration_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + control_mode.flag_control_tilt_enabled = false; control_mode.flag_control_manual_enabled = true; control_mode.flag_control_rates_enabled = stabilization_required(); control_mode.flag_control_attitude_enabled = stabilization_required(); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index cd4c5637ad78..f54cf96b338f 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -51,7 +51,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_STABILIZED, PX4_CUSTOM_MAIN_MODE_RATTITUDE, - PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */ + PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */ + PX4_CUSTOM_MAIN_MODE_TILT }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6d440b4303dc..421b41a86673 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -266,6 +266,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai case commander_state_s::MAIN_STATE_MANUAL: case commander_state_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_ACRO: + case commander_state_s::MAIN_STATE_TILT: case commander_state_s::MAIN_STATE_RATTITUDE: ret = TRANSITION_CHANGED; break; @@ -698,6 +699,12 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } + break; + + case commander_state_s::MAIN_STATE_TILT: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TILT; + break; + default: break; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 589ee046c7dd..82a20ae88f7b 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -55,6 +55,8 @@ #include #include #include +#include +#include #include @@ -134,6 +136,9 @@ class MulticopterAttitudeControl : public ModuleBase uint8_t _quat_reset_counter{0}; + int vehicle_control_mode_sp_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + struct vehicle_control_mode_s vehicle_control_mode_sp {}; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_roll_p, (ParamFloat) _param_mc_pitch_p, 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 a9228fe64697..2e93eaaa5cbe 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -162,7 +162,13 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f); Eulerf euler_sp = q_sp_rpy; attitude_setpoint.roll_body = euler_sp(0); - attitude_setpoint.pitch_body = math::constrain(euler_sp(1),0.0f,1000.0f); + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sp_sub_fd, &vehicle_control_mode_sp); + bool _tilt_enabled=vehicle_control_mode_sp.flag_control_tilt_enabled; + if(_tilt_enabled==true){ + attitude_setpoint.pitch_body = math::constrain(euler_sp(1),0.0f,1000.0f); + } else { + attitude_setpoint.pitch_body = euler_sp(1); + } // The axis angle can change the yaw as well (noticeable at higher tilt angles). // This is the formula by how much the yaw changes: // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index e89919f1fa0b..e48bfb1d22f6 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -434,9 +434,15 @@ void Tiltrotor::fill_actuator_outputs() //AIGHTECH - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub_fd, &manual_control_sp); + orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub_fd, &manual_control_sp); + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sp_sub_fd, &vehicle_control_mode_sp); _tilt_control=manual_control_sp.x; - _actuators_out_0->control[4] = _tilt_control; + bool _tilt_enabled=vehicle_control_mode_sp.flag_control_tilt_enabled; + if(_tilt_enabled==true){ + _actuators_out_0->control[4] = _tilt_control; + } else { + _actuators_out_0->control[4] = 0; + } //PX4_INFO("%d %d %d", actuator_controls_s::INDEX_ROLL, actuator_controls_s::INDEX_PITCH, actuator_controls_s::INDEX_YAW); if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index b245fe1dc126..1cde1db66800 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -45,6 +45,7 @@ #include #include #include +#include class Tiltrotor : public VtolType { @@ -106,6 +107,8 @@ class Tiltrotor : public VtolType bool _tilt_motors_for_startup{false}; //AIGHTECH int manual_sp_sub_fd = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vehicle_control_mode_sp_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + struct vehicle_control_mode_s vehicle_control_mode_sp {}; struct manual_control_setpoint_s manual_control_sp {}; }; From 207f709f772b38c9074410ba15e841102d36726d Mon Sep 17 00:00:00 2001 From: rien3271 Date: Mon, 21 Sep 2020 15:20:13 +0200 Subject: [PATCH 2/3] =?UTF-8?q?Impl=C3=A9mentation=20du=20switch=20modifie?= =?UTF-8?q?r?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- msg/commander_state.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 5aeea70dfe48..9456a7bec727 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -17,6 +17,6 @@ uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 uint8 MAIN_STATE_AUTO_PRECLAND = 13 uint8 MAIN_STATE_ORBIT = 14 uint8 MAIN_STATE_MAX = 15 -uint8 MAIN_STATE_TILT = 16 //Tilt des rotors +uint8 MAIN_STATE_TILT = 16 uint8 main_state # main state machine From 68ee67af83c7e6539f550e9e743ea7e7ba981ad5 Mon Sep 17 00:00:00 2001 From: rien3271 Date: Tue, 22 Sep 2020 16:37:21 +0200 Subject: [PATCH 3/3] stabilisation de l'assiette + swich mode --- msg/manual_control_setpoint.msg | 2 +- msg/rc_channels.msg | 3 +- src/drivers/rc_input/crsf_telemetry.cpp | 4 ++ src/modules/commander/Commander.cpp | 13 ++---- src/modules/commander/commander_params.c | 6 +++ .../FixedwingAttitudeControl.hpp | 1 + .../fw_att_control/fw_att_control_params.c | 14 ++++++ .../mc_att_control/mc_att_control_main.cpp | 1 - .../mc_att_control/mc_att_control_params.c | 14 ++++++ src/modules/rc_update/params.c | 44 +++++++++++++++++++ src/modules/rc_update/rc_update.cpp | 3 ++ src/modules/rc_update/rc_update.h | 2 + 12 files changed, 94 insertions(+), 13 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index c5e10190d165..3a8c81ea321d 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -65,4 +65,4 @@ uint8 mode_slot # the slot a specific model selector is in uint8 data_source # where this input is coming from uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL -bool tilt_switch # tilt enabled 2 position switch (optional): _MANUAL, TILT +uint8 tilt_switch # tilt enabled 2 position switch (optional): _MANUAL, TILT diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 8986ffb5a5a4..eb26b7502864 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -27,11 +27,12 @@ uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23 uint8 RC_CHANNELS_FUNCTION_STAB=24 uint8 RC_CHANNELS_FUNCTION_AUX_6=25 uint8 RC_CHANNELS_FUNCTION_MAN=26 +uint8 RC_CHANNELS_FUNCTION_TILT=27 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[27] function # Functions mapping +int8[28] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 243e711c8c47..caf9280e4fa8 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -186,6 +186,10 @@ bool CRSFTelemetry::send_flight_mode() case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: flight_mode = "Rattitude"; break; + + case vehicle_status_s::NAVIGATION_STATE_TILT: + flight_mode = "Tilt"; + break; } return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 53e4f9823e4c..e13f0083aa77 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3207,16 +3207,8 @@ Commander::update_control_mode() case vehicle_status_s::NAVIGATION_STATE_TILT: control_mode.flag_control_tilt_enabled = true; control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -3227,6 +3219,7 @@ Commander::update_control_mode() break; case vehicle_status_s::NAVIGATION_STATE_STAB: + control_mode.flag_control_tilt_enabled = true; control_mode.flag_control_manual_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a233e511e160..62e26d4a45b2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -389,6 +389,7 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); * @value 7 Offboard * @value 8 Stabilized * @value 9 Rattitude + * @value 10 Tilt * @value 12 Follow Me * @group Commander */ @@ -413,6 +414,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE1, -1); * @value 7 Offboard * @value 8 Stabilized * @value 9 Rattitude + * @value 10 Tilt * @value 12 Follow Me * @group Commander */ @@ -437,6 +439,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE2, -1); * @value 7 Offboard * @value 8 Stabilized * @value 9 Rattitude + * @value 10 Tilt * @value 12 Follow Me * @group Commander */ @@ -461,6 +464,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, -1); * @value 7 Offboard * @value 8 Stabilized * @value 9 Rattitude + * @value 10 Tilt * @value 12 Follow Me * @group Commander */ @@ -485,6 +489,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE4, -1); * @value 7 Offboard * @value 8 Stabilized * @value 9 Rattitude + * @value 10 Tilt * @value 12 Follow Me * @group Commander */ @@ -509,6 +514,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1); * @value 7 Offboard * @value 8 Stabilized * @value 9 Rattitude + * @value 10 Tilt * @value 12 Follow Me * @group Commander */ diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 1372e070abff..0965a0f27726 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -182,6 +182,7 @@ class FixedwingAttitudeControl final : public ModuleBase) _param_fw_psp_off, (ParamFloat) _param_fw_ratt_th, + (ParamFloat) _param_fw_tilt_th, (ParamFloat) _param_fw_r_rmax, (ParamFloat) _param_fw_r_tc, diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 9646d63d9aab..b7abc0c13f5e 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -662,6 +662,20 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); */ PARAM_DEFINE_FLOAT(FW_RATT_TH, 0.8f); +/** + * Threshold for Tilt mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_TILT_TH, 0.9f); + /** * Roll trim increment at minimum airspeed * 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 2e93eaaa5cbe..9b9eabb46fa6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -44,7 +44,6 @@ */ #include "mc_att_control.hpp" - #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 6ac8fd1479b4..af971606e37e 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -164,6 +164,20 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f); */ PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f); +/** + * Threshold for Tilt mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TILT_TH, 0.9f); + /** * Manual tilt input filter time constant * Setting this parameter to 0 disables the filter diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 7cc127109b13..cd7faf91e65d 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1726,6 +1726,34 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +/** + * Tilt 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_TILT_SW, 0); + /** * AUX1 Passthrough RC channel * @@ -2227,4 +2255,20 @@ PARAM_DEFINE_FLOAT(RC_STAB_TH, 0.5f); * @max 1 * @group Radio Switches */ + PARAM_DEFINE_FLOAT(RC_TILT_TH, 0.75f); + +/** + * Threshold for selecting Tilt mode + * + * 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, public ModuleParams, public px4::W (ParamInt) _param_rc_map_gear_sw, (ParamInt) _param_rc_map_stab_sw, (ParamInt) _param_rc_map_man_sw, + (ParamInt) _param_rc_map_tilt_sw, (ParamInt) _param_rc_map_aux1, (ParamInt) _param_rc_map_aux2, @@ -219,6 +220,7 @@ class RCUpdate : public ModuleBase, public ModuleParams, public px4::W (ParamFloat) _param_rc_stab_th, (ParamFloat) _param_rc_man_th, (ParamFloat) _param_rc_return_th, + (ParamFloat) _param_rc_tilt_th, (ParamInt) _param_rc_chan_cnt )