I’ve added the distance + direction (DD) PIDs: the targets come from the integrated velocity flight plan, the inputs come from the acceleration integrated twice and fused with LEDDAR height for the Z axis. The outputs feed the existing velocity PIDs’ targets with a max speed of 0.5m/s set for safety reasons during the initial testing.
First few test flights were a super-sky-soarer (3m off the ground in less than a second before I could react to kill!); a couple of careless bugs fixed (and a rebuilt chassis – falling from 3m leads to a harder impact than the lower PCB can cope with even with the super shock-resistant Tarot legs installed).
It’s still a β version with more tuning – I suspect removing the PID I-gain from the velocity PIDs. However, in principle, it’s working, and so I’ve updated GitHub. By default the DD processing is commented out – look around line number 2319. Simply uncomment them to convert the velocity PID targets from the flight plan to the DD PID output.