Fix for rocket start 2

Add hist_pos_z init.
After some tests, there is no difference between calling or not
set_altitude(baro) function at motor arming... weird!