Hi Angel,
this looks indeed very very promising!
Looking forward to the Teensy implementation.
Would also be another good test to have the LIDAR facing downwards for terrain-following test (controlling altitude, i.e. 'running down' a stairway).
Maybe you should add some prop shrouds to the craft, preventing injury to persons.
I would not really touch AQ code to add this. I would prefer to send data via MavLink to the AQ and keep the sensor tasks on a separate MCU. Also keep in mind: we don't have I2C master code for the AQ!
Joerg