Thanks for the advice,
I wish I had more time to add it to the firmware but I don't at the moment. I have begun restructuring my project with the anticipation of keeping my own reference and applying transformations to convert everything into the lcoal coordinate system. Maybe in the future I would consider contributing to the firmware.
One thing I continuously found confusing is that dronekit supports this form of travel with APM but PX4 does not,therefore I initially thought the modes I was attempting to achieve were possible under the PX4 flight stack. It was finally revealed to me when I looked into how dronekit is used with the Pixhawk.
I think before I write into the PX4 Firmware I would rather write support documentation and a complete wiki, to avoid other people making the same mistakes I have made.
But again, time is of the essence.
Thanks everybody for clarifying