logdata() function uncommented
Altitude controller implementation