Refactor failsafe2

[tongtybj]

branch: https://github.com/tongtybj/aerial_robot/tree/no_failsafe2

situation: the desired pitch and roll angles are calculated based on the desired thrust for hydrus.
If the desired thrust is insane, the pitch and roll might be very large (the case of mbzirc2020 task1 rehearsal in 2020.02.20, the leddarone went wrong, making the height estimation and control also went wrong).

TODO: set failsafe detection in control part: https://github.com/tongtybj/aerial_robot/blob/no_failsafe2/aerial_robot_base/src/control/agile_flatness_pid_controller.cpp#L213-L214