Commit Graph

41572 Commits

Author SHA1 Message Date
Silvan Fuhrer a787a326e3 Fixed-wing: split out control of steering wheel into seperate message LandingGearWheel
Completely detach the steering wheel logic from the yaw controller (beside using the
same manual stick input in a manual flight mode).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-22 13:46:25 -05:00
Silvan Fuhrer 6c611a7e8b VehicleAttitudeSetpoint: rename fw_control_yaw to fw_control_yaw_wheel to make usage clearer
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-22 13:46:25 -05:00
Thomas Stastny 4b036e6723 fixed-wing landing: dont allow land abort while flaring 2022-11-22 13:46:25 -05:00
Thomas Stastny a8c2eaf3e0 fixed-wing landing: add a touchdown time and subsequent ramp down to rwto pitch setpoint after the flare to keep wheels on ground 2022-11-22 13:46:25 -05:00
Thomas Stastny 1de1416773 fixed-wing landing: convert flare time pitch and throttle constraint ramps to sqrt function to make more aggressive 2022-11-22 13:46:25 -05:00
Thomas Stastny f23328d14f fixed-wing landing: ramp in flaring throttle setpoints from last throttle state to keep control continuity
- also put flaring internal states into a struct to organize a bit
- one concern with blending the throttle setpoint like this with the flare time param is that folding prop belly landing airframes may want to have a separate param for shorter throttle kill and still use the flare time ramps for everything else
2022-11-22 13:46:25 -05:00
Thomas Stastny 68e1921f27 make FW_LND_FL_TIME min positive non-zero to protect dividing by zero 2022-11-22 13:46:25 -05:00
Thomas Stastny 928be2958d fixed-wing landing: continue to follow path throughout flare
abruptly changing to a heading setpoint on flare can cause the aircraft to roll and deviate from the runway, this commit
- maintains path following control during the flare not to disrupt the tracking just before touchdown
- (unfortunately for crosswind landing) removes the body axis alignment for runway bearing - this is a compromise

to achieve both runway bearing body axis alignment AND a specific touchdown point, either
1. the wind would need to be considered, and an appropriate diagonal approach (obstructions allowing)  defined to the runway
2. slip control added, keeping path following outputs only commanding roll (controlling airspeed vector) and using yaw-rate command (only actuated by e.g. rudder) to align body axis with the runway
2022-11-22 13:46:25 -05:00
Thomas Stastny e5a9a57d79 fixed-wing landing: ramp in throttle constraints during flare 2022-11-22 13:46:25 -05:00
Thomas Stastny 4fbfc42805 fixed-wing runway takeoff: ramp in pitch constraints and throttle setpoint on takeoff rotation
- consolidate takeoff rotation transition times for pitch constraints and throttle setpoint with a single param
- consolidate pitch takeoff constraint parameters (remove rwto_max_pitch, use nominal max)
- input correct units to rwto pitch constraint getters
- encapsulate absolute time interpolator method for transitions
- start runway ops from idle throttle
2022-11-22 13:46:25 -05:00
Thomas Stastny a4349193b5 fixed-wing runway takeoff: climbout at specified takeoff airspeed and max climb rate
TECS climbout mode was used for takeoff climbout, which puts throttle to full and does not regulate a specific airspeed.
This commit sets the desired takeoff airspeed explicitly and allows max climb rate to track the ascent.
2022-11-22 13:46:25 -05:00
Thomas Stastny 47963b5b67 fixed-wing runway takeoff: define explicit takeoff speeds
previously a scale factor param on min airspeed was used to define the climbout airspeed for runway takeoff
additionally, the rotation speed was defined by another hardcoded scale on top of the previously scaled min airspeed
this commit explicitly defines a takeoff speed and rotation speed for runway takeoff in params, with option to disable
2022-11-22 13:46:25 -05:00
Beat Küng 46dbb7cf63 fix pwm_out, px4io: prevent disarm and rate param updates during boot
Before, the logic to update disarm and rate values also triggered during
bootup on the px4io, because the output functions are only set in
updateSubscriptions().
Therefore change the check to prevent updating during the first cycle.
2022-11-22 13:41:16 -05:00
Beat Küng a20c581111 mixer_module: remove unused limit_callbacks_to_primary argument 2022-11-22 13:41:16 -05:00
dsix-ls2n 2833832968
simulation/gz_bridge: ignition with no lockstep (#20561)
When using ignition SITL simulation with NO_LOCKSTEP, the SITL PX4 fails to update the IMU data from Ignition Gazebo.

The timestamp for the IMU data is taken from the ignition message:

 - In LOCKSTEP mode the clock from the ignition simulation and the one from PX4 SITL are synchronized, hence everything works fine
 - In NO_LOCKSTEP mode, those clocks are not synchronized anymore, so the timestamp for the IMU data should not be the one from Ignition but the current time in PX4 SITL when receiving the message.
2022-11-22 11:26:11 -05:00
Matthias Grob 55b454a8a5 MulticopterPositionControl: avoid invalid setpoint message when switching to altitude controlled mode 2022-11-22 16:03:41 +01:00
bresch 60c448ce3a ekf2: GNSS yaw, use reported yaw accuracy when available 2022-11-22 09:59:31 -05:00
bresch b25bc1b982 ekf2: GNSS yaw, update->fuse pattern 2022-11-22 09:59:31 -05:00
bresch 4cb327fba3 ekf2: remove old GPS yaw autogenerated code 2022-11-22 09:59:31 -05:00
bresch 16a3fee54f ekf2_test: check behavior of GNSS yaw fusion at singularities 2022-11-22 09:59:31 -05:00
bresch 4116de31ad ekf2: compare GNSS yaw fusion sympy vs symforce 2022-11-22 09:59:31 -05:00
bresch eff2c6adcf ekf2: migrate GNSS yaw fusion to SymForce 2022-11-22 09:59:31 -05:00
Michael Schaeuble 19bca47b9e Preserve gimbal control when receiving updates from an input that is not active
The update function of InputMavlinkGimbalV2 returns UpdatedNotActive when receiving control commands from a system or component that doesn't match the primary control. This can happen if a component just sends commands without being in control or when transitioning to a different primary control.
If the input is marked as already_active, it will reset last_input_active which in turn resets the primary control in the next iteration. According to the MAVLink gimbal protocol v2, a component needs to send  MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE to start controlling the gimbal and to remove control. However, with the current implementation there are several other commands that would reset the primary control.

With this PR, the primary control remains with the component that requested it last if updates from a not active component are received.
2022-11-22 12:08:47 +01:00
JaeyoungLim 2586900c26
Log local position setpoint reference for fixedwings when running NPFG (#20512)
* Log position setpoint reference of npfg

This commit logs the local position setpoint reference when using NPFG

* Address review comments

This commit address review comments from @tstastny
2022-11-22 08:19:38 +01:00
Silvan Fuhrer c24f9561e9 FW Att C: use new param FW_MAN_YR_MAX to control yawing with sticks in attitude controlled mode
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-21 21:26:20 -05:00
Silvan Fuhrer c186f798b6 FW Att C: do not directly override actuator_controls[YAW] but only change yaw rate sp
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-21 21:26:20 -05:00
Daniel Agar 08cdb96f77
boards: px4_fmu-v5_uavcanv0periph disable systemcmds to save flash 2022-11-21 21:23:02 -05:00
Daniel Agar 16564af788
Jenkinsfile-hardware: remove px4_fmu-v3 2022-11-21 21:18:01 -05:00
Silvan Fuhrer 44f831c41a Navigator: Mission feasibilty check: set _has_landing also for normal landing waypoints (without pattern)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-21 18:03:21 +01:00
Silvan Fuhrer e68ba81d6d MissionFeasibilityChecker: for MC only find if landing is present, but don't do any validation
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-21 18:03:21 +01:00
Silvan Fuhrer f2f094c33d MissionFeasibilityChecker: add @briefs
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-21 18:03:21 +01:00
Silvan Fuhrer 4cd7dfa162 MissionFeasibility: add combined takeoff/landing requirement check
Replace the existing check for the availability of a takeoff mission item with a combined
check for takeoff and landing item (or landing pattern). New param MIS_TKO_LAND_REQ
can be set to require only a takeoff, only a landing, both takeoff and landing, and
both or none. The latter is meant to be set if is e.g. deemed unsafe to start a flight
through a Takeoff WP without though defining a Landing - as then in case of a RTL the
vehicle doesn't follow a pre-defined path but instead only can do default RTL that especially
for FW and VTOL isn't always safe.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-21 18:03:21 +01:00
Alex Mikhalev cce3b43b4f mavlink: Fix setting MAV_SIK_RADIO_ID
The AT commands were formatted incorrectly
2022-11-21 08:12:40 +01:00
Daniel Agar 2cb4ef0629
NuttX 10.3+ upgrade (#20190)
Co-authored-by: Peter van der Perk <peter.vanderperk@nxp.com>
Co-authored-by: David Sidrane <David.Sidrane@NscDg.com>
Co-authored-by: alexklimaj <alex@arkelectron.com>
2022-11-20 20:28:07 -05:00
Beat Küng 45b390b0bf microdds: use UXR_DURABILITY_VOLATILE for data reader
This corresponds to the ROS2 default. Using reader=TRANSIENT_LOCAL and
writer=VOLATILE (=default) is incompatible according to
https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html
2022-11-18 21:41:24 -05:00
Silvan Fuhrer b36758b1a0 Update src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
fix typo

Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>
2022-11-18 17:35:34 +01:00
Silvan Fuhrer c4c94febfa FW Pos C: clearly define FW_AIRSPD_MIN as stall+margin, and automatically increase f(load_factor)
Previously the minimum airspeed setpoint was adjusted to the load_factor compensated
stall speed, which, when the stall speed was set without margin, gave the controller
no room for error (the vehicle would stall if the controller has even a small airspeed
error).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-18 17:35:34 +01:00
Silvan Fuhrer f2ca9387cf FW Position Control: also run airspeed adaptions based on wind, accelerated stall etc. in manual modes
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-18 17:35:34 +01:00
Silvan Fuhrer cec16dd9b3 FW Attidue Controller: use FW_AIRSPD_MIN for DTRIM instead of STALL
This is to make it again in line with the parameter description and docs

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-18 17:25:10 +01:00
alexklimaj 64768f1cda Increase allowed rtk injections to 8 for moving base. Update GPS submodule. 2022-11-18 11:04:17 -05:00
alexklimaj 8b61b22da6 Fix CANNODE_SUB_MBD typo 2022-11-18 11:04:17 -05:00
Jukka Laitinen f2607335ac rc/crsf_rc/CrsfRc.cpp: Include fcntl.h for "open"
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2022-11-18 07:10:27 +01:00
Jukka Laitinen 9081238dc5 microdds_client.cpp: Include posix.h for PX4_STACK_ADJUSTED macro
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2022-11-18 07:10:27 +01:00
Jukka Laitinen 9ce234ece8 SafetyButton.cpp: Include board_config.h
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2022-11-18 07:10:27 +01:00
Jukka Laitinen ff3a3dac01 cdc_acm_check.cpp: Add missing #includes
- Include board_config.h for BOARD_GET_EXTERNAL_LOCKOUT_STATE etc. macros
- Include fcntl.h for "open"

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2022-11-18 07:10:27 +01:00
Jukka Laitinen 966560edc0 Fix overflows in abstime_to_ts
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
2022-11-18 07:10:05 +01:00
Zachary Lowell 52b16d062c
uORB Remote Manager Update (#20623) 2022-11-17 13:51:01 -08:00
Silvan Fuhrer 8b7c074680 FW Position Control: remove 0.9 mergin factor on stall vs min param comparison
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-17 13:33:10 +01:00
Silvan Fuhrer 2e0c8da7ef FW Position Control: fix load factor calculation
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-17 13:33:10 +01:00
Silvan Fuhrer 798cc4f01c
LandDetectorMC: enforce that LNDMC_Z_VEL_MAX is larger than MPC_LAND_CRWL/MPC_LAND_SPEED (#20614)
* LandDetectorMC: enforce that LNDMC_Z_VEL_MAX * 1.2 is below *MPC_LAND_CRWL/MPC_LAND_SPEED

Otherwise the _in_descend flag doesn't get set correctly during the last part
of the landing, where the descend speed is at MPC_LAND_CRWL or LAND_SPEED.
The _in_descend flag is set it the velocity setpoint is >1.1*LNDMC_Z_VEL_MAX.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
2022-11-17 10:56:26 +01:00