Commit Graph

40933 Commits

Author SHA1 Message Date
Jaeyoung Lim bcdf61b65f Fix format 2022-08-19 09:40:48 +02:00
Silvan Fuhrer b87afb138c Update src/modules/fw_att_control/FixedwingAttitudeControl.cpp
reduce code duplication

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2022-08-19 09:40:48 +02:00
Jaeyoung Lim 70134296ed Fix parameter description from review comments
This commit makes the parameter description of the fixewing attitude controller more descriptive
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 70d7070307 ROMFS: plane airframe: make tuning tighter
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer c54f62e17a FW attitude control: fix ff terms and constrain outputs to (-1, 1)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer dadd4f39ad FW attitude control: fix yaw rate publishing
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 6724e4af26 FW attitude control: manual stick input sets yaw rate setpoint, not rudder directly
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 22067a1128 FW attitude control: set FF gains for rate controller to 0 as FF controls is handles outside
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 57e052d02d FW attitdue controller: use allocator status for anti-windup
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 6aab44e425 ControlAllocator: publish allocator_status from all active matrices (2 for VTOL, otherwise 1)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer a9b848cae3 FW attitue controller: fix publishing of rate controller status
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 5a2127d026 fixed-wing: update rate controller integrator handling
-always reset roll/pitch/yaw integrators at the same time
-reset them while waiting for launch or during FW Takeoff before Climbout
-reset wheel rate integrator only when disarmed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer cd080289c6 FW attitude controller: constrain rates correctly
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer 15c95a7b6a FW attitude controller: improve readability and fix euler rate sp vs. body rate sp
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Silvan Fuhrer e0a998e6ad FW attitude controller: remove unused control_input.scaler
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Jaeyoung Lim 33d068cfb2 Added feedforward terms in fw att control 2022-08-19 09:40:48 +02:00
Jaeyoung Lim 958c61dbba Populate rate control setpoints properly 2022-08-19 09:40:48 +02:00
JaeyoungLim 6f24f4cd1c Update src/modules/fw_att_control/fw_att_control_params.c
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-19 09:40:48 +02:00
Jaeyoung Lim dcff481219 Map derivative gains for rate controls 2022-08-19 09:40:48 +02:00
Jaeyoung-Lim f2877ce585 Replace rate controller with RateControlLibrary
This commit makes the fw attitude controller take share the rate controller as a library with the mc_rate_control module
2022-08-19 09:40:48 +02:00
Thomas Debrunner 44a18acd51
Fix race condition in px4io serial driver (#20005)
* px4io: prevent memory corruption on corrput io data

* px4io_serial: Prevent race between handling wait timeout case and interrupt posting semaphore
2022-08-18 17:46:47 +02:00
bresch 02c4e0361c MCPosControl: fix horizontal anti-reset windup algirithm
Since the horizontal and vertical velocity controllers are now
decoupled, it can be that the horizontal acceleration produced by the
controller is actually greater than the desired one (by design). This
condition would actually make the ARW run "backwards", degrading the
controller performance.
2022-08-18 14:19:10 +02:00
mcsauder 87a5705960 Rename math::gradual() to math::interpolate() and add unit tests to cover additional corner cases. 2022-08-18 14:18:02 +02:00
bresch 021b23826d wind_replay: allow estimated local vel and GNSS vel sources
local vel by default, set --gnss to use GNSS vel instead
2022-08-18 14:09:23 +02:00
bresch be6acb0a68 wind_est: add python script to replay wind estimator equations 2022-08-18 14:09:23 +02:00
bresch 4d21c90cbb wind_est: generate wind estimator equations for python use 2022-08-18 14:09:23 +02:00
Kalyan Sriram deb938fcea drivers/uavcan: update libuavcan 2022-08-16 11:05:51 -04:00
Daniel Agar 1c72f86761 Update src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
Co-authored-by: Kabir Mohammed  <kabir@corvus-robotics.com>
2022-08-16 08:40:43 -04:00
Daniel Agar 49d87f1907 adis16470: fix accel and gyro scaling 2022-08-16 08:40:43 -04:00
bresch 616b5689ba Python: move symforce to optional requirements
symforce requires python 3.8 which isn't available by default on ubuntu
18.04.
2022-08-15 21:14:07 -04:00
Silvan Fuhrer a7a8daaf4d ROMFS: tiltrotor sitl config: some small tuning improvements in hover
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-16 02:48:44 +02:00
Silvan Fuhrer 771dbf9395 ROMFS: tiltrotor sitl config: adapt CA params to new output indexes
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-16 02:48:44 +02:00
Silvan Fuhrer 09f83016c9 update sitl_gazebo (tiltrotor index fixes)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-16 02:48:44 +02:00
Junwoo Hwang d7a962b426 mavlink: fix PX4_DEBUG message formats 2022-08-12 09:43:12 +02:00
Junwoo Hwang 0c218e6628 atl/mantis-edu: enable mavlink ftp 2022-08-12 09:43:12 +02:00
Silvan Fuhrer ffb8fb4383 Allocation: add SteeringWheel type control surface
Directly use yaw controls for it, and don't add it to the allocation matrix,
as that would have an effect on rudder scaling if the wheel also would have
a yaw effectiveness.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-12 09:43:12 +02:00
Silvan Fuhrer 9e18b351bc Allocation: add Single Channel Aileron to CS types
This is the control surface type for airframes that have only a
single aileron servo or have the ailerons on a single output channel.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-12 09:43:12 +02:00
Silvan Fuhrer c6d1b1f50f Allocation: add A-tail servo type
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-08-12 09:43:12 +02:00
Beat Küng f454dcef6b ROMFS: set control allocation parameters for airframes
Removes some airframes:
- 1000_rc_fw_easystar.hil
- 10015_tbs_discovery
- 10016_3dr_iris
- 10018_tbs_endurance
- 13001_caipirinha_vtol
- 13002_firefly6
- 13003_quad_tailsitter
- 13004_quad+_tailsitter
- 13005_vtol_AAERT_quad
- 13006_vtol_standard_delta
- 13007_vtol_AAVVT_quad
- 13008_QuadRanger
- 13009_vtol_spt_ranger
- 13012_convergence
- 13050_generic_vtol_octo
- 14001_tri_y_yaw+
- 14002_tri_y_yaw-
- 15001_coax_heli
- 2105_maja
- 2200_mini_talon
- 3031_phantom
- 3032_skywalker_x5
- 3033_wingwing
- 3036_pigeon
- 3100_tbs_caipirinha
- 4003_qavr5
- 4009_qav250
- 4019_x500_v2
- 4030_3dr_solo
- 4031_3dr_quad
- 4051_s250aq
- 4072_draco
- 4080_zmr250
- 4090_nanomind
- 4100_tiltquadrotor
- 50003_aion_robotics_r1_rover
2022-08-12 09:43:12 +02:00
Beat Küng 4d60fadc05 ROMFS: set control allocation parameters for sitl airframes
Removes some airframes:
- if750a
- solo
- iris_ctrlalloc
- typhoon_h480_ctrlalloc
2022-08-12 09:43:12 +02:00
Beat Küng f2db7b8deb control_allocator: hide motor positions for fixed-wings
should not be set for now
2022-08-12 09:43:12 +02:00
Beat Küng 923a90d78b control_allocator: set default control surface type to (not set)
Before it was set to left aileron, but the torque values were all 0.
2022-08-12 09:43:12 +02:00
Beat Küng 720cf5a485 config: enable dynamic control allocation by default (SYS_CTRL_ALLOC=1) 2022-08-12 09:43:12 +02:00
Oleg 6ea3c6a7d2 mavlink_ftp: fix to correctly trim reply messages
Clear any not used payload data to correctly trim mavlink ftp message reply to avoid sending long ACK and NACK messages.
2022-08-12 08:53:33 +02:00
David Sidrane cd971948da NuttX Backport critical F7 Ethernet issues 2022-08-12 08:22:55 +02:00
Matthias Grob 1f81101994 mc_att_control_main: do not jump throttle scaling when taking off
2fbb70d9ca made the lowest possible
throttle value commanded by stick in Stabilized mode before taking off 0.
The real world problem with this is that when takeoff is detected then
the entire throttle scaling range jumps from
[0, MPC_MANTHR_MIN]
to [MPC_MANTHR_MIN, MPC_MANTHR_MIN].
As a result whenever MPC_MANTHR_MIN is not zero there is a thrust jump
on every takeoff just at the moment takeoff is detected even when the
stick is moved continuously.

Because of this I suggest to revert to having a higher throttle value
from the beginning on because it's less complicated and there's
no obvious value in starting out with zero thrust if anyways not
possible to go back to zero for safety once takeoff is detected.
2022-08-11 14:18:54 +02:00
Matthias Grob 5b1b6f6080 mc_att_control_main: use gradual() for throttle curve rescale without hover thrust 2022-08-11 14:18:54 +02:00
Matthias Grob 7cb6a47714 TIME_ESTIMATE_TO_TARGET: fill unsupported fields with value representing unavailability 2022-08-11 07:27:04 +02:00
bresch 7996a1a70f ekf2: reset ekf preflight checks on takeoff correctly
Using the control status flags isn't robust as this part of the code
runs at the EKF update rate while the in_air transition is don at the
prediction rate. It was then likely to miss the transition
2022-08-10 16:52:28 +02:00
marcirsch a8b342722e mavlink_main: Add TIME_ESTIMATE_TO_TARGET stream to config, onboard, and onboard_low_bandwidth modes
Signed-off-by: marcirsch <marcell@auterion.com>
2022-08-10 09:13:58 +02:00