It would make sense to make a local copy of the enu position ```struct EnuCoor_f pos_enu = *stateGetPositionEnu_f();``` and use that in the rest of the function instead of getting each coordinate from the state interface each time. That would be more efficient and also sure to be consistent (future-proof where it could happen that a new position is set between the calls).