So I've decided with the change of the DCM matrix and PI controller for navigation to change fundamentally the code layout with the use of data structures to make more sense of the information been passed on. I also spend most of list night trying to get my head around whether float division is really such a problem. We'll based on references xxx and xxx it is. Float memory allocation is not a real issue but float division can really boggle down your code to the core of tortoise pace. So this reference xxx explains that one can find an alternative around this issue which should speed things up quite a bit.The fact of the matter is that float division should really be an exception in any type of code (irrespective of the compiler and host). A lesson well learnt.