Implemented altitude and velocity hold mode
fixed somereview comments
Merge pull request #1476 from friekopter/vel_alt_hold

Implemented altitude and velocity hold mode