44 Commits

Author SHA1 Message Date
Beat Küng
989d9243b7 fix control_allocator: correct PRINT_MODULE_USAGE_NAME 2023-04-13 07:46:09 +02:00
Silvan Fuhrer
1e56d9c219 Rework flaps/spoilers logic
- remove deprecated actuator_controls[INDEX_FLAPS/SPOILERS/AIRBRAKES]
- use new topic normalized_unsigned_setpoint.msg (with instances flaps_setpoint
and spoilers_setpoint) to pass into control allocation
- remove flaps/spoiler related fields from attitude_setpoint topic
- CA: add possibility to map flaps/spoilers to any control surface
- move flaps/spoiler pitch trimming to CA (previously called DTRIM_FLAPS/SPOILER)
- move manual flaps/spoiler handling from rate to attitude controller

FW Position controller: change how negative switch readings are intepreted
for flaps/spoilers (considered negative as 0).

VTOL: Rework spoiler publishing in hover

- pushlish spoiler_setpoint.msg in the VTOL module if in hover
- also set spoilers to land configuration if in Descend mode

Allocation: add slew rate limit of 0.5 to flaps/spoilers configuration change

Instead of doing the flaps/spoilers slew rate limiting in the FW Position Controller
(which then only is applied in Auto flight), do it consistently over all flight
modes, so also for manual modes.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2023-03-06 22:43:41 +01:00
Silvan Fuhrer
83e906e2e9 Control_allocator_status.msg: remove allocated_ fields
It's enough that the setpoints and the unallocated values are logged, from these
 the allocated values can be calculated if required.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-02 14:56:13 +01:00
Silvan Fuhrer
f44713e8a3 ControlAllocator: enable custom saturation logic to override default one
Custom saturation logic currently implemented for Tiltrotor VTOL and Helicopter.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-11-02 14:56:13 +01:00
Beat Küng
d948fcd72b fix control_allocator: check if 2. thrust and torque sp topic is valid
Otherwise the 2. setpoint is equal to the first one. This is only an issue
if the 2. instance isn't advertised.
2022-09-21 15:11:11 +02:00
Beat Küng
4087c27e84
control_allocator: handle saturation flags for helicopters 2022-09-07 08:00:27 +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
Beat Küng
0074894637 control_allocator: disable backup schedule with lockstep
The backup schedule triggers updates too early and leads to simulator errors:
ERROR [simulator] poll timeout 0, 22
2022-05-31 07:17:30 +02:00
Alessandro Simovic
20ccfbb719 control_allocator: remove failed motor from effectiveness
- limit to handling only 1 motor failure
- Log which motor failures are handled
- Remove motor from effectiveness matrix without
  recomputing the scale / normalization
2022-05-12 07:58:56 +02:00
Beat Küng
5cdb6fbd8e control_allocator: add helicopter mixer
Same logic as the existing mixer.
Untested.
2022-05-10 08:57:39 +02:00
Daniel Agar
6135bb384b cleanup module callback registration failed errors 2022-03-23 20:44:50 -04:00
Daniel Agar
f4c3084c26 init adjustments to ensure used topics are advertised early (primarily for logging)
- multi-EKF create each instance as soon as IMU & mag are advertised (before device id populated)
2022-03-23 16:51:16 -04:00
Charles Cross
1e32398217 Adds scheduling call to ControlAllocator initialization 2022-02-25 08:16:08 +01:00
Charles Cross
aecfbef128 Enables 20Hz backup scheduling for ControlAllocator 2022-02-25 08:16:08 +01:00
Charles Cross
51c055832f Changes ControlAllocator to always publish actuator controls and status 2022-02-25 08:16:08 +01:00
Beat Küng
960003a86a control_allocator: fix weak authority matrix check & update
- use max(fabsf(val)) instead of max(val)
- use correct actuator count
2022-02-21 11:17:00 -05:00
Silvan Fuhrer
c51dc3b4b7 ControlAllocator: Set all the elements of a row to 0 if that row has weak authority
Weak authority on a axis is currently defined as: none of the actuators have an
effectivness on this particular axis larger than 0.05.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-02-17 13:21:50 +01:00
Silvan Fuhrer
df0e402c44 CA: refactor logic for matrix updating
-pass flag EffectivenessUpdateReason into effectiveness, indicating if there was an external
update or not. Reasons for external updates are:
  -config changes (parameter)
  -motor failure detected or certain redundant motors are switched off to save energy

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-02-14 09:23:10 +01:00
Silvan Fuhrer
0950bb81ab ControlAllocation: update normalization scale only if matrix updated is forced
The forced flag is used to distinguish between updates due to a configuration
(parameter) change (only enabled when disarmed), and matrix updates due to motor
tilt change. Only update the normalization scale if the forced flag is true, and
use a tilt angle of vertical position for it to have the scales tilt-invariant.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2022-01-19 08:22:12 +01:00
Beat Küng
d202bf9631 control_allocator: add scope to reduce stack usage by 256B 2021-12-24 20:06:13 -05:00
Beat Küng
5259877b1b control_allocator: update matrix normalization
- only normalize rpy for MC matrices
- for thrust use the 3D vector, so it works for FW and tilt rotors as well

This keeps MC unchanged.
2021-12-24 20:06:13 -05:00
Beat Küng
8c782b7cd9 control_allocator: add Custom + MC with tilts effectiveness
The param group is changed to Geometry, as this reflects the naming in the
QGC UI.
2021-12-24 20:06:13 -05:00
Silvan Fuhrer
07306c4be3 control_allocator: add support for Tailsitter VTOL
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2021-12-24 20:06:13 -05:00
Beat Küng
4f8e6a9548 control_allocator: do not update params while armed
As a precaution
2021-12-24 20:06:13 -05:00
Beat Küng
e04b4a8dbf control_allocator: add 6DOF configuration 2021-12-24 20:06:13 -05:00
Beat Küng
4c80adfaf1 control_allocator: implement trim + slew rate limits configuration 2021-12-24 20:06:13 -05:00
Beat Küng
8d9e2a28c4 control_allocator: add fixed-wing actuator effectiveness 2021-12-24 20:06:13 -05:00
Beat Küng
4d2a403afa control_allocator: add rover actuator effectiveness
This can be generalized later
2021-12-24 20:06:13 -05:00
Beat Küng
ed3a115c8f control_allocator: add status publication rate limitation comment 2021-12-24 20:06:13 -05:00
Beat Küng
70e46a194f control_allocator: major refactoring & additions
- allow effectiveness matrix to select control allocator method
  (desaturation algorithm)
- add actuator_servos publication
- add support for multiple matrices (for vtol)
- add updateSetpoint callback method to actuator effectiveness to allow it
  to manipulate the actuator setpoint after allocation
- handle motor stopping & reversal
- add control surfaces & tilt servos
- handle standard vtol + tiltrotor
- rename MC rotors params & class to be more generically usable
- fixes and enables ActuatorEffectivenessRotorsTest
2021-12-24 20:06:13 -05:00
Beat Küng
9c767ff1ee control_allocator: make effective source name a class member 2021-12-24 20:06:13 -05:00
Beat Küng
2809dd8944 control_allocator: remove min/max/scale params 2021-11-23 12:40:22 -05:00
Beat Küng
b29d9db7f1 control_allocator: limit status publication rate to 200Hz
Reduces CPU load by ~3.5% on F4 @2khz.
And compute getAllocatedControl as needed (~1.5% CPU reduction)
2021-11-23 12:40:22 -05:00
Beat Küng
bcba3dfe52 control_allocator,angular_velocity_controller: run on rate_ctrl wq 2021-11-23 12:40:22 -05:00
Beat Küng
a0e43bca96 msg: remove unused vehicle_actuator_setpoint topic 2021-11-23 12:40:22 -05:00
Beat Küng
38fa65a47e control_allocator: remove direct mixer, add actuator_{motors,servos} instead 2021-10-18 18:45:19 -04:00
bresch
f09b34007e CA: reload CA matrix to newly created CA class properly
Also remove the dependency of ActuatorEffectivenessMultirotor
to param update uORB topic; the CA module
sends a "force" parameter when needed
2021-09-11 15:07:49 -04:00
bresch
cf5917d188 control_allocator: restructure param update
recompute effectiveness on param change
clear update at the end
2021-09-11 15:07:49 -04:00
Beat Küng
164511a7b5 control_allocator: avoid vector copies and sqrt() 2021-01-18 11:25:37 -05:00
Beat Küng
0e66b0876b control_allocator: change SequentialDesaturation to existing MC mixer
And limit the operations to the number of configured outputs.

Only using the number of configured actuators reduces CPU load by ~2% on
F7 @1khz.
2021-01-18 11:25:37 -05:00
Beat Küng
308f614735 refactor control_allocator: directly get the effectiveness matrix when updated
Reduces stack + RAM usage
2021-01-18 11:25:37 -05:00
Beat Küng
38782029ad fix control_allocator: set _last_run at the correct place 2021-01-18 11:25:37 -05:00
Beat Küng
0e86ab47f6 fix control_allocator: use 'delete' instead of 'free', guard against out-of-memory 2021-01-18 11:25:37 -05:00
Julien Lecoeur
343cf5603e initial control allocation support
- control allocation module with multirotor, VTOL standard, and tiltrotor support
 - angular_velocity_controller
 - See https://github.com/PX4/PX4-Autopilot/pull/13351 for details

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Roman Bapst <bapstroman@gmail.com>
2021-01-18 11:25:37 -05:00