Mavlink waypoint handler is too complex

[mdouglas90]

As per one bullet point in Issue #187, the Mavlink_waypoint_handler class needs to be rewritten. Over time, it has grown, became complex and redundant, and tries to do too much. Some examples of things that need to be fixed are:

• Remove redundant variables (e.g. waypoint_next_ and next_waypoint).
• Merge the two redundant waypoint structures (waypoint_struct_t and waypoint_local_struct_t).
• Creation of a waypoint class.
• Remove redundant waypoint_struct_t.current variable. This is a variable that is 1 for the current waypoint and 0 for all other waypoints. It is broadcast to the ground control station, but is also sort of contained in the current_waypoint_index_ variable.
• Make the update of the waypoints more streamlined. Currently, when the waypoints change, all redundant variables need to be updated together, the various flags need to be set appropriately, the new current waypoint needs to recalculate its redundant structure, etc. This makes it very easy to forget something when modifying the code and should all be done automatically.
• The waypoint handler should not be trying to do mission planning, it should only deal with the creation, deletion, reception, transmission, and retrieval of waypoints. Currently, the waypoint handler contains functions for the planning of takeoff, landing, navigation, hold position, stop there, stop on position, manual control, and on ground, as well as the critical state cases. Some of these seem redundant and should probably be combined (e.g. hold position and stop on position). They should also be moved to a Mission_planner_handler class, which can be called if needed by a Mission_planner class.
• The waypoint handler has been managing the dubin state. This should be done separately and automatically, probably within the navigation class.
• The function mode_change returns true if the control mode has not been changed and false otherwise.