multirotor_pos_control: setpoint reset rewritten

Should fix takeoff setpoint when no mission set. Minor systemlib/pid fix.
Also fixes https://github.com/PX4/Firmware/issues/385

Need heavy testing before merging.