commander, navigator: minor cleanup (refactoring), code style fixed

src/modules/commander/commander.cpp

@@ -371,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
 				}
 			}
+
 			if (hil_ret == OK)
 				ret = true;
 
@@ -405,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					arming_res = TRANSITION_NOT_CHANGED;
 				}
 			}
+
 			if (arming_res == TRANSITION_CHANGED)
 				ret = true;
 
@@ -447,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					}
 				}
 			}
+
 			if (main_res == TRANSITION_CHANGED)
 				ret = true;
 
@@ -486,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 
 	case VEHICLE_CMD_OVERRIDE_GOTO: {
-		// TODO listen vehicle_command topic directly from navigator (?)
+			// TODO listen vehicle_command topic directly from navigator (?)
 			unsigned int mav_goto = cmd->param1;
+
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
 				status->set_nav_state = NAV_STATE_LOITER;
 				status->set_nav_state_timestamp = hrt_absolute_time();
@@ -508,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
-	/* Flight termination */
+		/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
 			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
@@ -900,6 +904,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
@@ -1135,6 +1140,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
 					/* recover from failsafe */
 					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+
 					if (res == TRANSITION_CHANGED) {
 						mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe");
 					}
@@ -1164,14 +1170,18 @@ int commander_thread_main(int argc, char *argv[])
 				if (status.main_state == MAIN_STATE_AUTO) {
 					/* check if AUTO mode still allowed */
 					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+
 					if (res == TRANSITION_DENIED) {
 						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
 						if (res == TRANSITION_CHANGED) {
 							mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
+
 						} else if (res == TRANSITION_DENIED) {
 							/* LAND mode denied, switch to failsafe state TERMINATION */
 							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+
 							if (res == TRANSITION_CHANGED) {
 								mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
 							}
@@ -1181,15 +1191,20 @@ int commander_thread_main(int argc, char *argv[])
 				} else if (armed.armed) {
 					/* failsafe for manual modes */
 					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+
 					if (res == TRANSITION_CHANGED) {
 						mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
+
 					} else if (res == TRANSITION_DENIED) {
 						/* RTL not allowed (no global position estimate), try LAND */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
 						if (res == TRANSITION_CHANGED) {
 							mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
+
 						} else if (res == TRANSITION_DENIED) {
 							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+
 							if (res == TRANSITION_CHANGED) {
 								mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
 							}

src/modules/commander/state_machine_helper.cpp

@@ -67,7 +67,7 @@ static bool failsafe_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-	arming_state_t new_arming_state, struct actuator_armed_s *armed)
+			arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
 	/*
 	 * Perform an atomic state update
@@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
+
 		} else {
 			armed->lockdown = false;
 		}
@@ -219,7 +220,7 @@ check_arming_state_changed()
 }
 
 transition_result_t
-main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
@@ -232,9 +233,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 	case MAIN_STATE_SEATBELT:
 
 		/* need at minimum altitude estimate */
-		if (!current_state->is_rotary_wing ||
+		if (!status->is_rotary_wing ||
-			(current_state->condition_local_altitude_valid ||
+		    (status->condition_local_altitude_valid ||
-			current_state->condition_global_position_valid)) {
+		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
 		}
 
@@ -243,8 +244,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 	case MAIN_STATE_EASY:
 
 		/* need at minimum local position estimate */
-		if (current_state->condition_local_position_valid ||
+		if (status->condition_local_position_valid ||
-			current_state->condition_global_position_valid) {
+		    status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 
@@ -253,7 +254,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 	case MAIN_STATE_AUTO:
 
 		/* need global position estimate */
-		if (current_state->condition_global_position_valid) {
+		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 
@@ -261,8 +262,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 	}
 
 	if (ret == TRANSITION_CHANGED) {
-		if (current_state->main_state != new_main_state) {
+		if (status->main_state != new_main_state) {
-			current_state->main_state = new_main_state;
+			status->main_state = new_main_state;
 			main_state_changed = true;
 
 		} else {
@@ -378,11 +379,14 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 		case FAILSAFE_STATE_NORMAL:
 			ret = TRANSITION_CHANGED;
 			break;
+
 		case FAILSAFE_STATE_RTL:
 			if (status->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
+
 			break;
+
 		case FAILSAFE_STATE_TERMINATION:
 			ret = TRANSITION_CHANGED;
 			break;

src/modules/navigator/navigator_main.cpp

@@ -171,7 +171,7 @@ private:
 	bool						_geofence_violation_warning_sent;
 
 	bool						_fence_valid;		/**< flag if fence is valid */
-        bool						_inside_fence;		/**< vehicle is inside fence */
+	bool						_inside_fence;		/**< vehicle is inside fence */
 
 	struct navigation_capabilities_s		_nav_caps;
 
@@ -490,16 +490,20 @@ void
 Navigator::offboard_mission_update(bool isrotaryWing)
 {
 	struct mission_s offboard_mission;
+
 	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current;
+
 		if (offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
+
 		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
 
 		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
@@ -516,6 +520,7 @@ void
 Navigator::onboard_mission_update()
 {
 	struct mission_s onboard_mission;
+
 	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
 		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -558,11 +563,13 @@ Navigator::task_main()
 	 * else clear geofence data in datamanager
 	 */
 	struct stat buffer;
-	if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) {
+
+	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
 		warnx("Try to load geofence.txt");
 		_geofence.loadFromFile(GEOFENCE_FILENAME);
+
 	} else {
-		if (_geofence.clearDm() > 0 )
+		if (_geofence.clearDm() > 0)
 			warnx("Geofence cleared");
 		else
 			warnx("Could not clear geofence");
@@ -649,6 +656,7 @@ Navigator::task_main()
 				if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) {
 					if (_vstatus.main_state == MAIN_STATE_AUTO) {
 						bool stick_mode = false;
+
 						if (!_vstatus.rc_signal_lost) {
 							/* RC signal available, use control switches to set mode */
 							/* RETURN switch, overrides MISSION switch */
@@ -656,21 +664,27 @@ Navigator::task_main()
 								if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
 									dispatch(EVENT_RTL_REQUESTED);
 								}
+
 								stick_mode = true;
+
 							} else {
 								/* MISSION switch */
 								if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
 									dispatch(EVENT_LOITER_REQUESTED);
 									stick_mode = true;
+
 								} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
 									/* switch to mission only if available */
 									if (_mission.current_mission_available()) {
 										dispatch(EVENT_MISSION_REQUESTED);
+
 									} else {
 										dispatch(EVENT_LOITER_REQUESTED);
 									}
+
 									stick_mode = true;
 								}
+
 								if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
 									/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
 									dispatch(EVENT_LOITER_REQUESTED);
@@ -696,15 +710,18 @@ Navigator::task_main()
 								case NAV_STATE_MISSION:
 									if (_mission.current_mission_available()) {
 										dispatch(EVENT_MISSION_REQUESTED);
+
 									} else {
 										dispatch(EVENT_LOITER_REQUESTED);
 									}
+
 									break;
 
 								case NAV_STATE_RTL:
 									if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
 										dispatch(EVENT_RTL_REQUESTED);
 									}
+
 									break;
 
 								default:
@@ -717,12 +734,14 @@ Navigator::task_main()
 								if (myState == NAV_STATE_NONE) {
 									if (_mission.current_mission_available()) {
 										dispatch(EVENT_MISSION_REQUESTED);
+
 									} else {
 										dispatch(EVENT_LOITER_REQUESTED);
 									}
 								}
 							}
 						}
+
 					} else {
 						/* not in AUTO mode */
 						dispatch(EVENT_NONE_REQUESTED);
@@ -786,6 +805,7 @@ Navigator::task_main()
 		/* global position updated */
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
+
 			/* only check if waypoint has been reached in MISSION and RTL modes */
 			if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
 				if (check_mission_item_reached()) {
@@ -794,15 +814,15 @@ Navigator::task_main()
 			}
 
 			/* Check geofence violation */
-			if(!_geofence.inside(&_global_pos)) {
+			if (!_geofence.inside(&_global_pos)) {
 				//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
 
 				/* Issue a warning about the geofence violation once */
-				if (!_geofence_violation_warning_sent)
+				if (!_geofence_violation_warning_sent) {
-				{
 					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
 					_geofence_violation_warning_sent = true;
 				}
+
 			} else {
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
@@ -855,45 +875,52 @@ void
 Navigator::status()
 {
 	warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
+
 	if (_global_pos.valid) {
 		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
 		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
-			(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
+		      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
 		warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
-			(double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
+		      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
 		warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
 	}
+
 	if (_fence_valid) {
 		warnx("Geofence is valid");
 //		warnx("Vertex longitude latitude");
 //		for (unsigned i = 0; i < _fence.count; i++)
 //		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
+
 	} else {
 		warnx("Geofence not set");
 	}
 
 	switch (myState) {
-		case NAV_STATE_NONE:
+	case NAV_STATE_NONE:
-			warnx("State: None");
+		warnx("State: None");
-			break;
+		break;
-		case NAV_STATE_LOITER:
+
-			warnx("State: Loiter");
+	case NAV_STATE_LOITER:
-			break;
+		warnx("State: Loiter");
-		case NAV_STATE_MISSION:
+		break;
-			warnx("State: Mission");
+
-			break;
+	case NAV_STATE_MISSION:
-		case NAV_STATE_RTL:
+		warnx("State: Mission");
-			warnx("State: RTL");
+		break;
-			break;
+
-		default:
+	case NAV_STATE_RTL:
-			warnx("State: Unknown");
+		warnx("State: RTL");
-			break;
+		break;
+
+	default:
+		warnx("State: Unknown");
+		break;
 	}
 }
 
 StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 	{
-							/* STATE_NONE */
+		/* STATE_NONE */
 		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE},
 		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -904,7 +931,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE},
 	},
 	{
-							/* STATE_READY */
+		/* STATE_READY */
 		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
 		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
@@ -915,7 +942,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
 	},
 	{
-							/* STATE_LOITER */
+		/* STATE_LOITER */
 		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
 		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
@@ -926,7 +953,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
 	},
 	{
-							/* STATE_MISSION */
+		/* STATE_MISSION */
 		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
 		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -937,7 +964,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
 	},
 	{
-							/* STATE_RTL */
+		/* STATE_RTL */
 		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
 		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -948,7 +975,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
 	},
 	{
-							/* STATE_LAND */
+		/* STATE_LAND */
 		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
 		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -1013,6 +1040,7 @@ Navigator::start_loiter()
 		if (_global_pos.alt < min_alt_amsl) {
 			_pos_sp_triplet.current.alt = min_alt_amsl;
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+
 		} else {
 			_pos_sp_triplet.current.alt = _global_pos.alt;
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
@@ -1065,22 +1093,22 @@ Navigator::set_mission_item()
 		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
 		if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
-			_mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
+		    _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
-			_mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
+		    _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
-			_mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
+		    _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
 			/* don't reset RTL state on RTL or LOITER items */
 			_rtl_state = RTL_STATE_NONE;
 		}
 
 		if (_vstatus.is_rotary_wing) {
 			if (_need_takeoff && (
-				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				    _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
-				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
+				    _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+				    _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
+				    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
-				)) {
+			    )) {
 				/* do special TAKEOFF handling for VTOL */
 				_need_takeoff = false;
 
@@ -1108,6 +1136,7 @@ Navigator::set_mission_item()
 					_pos_sp_triplet.current.yaw = NAN;
 					_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
 				}
+
 			} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
 				/* will need takeoff after landing */
 				_need_takeoff = true;
@@ -1116,13 +1145,16 @@ Navigator::set_mission_item()
 
 		if (_do_takeoff) {
 			mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt);
+
 		} else {
 			if (onboard) {
 				mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+
 			} else {
 				mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
 			}
 		}
+
 	} else {
 		/* since a mission is not advanced without WPs available, this is not supposed to happen */
 		_mission_item_valid = false;
@@ -1136,6 +1168,7 @@ Navigator::set_mission_item()
 
 		if (ret == OK) {
 			position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
+
 		} else {
 			/* this will fail for the last WP */
 			_pos_sp_triplet.next.valid = false;
@@ -1149,17 +1182,21 @@ void
 Navigator::start_rtl()
 {
 	_do_takeoff = false;
+
 	if (_rtl_state == RTL_STATE_NONE) {
 		if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
 			_rtl_state = RTL_STATE_CLIMB;
+
 		} else {
 			_rtl_state = RTL_STATE_RETURN;
+
 			if (_reset_loiter_pos) {
 				_mission_item.altitude_is_relative = false;
 				_mission_item.altitude = _global_pos.alt;
 			}
 		}
 	}
+
 	_reset_loiter_pos = true;
 	mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
 	set_rtl_item();
@@ -1191,118 +1228,123 @@ Navigator::set_rtl_item()
 {
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
-		float climb_alt = _home_pos.alt + _parameters.rtl_alt;
+			float climb_alt = _home_pos.alt + _parameters.rtl_alt;
-		if (_vstatus.condition_landed) {
+
-			climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
+			if (_vstatus.condition_landed) {
+				climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
+			}
+
+			_mission_item_valid = true;
+
+			_mission_item.lat = _global_pos.lat;
+			_mission_item.lon = _global_pos.lon;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.altitude = climb_alt;
+			_mission_item.yaw = NAN;
+			_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_direction = 1;
+			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.time_inside = 0.0f;
+			_mission_item.pitch_min = 0.0f;
+			_mission_item.autocontinue = true;
+			_mission_item.origin = ORIGIN_ONBOARD;
+
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt);
+			break;
 		}
 
-		_mission_item_valid = true;
-
-		_mission_item.lat = _global_pos.lat;
-		_mission_item.lon = _global_pos.lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = climb_alt;
-		_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _parameters.loiter_radius;
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-		_mission_item.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
-
-		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-		_pos_sp_triplet.next.valid = false;
-
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt);
-		break;
-	}
 	case RTL_STATE_RETURN: {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
-		_mission_item_valid = true;
+			_mission_item_valid = true;
 
-		_mission_item.lat = _home_pos.lat;
+			_mission_item.lat = _home_pos.lat;
-		_mission_item.lon = _home_pos.lon;
+			_mission_item.lon = _home_pos.lon;
-		// don't change altitude
+			// don't change altitude
-		_mission_item.yaw = NAN;	// TODO set heading to home
+			_mission_item.yaw = NAN;	// TODO set heading to home
-		_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_radius = _parameters.loiter_radius;
-		_mission_item.loiter_direction = 1;
+			_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item.time_inside = 0.0f;
+			_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
+			_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
+			_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.origin = ORIGIN_ONBOARD;
 
-		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
-		_pos_sp_triplet.next.valid = false;
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
+			break;
+		}
 
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
-		break;
-	}
 	case RTL_STATE_DESCEND: {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
-		_mission_item_valid = true;
+			_mission_item_valid = true;
 
-		_mission_item.lat = _home_pos.lat;
+			_mission_item.lat = _home_pos.lat;
-		_mission_item.lon = _home_pos.lon;
+			_mission_item.lon = _home_pos.lon;
-		_mission_item.altitude_is_relative = false;
+			_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
+			_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
-		_mission_item.yaw = NAN;
+			_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_radius = _parameters.loiter_radius;
-		_mission_item.loiter_direction = 1;
+			_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item.time_inside = 0.0f;
+			_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
+			_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
+			_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.origin = ORIGIN_ONBOARD;
 
-		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
-		_pos_sp_triplet.next.valid = false;
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude);
+			break;
+		}
 
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude);
-		break;
-	}
 	case RTL_STATE_LAND: {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
-		_mission_item_valid = true;
+			_mission_item_valid = true;
 
-		_mission_item.lat = _home_pos.lat;
+			_mission_item.lat = _home_pos.lat;
-		_mission_item.lon = _home_pos.lon;
+			_mission_item.lon = _home_pos.lon;
-		_mission_item.altitude_is_relative = false;
+			_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _home_pos.alt;
+			_mission_item.altitude = _home_pos.alt;
-		_mission_item.yaw = NAN;
+			_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _parameters.loiter_radius;
+			_mission_item.loiter_radius = _parameters.loiter_radius;
-		_mission_item.loiter_direction = 1;
+			_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_LAND;
+			_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item.time_inside = 0.0f;
+			_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
+			_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
+			_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.origin = ORIGIN_ONBOARD;
 
-		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
-		_pos_sp_triplet.next.valid = false;
+			_pos_sp_triplet.next.valid = false;
+
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
+			break;
+		}
 
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
-		break;
-	}
 	default: {
-		mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
+			mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
-		start_loiter();
+			start_loiter();
-		break;
+			break;
-	}
+		}
 	}
 
 	publish_position_setpoint_triplet();
@@ -1312,28 +1354,35 @@ void
 Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
 {
 	sp->valid = true;
+
 	if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
 		/* set home position for RTL item */
 		sp->lat = _home_pos.lat;
 		sp->lon = _home_pos.lon;
 		sp->alt = _home_pos.alt + _parameters.rtl_alt;
+
 	} else {
 		sp->lat = item->lat;
 		sp->lon = item->lon;
 		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
 	}
+
 	sp->yaw = item->yaw;
 	sp->loiter_radius = item->loiter_radius;
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
+
 	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
 		sp->type = SETPOINT_TYPE_TAKEOFF;
+
 	} else if (item->nav_cmd == NAV_CMD_LAND) {
 		sp->type = SETPOINT_TYPE_LAND;
+
 	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-			   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-			   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
 		sp->type = SETPOINT_TYPE_LOITER;
+
 	} else {
 		sp->type = SETPOINT_TYPE_NORMAL;
 	}
@@ -1350,6 +1399,7 @@ Navigator::check_mission_item_reached()
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
 		if (_vstatus.is_rotary_wing) {
 			return _vstatus.condition_landed;
+
 		} else {
 			/* For fw there is currently no landing detector:
 			 * make sure control is not stopped when overshooting the landing waypoint */
@@ -1384,18 +1434,20 @@ Navigator::check_mission_item_reached()
 
 		/* current relative or AMSL altitude depending on mission item altitude_is_relative flag */
 		float wp_alt_amsl = _mission_item.altitude;
+
 		if (_mission_item.altitude_is_relative)
 			wp_alt_amsl += _home_pos.alt;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-										  (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+				(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-										  &dist_xy, &dist_z);
+				&dist_xy, &dist_z);
 
 		if (_do_takeoff) {
 			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
 				/* require only altitude for takeoff */
 				_waypoint_position_reached = true;
 			}
+
 		} else {
 			if (dist >= 0.0f && dist <= acceptance_radius) {
 				_waypoint_position_reached = true;
@@ -1407,9 +1459,11 @@ Navigator::check_mission_item_reached()
 		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
+
 			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
+
 		} else {
 			_waypoint_yaw_reached = true;
 		}
@@ -1419,6 +1473,7 @@ Navigator::check_mission_item_reached()
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 		if (_time_first_inside_orbit == 0) {
 			_time_first_inside_orbit = now;
+
 			if (_mission_item.time_inside > 0.01f) {
 				mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
 			}
@@ -1426,13 +1481,14 @@ Navigator::check_mission_item_reached()
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
-		     || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			_time_first_inside_orbit = 0;
 			_waypoint_yaw_reached = false;
 			_waypoint_position_reached = false;
 			return true;
 		}
 	}
+
 	return false;
 
 }
@@ -1445,6 +1501,7 @@ Navigator::on_mission_item_reached()
 			/* takeoff completed */
 			_do_takeoff = false;
 			mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
+
 		} else {
 			/* advance by one mission item */
 			_mission.move_to_next();
@@ -1452,23 +1509,28 @@ Navigator::on_mission_item_reached()
 
 		if (_mission.current_mission_available()) {
 			set_mission_item();
+
 		} else {
 			/* if no more mission items available then finish mission */
 			/* loiter at last waypoint */
 			_reset_loiter_pos = false;
 			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
+
 			if (_vstatus.condition_landed) {
 				dispatch(EVENT_READY_REQUESTED);
+
 			} else {
 				dispatch(EVENT_LOITER_REQUESTED);
 			}
 		}
+
 	} else {
 		/* RTL finished */
 		if (_rtl_state == RTL_STATE_LAND) {
 			/* landed at home position */
 			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
 			dispatch(EVENT_READY_REQUESTED);
+
 		} else {
 			/* next RTL step */
 			_rtl_state = (RTLState)(_rtl_state + 1);
@@ -1546,6 +1608,7 @@ Navigator::publish_control_mode()
 		default:
 			break;
 		}
+
 		break;
 
 	case FAILSAFE_STATE_RTL:
@@ -1583,6 +1646,7 @@ Navigator::publish_control_mode()
 	/* navigator has control, set control mode flags according to nav state*/
 	if (navigator_enabled) {
 		_control_mode.flag_control_manual_enabled = false;
+
 		if (myState == NAV_STATE_READY) {
 			/* disable all controllers, armed but idle */
 			_control_mode.flag_control_rates_enabled = false;
@@ -1591,6 +1655,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_velocity_enabled = false;
 			_control_mode.flag_control_altitude_enabled = false;
 			_control_mode.flag_control_climb_rate_enabled = false;
+
 		} else {
 			_control_mode.flag_control_rates_enabled = true;
 			_control_mode.flag_control_attitude_enabled = true;
@@ -1669,8 +1734,9 @@ int navigator_main(int argc, char *argv[])
 
 	} else if (!strcmp(argv[1], "fence")) {
 		navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
+
 	} else if (!strcmp(argv[1], "fencefile")) {
-			navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
+		navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);
 
 	} else {
 		usage();