Navigator move get_time_inside and cleanup (#7062)

src/modules/navigator/mission.cpp

@@ -44,28 +44,20 @@
  * @author Sander Smeets <sander@droneslab.com>
  */
 
-#include <sys/types.h>
-#include <string.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <float.h>
+#include "mission.h"
+#include "navigator.h"
 
 #include <drivers/drv_hrt.h>
-
 #include <dataman/dataman.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <navigator/navigation.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
-#include "mission.h"
-#include "navigator.h"
-
 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
@@ -75,28 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_yawmode(this, "MIS_YAWMODE", false),
 	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
-	_onboard_mission{},
-	_offboard_mission{},
-	_current_onboard_mission_index(-1),
-	_current_offboard_mission_index(-1),
-	_need_takeoff(true),
-	_mission_type(MISSION_TYPE_NONE),
-	_inited(false),
-	_home_inited(false),
-	_need_mission_reset(false),
-	_missionFeasibilityChecker(),
-	_min_current_sp_distance_xy(FLT_MAX),
-	_distance_current_previous(0.0f),
-	_work_item_type(WORK_ITEM_TYPE_DEFAULT)
+	_missionFeasibilityChecker()
 {
-	/* load initial params */
 	updateParams();
 }
 
-Mission::~Mission()
-{
-}
-
 void
 Mission::on_inactive()
 {
@@ -136,7 +111,7 @@ Mission::on_inactive()
 	} else {
 
 		/* load missions from storage */
-		mission_s mission_state;
+		mission_s mission_state = {};
 
 		dm_lock(DM_KEY_MISSION_STATE);
 
@@ -221,6 +196,7 @@ Mission::on_active()
 		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
+
 		altitude_sp_foh_update();
 
 	} else {
@@ -230,7 +206,6 @@ Mission::on_active()
 		}
 	}
 
-
 	/* check if a cruise speed change has been commanded */
 	if (_mission_type != MISSION_TYPE_NONE) {
 		cruising_speed_sp_update();
@@ -251,10 +226,10 @@ Mission::on_active()
 
 		do_abort_landing();
 	}
-
 }
 
-bool Mission::set_current_offboard_mission_index(unsigned index)
+bool
+Mission::set_current_offboard_mission_index(unsigned index)
 {
 	if (index < _offboard_mission.count) {
 
@@ -275,7 +250,7 @@ bool Mission::set_current_offboard_mission_index(unsigned index)
 	return false;
 }
 
-unsigned
+int
 Mission::find_offboard_land_start()
 {
 	/* find the first MAV_CMD_DO_LAND_START and return the index
@@ -433,11 +408,11 @@ Mission::advance_mission()
 float
 Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
 {
-	if (_mission_item.altitude_is_relative) {
-		return _mission_item.altitude + _navigator->get_home_position()->alt;
+	if (mission_item.altitude_is_relative) {
+		return mission_item.altitude + _navigator->get_home_position()->alt;
 
 	} else {
-		return _mission_item.altitude;
+		return mission_item.altitude;
 	}
 }
 
@@ -544,6 +519,7 @@ Mission::set_mission_items()
 		if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
 		    && !_navigator->get_vstatus()->is_rotary_wing
 		    && _navigator->get_vstatus()->is_vtol) {
+
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
 
@@ -825,7 +801,7 @@ Mission::set_mission_items()
 		set_current_offboard_mission_item();
 	}
 
-	if (_mission_item.autocontinue && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) {
+	if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
 		/* try to process next mission item */
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
@@ -994,7 +970,7 @@ Mission::heading_sp_update()
 	}
 
 	/* set yaw angle for the waypoint if a loiter time has been specified */
-	if (_waypoint_position_reached && Navigator::get_time_inside(_mission_item) > FLT_EPSILON) {
+	if (_waypoint_position_reached && get_time_inside(_mission_item) > FLT_EPSILON) {
 		// XXX: should actually be param4 from mission item
 		// at the moment it will just keep the heading it has
 		//_mission_item.yaw = _on_arrival_yaw;
@@ -1054,7 +1030,6 @@ Mission::heading_sp_update()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
-
 void
 Mission::altitude_sp_foh_update()
 {
@@ -1313,7 +1288,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 void
 Mission::save_offboard_mission_state()
 {
-	mission_s mission_state;
+	mission_s mission_state = {};
 
 	/* lock MISSION_STATE item */
 	dm_lock(DM_KEY_MISSION_STATE);