Mellinger controller is adding yaw for Z-distance set points

[krichardsson]

How to reproduce:

1. Enable the mellinger controller
2. Send set Z-distane points to the CF with yaw-rate=0. The set points are decoded in https://github.com/bitcraze/crazyflie-firmware/blob/2021.03/src/modules/src/crtp_commander_generic.c#L126-L148

Observed: The CF is yawing Expected: No yaw change

Other information:

• The expected behaviour is observed with the PID controller.
• Tested with lighthouse system

Script to reproduce:

import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.utils import uri_helper

uri = uri_helper.uri_from_env(default='radio://0/30/2M/E7E7E7E7E7')
cflib.crtp.init_drivers()

with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
    crazyf = scf.cf

    # PID controller
    # crazyf.param.set_value('stabilizer.controller', '1')

    # Mellinger controller
    crazyf.param.set_value('stabilizer.controller', '2')

    height = 0.3
    yawrate = 0

    # Take off and fly
    for i in range(40):
        crazyf.commander.send_zdistance_setpoint(0, 0, yawrate, height)
        time.sleep(0.1)

    # land
    for i in range(5):
        crazyf.commander.send_zdistance_setpoint(0, 0, 0, 0.0)
        time.sleep(0.1)

[krichardsson]

It has been reported that there might be a similar problem for the roll as well in the Mellinger controller

[jpreiss]

As far as I know, the Mellinger controller was only written to work with position/fullstate and attitude-thrust commands. It needs a reworking of the setpoint mode parsing before it can handle all setpoint types.

[knmcguire]

This should be part of the overall setpoint configurability / framework discussion