Sensor startup cleanup

This PR represents a review of the startup routines and (excessive) stack allocations on the startup routine handlers. I will add testing on FMUv1 and v3.