diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index 265c09f4b4..f85d7e77e6 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/docs/en/flight_modes_fw/takeoff.md @@ -85,6 +85,7 @@ The vehicle always respects normal FW max/min throttle settings during takeoff ( In _catapult/hand-launch mode_ the vehicle waits to detect launch (based on acceleration trigger). On launch it enables the motor(s) and climbs with the maximum climb rate [FW_T_CLMB_MAX](#FW_T_CLMB_MAX) while keeping the pitch setpoint above [FW_TKO_PITCH_MIN](#FW_TKO_PITCH_MIN). Once it reaches [MIS_TAKEOFF_ALT](#MIS_TAKEOFF_ALT) it will automatically switch to [Hold mode](../flight_modes_fw/hold.md) and loiter. +It is possible to delay the activation of the motors and control surfaces separately, see parameters [FW_LAUN_MOT_DEL](#FW_LAUN_MOT_DEL), [FW_LAUN_CS_LK_DY](#FW_LAUN_CS_LK_DY) and [CA_CS_LAUN_LK](#CA_CS_LAUN_LK). The later is also exposed in the actuator configuration page under the advanced view. All RC stick movement is ignored during the full takeoff sequence. @@ -104,7 +105,9 @@ The _launch detector_ is affected by the following parameters: | [FW_LAUN_DETCN_ON](../advanced_config/parameter_reference.md#FW_LAUN_DETCN_ON) | Enable automatic launch detection. If disabled motors spin up on arming already | | [FW_LAUN_AC_THLD](../advanced_config/parameter_reference.md#FW_LAUN_AC_THLD) | Acceleration threshold (acceleration in body-forward direction must be above this value) | | [FW_LAUN_AC_T](../advanced_config/parameter_reference.md#FW_LAUN_AC_T) | Trigger time (acceleration must be above threshold for this amount of seconds) | -| [FW_LAUN_MOT_DEL](../advanced_config/parameter_reference.md#FW_LAUN_MOT_DEL) | Delay from launch detection to motor spin up | +| [FW_LAUN_MOT_DEL](../advanced_config/parameter_reference.md#FW_LAUN_MOT_DEL) | Delay from launch detection to motor spin up +| [FW_LAUN_CS_LK_DY](../advanced_config/parameter_reference.md#FW_LAUN_CS_LK_DY) | Delay from launch detection to unlocking the control surfaces +| [CA_CS_LAUN_LK](../advanced_config/parameter_reference.md#CA_CS_LAUN_LK) | Bitmask to select which control surfaces are to be locked during launch | ## Runway Takeoff {#runway_launch} diff --git a/msg/LaunchDetectionStatus.msg b/msg/LaunchDetectionStatus.msg index 6917f4bc24..5693e5a822 100644 --- a/msg/LaunchDetectionStatus.msg +++ b/msg/LaunchDetectionStatus.msg @@ -7,3 +7,5 @@ uint8 STATE_LAUNCH_DETECTED_DISABLED_MOTOR = 1 # launch detected, but keep moto uint8 STATE_FLYING = 2 # launch detected, use normal takeoff/flying configuration uint8 launch_detection_state + +bool selected_control_surface_disarmed # [-] flag indicating whether selected actuators should kept disarmed (have to be configured in control allocation) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 8a033770ce..1a781035a4 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -63,6 +63,7 @@ ActuatorEffectivenessControlSurfaces::ActuatorEffectivenessControlSurfaces(Modul _spoilers_setpoint_with_slewrate.setSlewRate(kSpoilersSlewRate); _count_handle = param_find("CA_SV_CS_COUNT"); + _param_handle_ca_cs_laun_lk = param_find("CA_CS_LAUN_LK"); updateParams(); } @@ -112,6 +113,8 @@ void ActuatorEffectivenessControlSurfaces::updateParams() } } + param_get(_param_handle_ca_cs_laun_lk, &_param_ca_cs_laun_lk); + for (int i = 0; i < _count; i++) { param_get(_param_handles[i].type, (int32_t *)&_params[i].type); @@ -234,3 +237,14 @@ void ActuatorEffectivenessControlSurfaces::applySpoilers(float spoilers_control, actuator_sp(i + first_actuator_idx) += _spoilers_setpoint_with_slewrate.getState() * _params[i].scale_spoiler; } } + +void ActuatorEffectivenessControlSurfaces::applyLaunchLock(int first_actuator_idx, + ActuatorVector &actuator_sp) +{ + for (int i = 0; i < _count; ++i) { + + if (_param_ca_cs_laun_lk & (1u << i)) { + actuator_sp(i + first_actuator_idx) = NAN; + } + } +} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 8a77a08080..7da030fc18 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -90,6 +90,7 @@ public: void applyFlaps(float flaps_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp); void applySpoilers(float spoilers_control, int first_actuator_idx, float dt, ActuatorVector &actuator_sp); + void applyLaunchLock(int first_actuator_idx, ActuatorVector &actuator_sp); private: void updateParams() override; @@ -104,9 +105,11 @@ private: }; ParamHandles _param_handles[MAX_COUNT]; param_t _count_handle; + param_t _param_handle_ca_cs_laun_lk; Params _params[MAX_COUNT] {}; int32_t _count{0}; + int32_t _param_ca_cs_laun_lk{0}; SlewRate _flaps_setpoint_with_slewrate; SlewRate _spoilers_setpoint_with_slewrate; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index e91783cf80..9a3c48e560 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -65,6 +65,18 @@ void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector +#include class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffectiveness { @@ -60,6 +61,7 @@ private: uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; int _first_control_surface_idx{0}; ///< applies to matrix 1 diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 1790484347..93e8c6ddc4 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -356,6 +356,22 @@ parameters: instance_start: 0 default: 0 + CA_CS_LAUN_LK: + description: + short: Control surface launch lock enabled + long: If actuator launch lock is enabled, this surface is kept at the disarmed value. + type: bitmask + bit: + 0: Control Surface 1 + 1: Control Surface 2 + 2: Control Surface 3 + 3: Control Surface 4 + 4: Control Surface 5 + 5: Control Surface 6 + 6: Control Surface 7 + 7: Control Surface 8 + default: 0 + # Tilts CA_SV_TL_COUNT: description: @@ -659,7 +675,7 @@ mixer: rules: - select_identifier: 'servo-type' # restrict torque based on servo type - apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler'] + apply_identifiers: ['servo-torque-roll', 'servo-torque-pitch', 'servo-torque-yaw', 'servo-scale-flap', 'servo-scale-spoiler', 'servo-launch-lock'] items: # Convention: horizontal surfaces: up=positive, vertical: right=positive, mixed: up=positive. # By default the scale is set to 1/N, where N is the amount of actuators with an effect on @@ -858,6 +874,12 @@ mixer: label: 'Spoiler Scale' advanced: true identifier: 'servo-scale-spoiler' + - name: 'CA_CS_LAUN_LK' + label: 'Launch Lock' + advanced: true + identifier: 'servo-launch-lock' + index_offset: 0 + show_as: 'bitset' 2: # Standard VTOL title: 'Standard VTOL' diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 1f752608ca..710fffb4f8 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -1207,6 +1207,7 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c _takeoff_init_position = global_position; _takeoff_ground_alt = _current_altitude; _launch_current_yaw = _yaw; + _time_launch_detected = now; } const Vector2f launch_local_position = _global_local_proj_ref.project(_takeoff_init_position(0), @@ -1282,6 +1283,9 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c launch_detection_status_s launch_detection_status; launch_detection_status.timestamp = now; launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected(); + launch_detection_status.selected_control_surface_disarmed = + hrt_elapsed_time(&_time_launch_detected) < _param_fw_laun_cs_lk_dy.get() * 1_s + || _time_launch_detected == 0; _launch_detection_status_pub.publish(launch_detection_status); } @@ -1377,6 +1381,7 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { _launch_detected = true; _takeoff_ground_alt = _current_altitude; + _time_launch_detected = now; } /* Launch has been detected, hence we have to control the plane. */ @@ -1408,6 +1413,9 @@ FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const launch_detection_status_s launch_detection_status; launch_detection_status.timestamp = now; launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected(); + launch_detection_status.selected_control_surface_disarmed = + hrt_elapsed_time(&_time_launch_detected) < _param_fw_laun_cs_lk_dy.get() * 1_s + || _time_launch_detected == 0; _launch_detection_status_pub.publish(launch_detection_status); } @@ -2312,6 +2320,8 @@ FixedWingModeManager::reset_takeoff_state() _launch_detected = false; + _time_launch_detected = 0; + _takeoff_ground_alt = _current_altitude; } diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index e026f8ce26..2a43968ffb 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -298,6 +298,9 @@ private: // true if a launch, specifically using the launch detector, has been detected bool _launch_detected{false}; + // [us] time stamp of (runway/catapult) launch detection + hrt_abstime _time_launch_detected{0}; + // [deg] global position of the vehicle at the time launch is detected (using launch detector) or takeoff is started (runway) Vector2d _takeoff_init_position{0, 0}; @@ -865,6 +868,10 @@ private: (ParamFloat) _param_nav_gpsf_r, (ParamFloat) _param_t_spdweight, + // Launch detection parameters + (ParamBool) _param_fw_laun_detcn_on, + (ParamFloat) _param_fw_laun_cs_lk_dy, + // external parameters (ParamBool) _param_fw_use_airspd, (ParamFloat) _param_nav_loiter_rad, @@ -881,7 +888,6 @@ private: (ParamInt) _param_fw_lnd_abort, (ParamFloat) _param_fw_tko_airspd, (ParamFloat) _param_rwto_psp, - (ParamBool) _param_fw_laun_detcn_on, (ParamFloat) _param_fw_airspd_max, (ParamFloat) _param_fw_airspd_min, (ParamFloat) _param_fw_airspd_trim, diff --git a/src/modules/fw_mode_manager/fw_mode_manager_params.c b/src/modules/fw_mode_manager/fw_mode_manager_params.c index 3a2edb6052..0d3cdf292c 100644 --- a/src/modules/fw_mode_manager/fw_mode_manager_params.c +++ b/src/modules/fw_mode_manager/fw_mode_manager_params.c @@ -580,6 +580,21 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); */ PARAM_DEFINE_INT32(FW_LAUN_DETCN_ON, 0); +/** + * Control surface launch delay + * + * Locks control surfaces during pre-launch (armed) and until this time since launch has passed. + * Only affects control surfaces that have corresponding flag set, and not active for runway takeoff. + * Set to 0 to disable any surface locking after arming. + * + * @unit s + * @min 0.0 + * @decimal 1 + * @increment 0.1 + * @group FW Auto Takeoff + */ +PARAM_DEFINE_FLOAT(FW_LAUN_CS_LK_DY, 0.f); + /** * Flaps setting during take-off * diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 993d68bdd0..69631979a5 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -287,8 +287,18 @@ void FixedwingRateControl::Run() _rate_control.resetIntegral(); } + launch_detection_status_s launch_detection_status{}; + _launch_detection_status_sub.copy(&launch_detection_status); + + bool control_surfaces_locked = false; + + if (hrt_elapsed_time(&launch_detection_status.timestamp) < 100_ms + && launch_detection_status.selected_control_surface_disarmed) { + control_surfaces_locked = true; + } + // Reset integrators if the aircraft is on ground or not in a state where the fw attitude controller is run - if (_landed || !_in_fw_or_transition_wo_tailsitter_transition) { + if (_landed || !_in_fw_or_transition_wo_tailsitter_transition || control_surfaces_locked) { _gain_compression.reset(); _rate_control.resetIntegral(); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 54cf23be00..1bdab1c135 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -70,6 +70,7 @@ #include #include #include +#include using matrix::Eulerf; using matrix::Quatf; @@ -110,6 +111,7 @@ private: uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; uORB::SubscriptionMultiArray _control_allocator_status_subs{ORB_ID::control_allocator_status};