From bb12fce66cafd931dda819fc7c7b3ba2bbe5e3d7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 3 Mar 2021 19:27:56 -0500 Subject: [PATCH] delete RATTITUDE flight mode --- .vscode/settings.json | 1 - msg/commander_state.msg | 30 +++++----- msg/manual_control_switches.msg | 1 - msg/rc_channels.msg | 55 +++++++++---------- msg/vehicle_control_mode.msg | 1 - msg/vehicle_status.msg | 2 +- src/drivers/osd/atxxxx/atxxxx.cpp | 4 -- src/drivers/rc_input/crsf_telemetry.cpp | 4 -- .../frsky_telemetry/frsky_telemetry.cpp | 2 - src/modules/commander/Commander.cpp | 33 +---------- src/modules/commander/commander_params.c | 8 +-- .../failure_detector/FailureDetector.cpp | 6 +- src/modules/commander/px4_custom_mode.h | 2 +- .../commander/state_machine_helper.cpp | 6 -- .../FixedwingAttitudeControl.cpp | 8 --- .../FixedwingAttitudeControl.hpp | 2 - .../fw_att_control/fw_att_control_params.c | 14 ----- src/modules/mavlink/mavlink_messages.cpp | 5 -- src/modules/mc_att_control/mc_att_control.hpp | 2 - .../mc_att_control/mc_att_control_main.cpp | 13 +---- .../mc_att_control/mc_att_control_params.c | 14 ----- .../MulticopterRateControl.cpp | 8 --- .../MulticopterRateControl.hpp | 2 - src/modules/rc_update/params.c | 44 --------------- src/modules/rc_update/rc_update.cpp | 3 - src/modules/rc_update/rc_update.h | 2 - 26 files changed, 50 insertions(+), 222 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5ded130f74..f8ce7d827b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -27,7 +27,6 @@ "cSpell.showStatus": false, "cSpell.words": [ "acro", - "rattitude", "nuttx", "esc" ], diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 38c94931f1..8e7c54fe93 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -1,21 +1,21 @@ # Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. uint64 timestamp # time since system start (microseconds) -uint8 MAIN_STATE_MANUAL = 0 -uint8 MAIN_STATE_ALTCTL = 1 -uint8 MAIN_STATE_POSCTL = 2 -uint8 MAIN_STATE_AUTO_MISSION = 3 -uint8 MAIN_STATE_AUTO_LOITER = 4 -uint8 MAIN_STATE_AUTO_RTL = 5 -uint8 MAIN_STATE_ACRO = 6 -uint8 MAIN_STATE_OFFBOARD = 7 -uint8 MAIN_STATE_STAB = 8 -uint8 MAIN_STATE_RATTITUDE = 9 -uint8 MAIN_STATE_AUTO_TAKEOFF = 10 -uint8 MAIN_STATE_AUTO_LAND = 11 +uint8 MAIN_STATE_MANUAL = 0 +uint8 MAIN_STATE_ALTCTL = 1 +uint8 MAIN_STATE_POSCTL = 2 +uint8 MAIN_STATE_AUTO_MISSION = 3 +uint8 MAIN_STATE_AUTO_LOITER = 4 +uint8 MAIN_STATE_AUTO_RTL = 5 +uint8 MAIN_STATE_ACRO = 6 +uint8 MAIN_STATE_OFFBOARD = 7 +uint8 MAIN_STATE_STAB = 8 +# LEGACY RATTITUDE = 9 +uint8 MAIN_STATE_AUTO_TAKEOFF = 10 +uint8 MAIN_STATE_AUTO_LAND = 11 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_AUTO_PRECLAND = 13 +uint8 MAIN_STATE_ORBIT = 14 +uint8 MAIN_STATE_MAX = 15 uint8 main_state # main state machine diff --git a/msg/manual_control_switches.msg b/msg/manual_control_switches.msg index fb68c1d44d..444120e595 100644 --- a/msg/manual_control_switches.msg +++ b/msg/manual_control_switches.msg @@ -30,7 +30,6 @@ uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGH uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO -uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 18d8d97f58..024b5d5deb 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,37 +1,36 @@ uint64 timestamp # time since system start (microseconds) -uint8 FUNCTION_THROTTLE=0 -uint8 FUNCTION_ROLL=1 -uint8 FUNCTION_PITCH=2 -uint8 FUNCTION_YAW=3 -uint8 FUNCTION_MODE=4 -uint8 FUNCTION_RETURN=5 -uint8 FUNCTION_POSCTL=6 -uint8 FUNCTION_LOITER=7 -uint8 FUNCTION_OFFBOARD=8 -uint8 FUNCTION_ACRO=9 -uint8 FUNCTION_FLAPS=10 -uint8 FUNCTION_AUX_1=11 -uint8 FUNCTION_AUX_2=12 -uint8 FUNCTION_AUX_3=13 -uint8 FUNCTION_AUX_4=14 -uint8 FUNCTION_AUX_5=15 -uint8 FUNCTION_PARAM_1=16 -uint8 FUNCTION_PARAM_2=17 -uint8 FUNCTION_PARAM_3_5=18 -uint8 FUNCTION_RATTITUDE=19 -uint8 FUNCTION_KILLSWITCH=20 -uint8 FUNCTION_TRANSITION=21 -uint8 FUNCTION_GEAR=22 -uint8 FUNCTION_ARMSWITCH=23 -uint8 FUNCTION_STAB=24 -uint8 FUNCTION_AUX_6=25 -uint8 FUNCTION_MAN=26 +uint8 FUNCTION_THROTTLE = 0 +uint8 FUNCTION_ROLL = 1 +uint8 FUNCTION_PITCH = 2 +uint8 FUNCTION_YAW = 3 +uint8 FUNCTION_MODE = 4 +uint8 FUNCTION_RETURN = 5 +uint8 FUNCTION_POSCTL = 6 +uint8 FUNCTION_LOITER = 7 +uint8 FUNCTION_OFFBOARD = 8 +uint8 FUNCTION_ACRO = 9 +uint8 FUNCTION_FLAPS = 10 +uint8 FUNCTION_AUX_1 = 11 +uint8 FUNCTION_AUX_2 = 12 +uint8 FUNCTION_AUX_3 = 13 +uint8 FUNCTION_AUX_4 = 14 +uint8 FUNCTION_AUX_5 = 15 +uint8 FUNCTION_PARAM_1 = 16 +uint8 FUNCTION_PARAM_2 = 17 +uint8 FUNCTION_PARAM_3_5 = 18 +uint8 FUNCTION_KILLSWITCH = 19 +uint8 FUNCTION_TRANSITION = 20 +uint8 FUNCTION_GEAR = 21 +uint8 FUNCTION_ARMSWITCH = 22 +uint8 FUNCTION_STAB = 23 +uint8 FUNCTION_AUX_6 = 24 +uint8 FUNCTION_MAN = 25 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[26] 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/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 4ca702a946..9592a694ca 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -8,7 +8,6 @@ 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_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in -bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled bool flag_control_acceleration_enabled # true if acceleration is controlled bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled bool flag_control_position_enabled # true if position is controlled diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index b841549637..fae2a01543 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -37,7 +37,7 @@ uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode -uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode +uint8 NAVIGATION_STATE_UNUSED2 = 16 # Free slot uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index 6709dc9334..8b40635b97 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -424,10 +424,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state) case vehicle_status_s::NAVIGATION_STATE_STAB: flight_mode = "STABILIZED"; break; - - case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - flight_mode = "RATTITUDE"; - break; } return flight_mode; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 243e711c8c..37a5092cd3 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -182,10 +182,6 @@ bool CRSFTelemetry::send_flight_mode() case vehicle_status_s::NAVIGATION_STATE_STAB: flight_mode = "Stabilized"; break; - - case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - flight_mode = "Rattitude"; - break; } return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode); diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp index 504a5bd020..1924b3529f 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp +++ b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp @@ -115,8 +115,6 @@ uint16_t get_telemetry_flight_mode(int px4_flight_mode) case 15: return 20; // stabilized - case 16: return 21; // rattitude - case 17: return 25; // takeoff case 8: diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8affad21d5..8b7a7f5bd6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -321,9 +321,6 @@ int Commander::custom_command(int argc, char *argv[]) } else if (!strcmp(argv[1], "stabilized")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED); - } else if (!strcmp(argv[1], "rattitude")) { - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_RATTITUDE); - } else if (!strcmp(argv[1], "auto:takeoff")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF); @@ -664,10 +661,6 @@ Commander::handle_command(const vehicle_command_s &cmd) /* ACRO */ main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) { - /* RATTITUDE */ - main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { /* STABILIZED */ main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state); @@ -2790,7 +2783,6 @@ Commander::set_main_state_rc() || (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch) || (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch) || (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch) - || (_last_manual_control_switches.rattitude_switch != _manual_control_switches.rattitude_switch) || (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch) || (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch) || (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot) @@ -3044,16 +3036,6 @@ Commander::set_main_state_rc() res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state); } - } else if (_manual_control_switches.rattitude_switch == manual_control_switches_s::SWITCH_POS_ON) { - /* Similar to acro transitions for multirotors. FW aircraft don't need a - * rattitude mode.*/ - if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state); - - } else { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state); - } - } else { res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state); } @@ -3069,9 +3051,6 @@ Commander::set_main_state_rc() } else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) { res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state); - } else if (_manual_control_switches.rattitude_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state); - } else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) { res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state); @@ -3251,13 +3230,6 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_attitude_enabled = true; break; - case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - _vehicle_control_mode.flag_control_manual_enabled = true; - _vehicle_control_mode.flag_control_rates_enabled = true; - _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_rattitude_enabled = true; - break; - case vehicle_status_s::NAVIGATION_STATE_ALTCTL: _vehicle_control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true; @@ -3360,7 +3332,6 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_auto_enabled = false; _vehicle_control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_rattitude_enabled = false; _vehicle_control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true; _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; @@ -4059,7 +4030,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|rattitude|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("lockdown"); PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 91c2d3ec0e..9e6ad34e9e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -405,7 +405,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); * @value 6 Acro * @value 7 Offboard * @value 8 Stabilized - * @value 9 Rattitude * @value 12 Follow Me * @group Commander */ @@ -429,7 +428,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE1, -1); * @value 6 Acro * @value 7 Offboard * @value 8 Stabilized - * @value 9 Rattitude * @value 12 Follow Me * @group Commander */ @@ -453,7 +451,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE2, -1); * @value 6 Acro * @value 7 Offboard * @value 8 Stabilized - * @value 9 Rattitude * @value 12 Follow Me * @group Commander */ @@ -477,7 +474,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, -1); * @value 6 Acro * @value 7 Offboard * @value 8 Stabilized - * @value 9 Rattitude * @value 12 Follow Me * @group Commander */ @@ -501,7 +497,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE4, -1); * @value 6 Acro * @value 7 Offboard * @value 8 Stabilized - * @value 9 Rattitude * @value 12 Follow Me * @group Commander */ @@ -525,7 +520,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1); * @value 6 Acro * @value 7 Offboard * @value 8 Stabilized - * @value 9 Rattitude * @value 12 Follow Me * @group Commander */ @@ -966,7 +960,7 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1); * * A non-zero, positive value specifies the timeframe in seconds within failure detector is allowed to put the vehicle into * a lockdown state if attitude exceeds the limits defined in FD_FAIL_P and FD_FAIL_R. - * The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW), Rattitude and Manual (FW). + * The check is not executed for flight modes that do support acrobatic maneuvers, e.g: Acro (MC/FW) and Manual (FW). * A zero or negative value means that the check is disabled. * * @group Commander diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 9b8fa5a49e..4983414d42 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -76,13 +76,11 @@ bool FailureDetector::isAttitudeStabilized(const vehicle_status_s &vehicle_statu const uint8_t nav_state = vehicle_status.nav_state; if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO && - nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO; } else if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { attitude_is_stabilized = nav_state != vehicle_status_s::NAVIGATION_STATE_MANUAL && - nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO && - nav_state != vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO; } return attitude_is_stabilized; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index cd4c5637ad..75a7bb0b9a 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -50,7 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_STABILIZED, - PX4_CUSTOM_MAIN_MODE_RATTITUDE, + PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY, PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */ }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fb078991a3..595426b5f0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -270,7 +270,6 @@ 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_RATTITUDE: ret = TRANSITION_CHANGED; break; @@ -431,7 +430,6 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ switch (internal_state->main_state) { case commander_state_s::MAIN_STATE_ACRO: case commander_state_s::MAIN_STATE_MANUAL: - case commander_state_s::MAIN_STATE_RATTITUDE: case commander_state_s::MAIN_STATE_STAB: case commander_state_s::MAIN_STATE_ALTCTL: @@ -451,10 +449,6 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; - case commander_state_s::MAIN_STATE_RATTITUDE: - status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; - break; - case commander_state_s::MAIN_STATE_STAB: status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; break; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 66183d8c1d..028bb3b3b5 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -143,14 +143,6 @@ FixedwingAttitudeControl::vehicle_manual_poll() // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { - // Check if we are in rattitude mode and the pilot is above the threshold on pitch - if (_vcontrol_mode.flag_control_rattitude_enabled) { - if (fabsf(_manual_control_setpoint.y) > _param_fw_ratt_th.get() - || fabsf(_manual_control_setpoint.x) > _param_fw_ratt_th.get()) { - _vcontrol_mode.flag_control_attitude_enabled = false; - } - } - if (!_vcontrol_mode.flag_control_climb_rate_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 35b9341a07..21fe69cd0b 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -184,8 +184,6 @@ private: (ParamFloat) _param_fw_pr_p, (ParamFloat) _param_fw_psp_off, - (ParamFloat) _param_fw_ratt_th, - (ParamFloat) _param_fw_r_rmax, (ParamFloat) _param_fw_r_tc, (ParamFloat) _param_fw_rll_to_yaw_ff, 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 927a7e9c03..4bc2ad6bf8 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -632,20 +632,6 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); */ PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45); -/** - * Threshold for Rattitude 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_RATT_TH, 0.8f); - /** * Roll trim increment at minimum airspeed * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2a9fb2b012..adc194cdf1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -279,11 +279,6 @@ void get_mavlink_navigation_mode(const struct vehicle_status_s *const status, ui custom_mode->main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; - case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode->main_mode = PX4_CUSTOM_MAIN_MODE_RATTITUDE; - break; - case vehicle_status_s::NAVIGATION_STATE_STAB: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 9386a4e12d..c8cc197aaa 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -148,8 +148,6 @@ private: (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _param_mc_ratt_th, - /* Stabilized mode params */ (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ 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 ada0136430..3acf95dfc8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -287,15 +287,6 @@ MulticopterAttitudeControl::Run() } } - /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't - * even bother running the attitude controllers */ - if (_v_control_mode.flag_control_rattitude_enabled) { - _v_control_mode.flag_control_attitude_enabled = - fabsf(_manual_control_setpoint.y) <= _param_mc_ratt_th.get() && - fabsf(_manual_control_setpoint.x) <= _param_mc_ratt_th.get(); - } - bool attitude_setpoint_generated = false; const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode); @@ -338,9 +329,7 @@ MulticopterAttitudeControl::Run() // reset yaw setpoint during transitions, tailsitter.cpp generates // attitude setpoint for the transition - _reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || - _landed || (_vtol && _vtol_in_transition_mode); - + _reset_yaw_sp = !attitude_setpoint_generated || _landed || (_vtol && _vtol_in_transition_mode); } perf_end(_loop_perf); 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 b63c3ad878..7dc1ff9a81 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -146,20 +146,6 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f); -/** - * Threshold for Rattitude 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_RATT_TH, 0.8f); - /** * Manual tilt input filter time constant * Setting this parameter to 0 disables the filter diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 6edea9cbcd..94e0c56209 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -172,14 +172,6 @@ MulticopterRateControl::Run() if (!_v_control_mode.flag_control_attitude_enabled) { manual_rate_sp = true; } - - // Check if we are in rattitude mode and the pilot is within the center threshold on pitch and roll - // if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro) - if (_v_control_mode.flag_control_rattitude_enabled) { - manual_rate_sp = - (fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) || - (fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get()); - } } if (manual_rate_sp) { diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 6816848013..1e64e9c9a3 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -160,8 +160,6 @@ private: (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ - (ParamFloat) _param_mc_ratt_th, - (ParamBool) _param_mc_bat_scale_en, (ParamInt) _param_cbrk_rate_ctrl diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index ee550a65a8..ed14dea538 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1391,34 +1391,6 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -/** - * Rattitude 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_RATT_SW, 0); - /** * Position Control switch channel * @@ -2044,22 +2016,6 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); */ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); -/** - * Threshold for selecting rattitude 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) _param_rc_map_flaps, (ParamInt) _param_rc_map_return_sw, - (ParamInt) _param_rc_map_ratt_sw, (ParamInt) _param_rc_map_posctl_sw, (ParamInt) _param_rc_map_loiter_sw, (ParamInt) _param_rc_map_acro_sw, @@ -229,7 +228,6 @@ private: (ParamFloat) _param_rc_assist_th, (ParamFloat) _param_rc_auto_th, - (ParamFloat) _param_rc_ratt_th, (ParamFloat) _param_rc_posctl_th, (ParamFloat) _param_rc_loiter_th, (ParamFloat) _param_rc_acro_th,