VTOL Control Allocation Bug

[danielhonies]

Describe the bug

A standard VTOL with two pusher motors will not yaw in MC mode if VT_FWD_THRUST_EN is disabled. Due to VT_FWD_THRUST_EN being always disabled in stabilized the vehicle will not yaw at all in that mode.

I suspect this is due to CA trying to use the pushers with differential thrust but the VTOL code disables them in MC mode on a higher level if the parameter is not set. I reproduced this both in sim and real world.

To Reproduce

Steps to reproduce the behavior: Equip standard vtol with two pushers and set VT_FWD_THRUST_EN to disabled.

Expected behavior

CA needs to take notice of disabled pushers and use normal yaw control through the MC motors.

[MaEtUgR]

@sfuhrer Dev call: Two pushers, used in pusher assist for yaw but getting disabled in multicopter flight 👀

[danielhonies]

Current hack to circumvent this: in CA configuration put both pusher motors on Y-axis 0. This will stop CA trying to use them for yaw control and use the upwards motors again.

[dagar]

@sfuhrer @RomanBapst

[danielhonies]

<?xml version="1.0"?>

0 0 0.246 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 5 0.477708333333 0 0 0.341666666667 0 0.811041666667 0 0 -0.07 0 0 0 0.55 2.144 0.05 100000 1.0 0.1 0.001 0.53 -1.072 -0.1 1.5707963268 0 3.1415926536 0.001 0.001 0.001 model://standard_vtol/meshes/x8_wing.dae 0 0.35 0.01 0 0 0 0.74 0.03 0.03 0 -0.35 0.01 0 0 0 0.74 0.03 0.03 -0.35 0.35 0.045 0 0 0 0.035 0.02 -0.35 -0.35 0.045 0 0 0 0.035 0.02 0.35 -0.35 0.045 0 0 0 0.035 0.02 0.35 0.35 0.045 0 0 0 0.035 0.02 1 0 model://imu 0 0 0 0 0 0 imu imu::link base_link 1 0 0 0 0 0 0 0 0 1 model://airspeed 0 0 0 0 0 0 airspeed airspeed::link base_link model://magnetometer 0 0 0 0 0 0 mag mag::link base_link 0.35 -0.35 0.07 0 0 0 0 0 0 0 0 0 0.005 9.75e-07 0 0 0.000166704 0 0.000167604 0 0 0 0 0 0 0.005 0.1 0 0 0 0 0 0 1 1 1 model://standard_vtol/meshes/iris_prop_ccw.dae 1 0 rotor_0 base_link 0 0 1 -1e+16 1e+16 0 0 1 -0.35 0.35 0.07 0 0 0 0 0 0 0 0 0 0.005 9.75e-07 0 0 0.000166704 0 0.000167604 0 0 0 0 0 0 0.005 0.1 0 0 0 0 0 0 1 1 1 model://standard_vtol/meshes/iris_prop_ccw.dae 1 0 rotor_1 base_link 0 0 1 -1e+16 1e+16 0 0 1 0.35 0.35 0.07 0 0 0 0 0 0 0 0 0 0.005 9.75e-07 0 0 0.000166704 0 0.000167604 0 0 0 0 0 0 0.005 0.1 0 0 0 0 0 0 1 1 1 model://standard_vtol/meshes/iris_prop_ccw.dae 1 0 rotor_2 base_link 0 0 1 -1e+16 1e+16 0 0 1 -0.35 -0.35 0.07 0 0 0 0 0 0 0 0 0 0.005 9.75e-07 0 0 0.000166704 0 0.000167604 0 0 0 0 0 0 0.005 0.1 0 0 0 0 0 0 1 1 1 model://standard_vtol/meshes/iris_prop_ccw.dae 1 0 rotor_3 base_link 0 0 1 -1e+16 1e+16 0 0 1 -0.22 0.25 0.0 0 1.57 0 0 0 0 0 0 0 0.005 9.75e-07 0 0 0.000166704 0 0.000167604 0.0 0 -0.04 0 0 0 0.005 0.06 0 0 -0.04 0 0 0 0.8 0.8 0.8 model://standard_vtol/meshes/iris_prop_cw.dae 1 0 rotor_puller base_link 1 0 0 -1e+16 1e+16 0 0 1 -0.22 -0.25 0.0 0 1.57 0 0 0 0 0 0 0 0.005 9.75e-07 0 0 0.000166704 0 0.000167604 0.0 0 -0.04 0 0 0 0.005 0.06 0 0 -0.04 0 0 0 0.8 0.8 0.8 model://standard_vtol/meshes/iris_prop_ccw.dae 1 0 rotor_puller2 base_link 1 0 0 -1e+16 1e+16 0 0 1 0.00000001 0.000001 0.0 0.000001 0.0 0.0 0.000001 0 0.3 0 0.00 0 0.0 -0.105 0.004 -0.034 1.5707963268 0 3.1415926536 0.001 0.001 0.001 model://standard_vtol/meshes/x8_elevon_left.dae 0.00000001 0.000001 0.0 0.000001 0.0 0.0 0.000001 0 -0.6 0 0.00 0 0.0 0.281 -1.032 -0.034 1.5707963268 0 3.1415926536 0.001 0.001 0.001 model://standard_vtol/meshes/x8_elevon_right.dae 0.00000001 0.000001 0.0 0.000001 0.0 0.0 0.000001 -0.5 0 0 0.00 0 0.0 0.00000001 0.000001 0.0 0.000001 0.0 0.0 0.000001 -0.5 0 0.05 0 0 0 base_link left_elevon -0.18 0.6 -0.005 0 0 0.265 0 1 0 -0.53 0.53 1.000 1 base_link right_elevon -0.18 -0.6 -0.005 0 0 -0.265 0 1 0 -0.53 0.53 1.000 1 base_link elevator -0.5 0 0 0 0 0 0 1 0 -0.53 0.53 1.000 1 base_link rudder -0.5 0 0.05 0.00 0 0.0 1 0 0 -0.01 0.01 1.000 1 model://gps 0 0 0 0 0 0 gps gps::link base_link 0.05984281113 4.752798721 0.6417112299 0.0 0.3391428111 -3.85 -0.9233984055 0 -0.05 0.3 0.05 0.50 1.2041 1 0 0 0 0 1 base_link left_elevon_joint -1.0 world_wind 0.05984281113 4.752798721 0.6417112299 0.0 0.3391428111 -3.85 -0.9233984055 0 -0.05 -0.3 0.05 0.50 1.2041 1 0 0 0 0 1 base_link right_elevon_joint -1.0 world_wind -0.2 4.752798721 0.6417112299 0.0 0.3391428111 -3.85 -0.9233984055 0 -0.5 0 0 0.01 1.2041 1 0 0 0 0 1 base_link elevator_joint -12.0 world_wind 0.0 4.752798721 0.6417112299 0.0. 0.3391428111 -3.85 -0.9233984055 0 -0.5 0 0.05 0.02 1.2041 1 0 0 0 1 0 base_link world_wind rotor_0_joint rotor_0 ccw 0.0125 0.025 1500 2e-05 0.06 /gazebo/command/motor_speed 0 0.000106428 1e-06 /motor_speed/0 20 rotor_1_joint rotor_1 ccw 0.0125 0.025 1500 2e-05 0.06 /gazebo/command/motor_speed 1 0.000106428 1e-06 /motor_speed/1 20 rotor_2_joint rotor_2 cw 0.0125 0.025 1500 2e-05 0.06 /gazebo/command/motor_speed 2 0.000106428 1e-06 /motor_speed/2 20 rotor_3_joint rotor_3 cw 0.0125 0.025 1500 2e-05 0.06 /gazebo/command/motor_speed 3 0.000106428 1e-06 /motor_speed/3 20 rotor_puller_joint rotor_puller cw 0.0125 0.025 3500 8.54858e-06 0.01 /gazebo/command/motor_speed 4 0.000106428 1e-06 /motor_speed/4 20 rotor_puller2_joint rotor_puller2 ccw 0.0125 0.025 3500 8.54858e-06 0.01 /gazebo/command/motor_speed 5 0.000106428 1e-06 /motor_speed/5 20 50 /baro /imu /baro INADDR_ANY {{ mavlink_tcp_port }} {{ mavlink_udp_port }} {{ serial_enabled }} {{ serial_device }} {{ serial_baudrate }} INADDR_ANY 14550 INADDR_ANY 14540 {{ hil_mode }} false true true /gazebo/command/motor_speed 0 0 1500 0 0 velocity rotor_0_joint 1 0 1500 0 0 velocity rotor_1_joint 2 0 1500 0 0 velocity rotor_2_joint 3 0 1500 0 0 velocity rotor_3_joint 4 0 5500 0 0 velocity rotor_puller_joint 5 0 5500 0 0 velocity rotor_puller2_joint 6 0 1 0 0 position_kinematic left_elevon_joint 7 0 1 0 0 position_kinematic right_elevon_joint 8 0 1 0 0 position_kinematic elevator_joint 0

[danielhonies]

paste this into standard_vtol.sdf.jinja to observe the behaviour in gazebo and change the following params: param set-default CA_ROTOR4_KM -0.0500 param set-default CA_ROTOR4_PX -0.2000 param set-default CA_ROTOR4_PY -0.2500 param set-default CA_ROTOR5_AX 1.0000 param set-default CA_ROTOR5_AZ 0.0000 param set-default CA_ROTOR5_PX -0.2000 param set-default CA_ROTOR5_PY 0.2500 param set-default CA_ROTOR_COUNT 6 param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 201 param set-default PWM_MAIN_FUNC8 202 param set-default PWM_MAIN_FUNC9 203 param set-default VT_PITCH_MIN 2.0000

[sfuhrer]

Thanks for the detailed report! Currently it will use all configured motors for attitude control in hover, so also the pusher for yaw if it's configured that way. Setting the Y Position to 0 was the correct move, and it doesn't have side effects, it's not needed anywhere. Pusher/Puller motors currently cannot be used for any attitude control, and we should probably disable it in the control allocation. Or at lest remove the Position fields if the direction is Forwards on a Standard VTOL.

[danielhonies]

What if we only add the pusher motors to the CA matrix if VT_FWD_THRUST_EN is enabled (and it's conditions met, also in FW flight) and otherwise remove them? I'm pretty sure it would work as expected then and the pushers could be used for yaw (which could be beneficial for bigger vehicles with less control authority through the hover motors).

[sfuhrer]

Can we make a step back - you want to use the horizontal motors for yaw control in hover mode, but with VT_FWD_THRUST_EN enabled? But then in ever mode (pusher assist is only active in Altitude and higher modes)? There really should be some refactoring on how we do the whole puller/pusher allocation for VTOLs. The allocation should only receive 3d thrust and 3d torque, plus maybe a flag for enabling/disabling the horizontal motors, and with that info do the whole allocation (currently parts of which are done in the VTOL attitude controller). For your specific feature request: I think easiest for you is to pass a flag "horizontal motors enabled" to the CA, and in there then update the matrix accordingly.