Quat decompress part of cflib? about crazyflie-lib-python HOT 4 CLOSED

I usually have a version of the decompress in my cfusdlog decoder (since I only use these with the usd card).

# decompress a quaternion, see quatcompress.h in firmware
# input: 32-bit number, output q = [x,y,z,w]
def quatdecompress(comp):
  q = np.zeros(4)
  mask = (1 << 9) - 1
  i_largest = comp >> 30
  sum_squares = 0
  for i in range(3, -1, -1):
    if i != i_largest:
      mag = comp & mask
      negbit = (comp >> 9) & 0x1
      comp = comp >> 10
      q[i] = mag / mask / np.sqrt(2)
      if negbit == 1:
        q[i] = -q[i]
      sum_squares += q[i] * q[i]
  q[i_largest] = np.sqrt(1.0 - sum_squares)
  return q

I think using FP16, or any other generic compression technique would be much nicer than having the *Z log topics hanging around. However, one would need to double check what the precision impact of that would be.

from crazyflie-lib-python.

Ah thanks! Yeah it would be good to figure it out indeed. the StateEstimateZ logs are not core log variables so we can tranfer to FP16 should be sufficient. Let's investigate

from crazyflie-lib-python.

Hmm... I'm afraid that we probably would need to implement a quadcompress/decompress in the cfilb because of this issue #372. The compressed quat is part of the full state setpoint sent and if we want to implement that into cflib we need those functions too

from crazyflie-lib-python.

this was fixed in #382

from crazyflie-lib-python.