cammander, navigator: code style fixed

src/modules/commander/commander.cpp

@@ -372,6 +372,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;
 
@@ -406,6 +407,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;
 
@@ -454,6 +456,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 						}
 					}
 				}
+
 			} else {
 				mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
 			}
@@ -497,8 +500,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();
@@ -519,7 +523,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
@@ -891,6 +895,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;
@@ -1066,9 +1071,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
-				(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
-				sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1086,7 +1091,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (status.arming_state == ARMING_STATE_STANDBY &&
-				sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off) {
 						print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1143,14 +1148,18 @@ int commander_thread_main(int argc, char *argv[])
 				status.rc_signal_lost = true;
 				status_changed = true;
 			}
+
 			if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
 				transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+
 				if (res == TRANSITION_CHANGED) {
 					mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
 					status.set_nav_state = NAV_STATE_RTL;
 					status.set_nav_state_timestamp = hrt_absolute_time();
+
 				} else if (status.main_state != MAIN_STATE_SEATBELT) {
 					res = main_state_transition(&status, MAIN_STATE_SEATBELT);
+
 					if (res == TRANSITION_CHANGED) {
 						mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
 					}
@@ -1178,13 +1187,16 @@ int commander_thread_main(int argc, char *argv[])
 				if (offboard_sp.armed && !armed.armed) {
 					if (!safety.safety_switch_available || safety.safety_off) {
 						transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+
 						if (res == TRANSITION_CHANGED) {
 							mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal");
 						}
 					}
+
 				} else if (!offboard_sp.armed && armed.armed) {
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 					transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+
 					if (res == TRANSITION_CHANGED) {
 						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal");
 					}
@@ -1202,9 +1214,11 @@ int commander_thread_main(int argc, char *argv[])
 		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
 		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
 			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
+
 			if (flighttermination_res == TRANSITION_CHANGED) {
 				tune_positive();
 			}
+
 		} else {
 			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
 		}

src/modules/commander/state_machine_helper.cpp

@@ -67,7 +67,7 @@ static bool flighttermination_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;
 		}
@@ -238,8 +239,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 			/* need at minimum altitude estimate */
 			if (!current_state->is_rotary_wing ||
-				(current_state->condition_local_altitude_valid ||
+			    (current_state->condition_local_altitude_valid ||
-				current_state->condition_global_position_valid)) {
+			     current_state->condition_global_position_valid)) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -249,7 +250,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 			/* need at minimum local position estimate */
 			if (current_state->condition_local_position_valid ||
-				current_state->condition_global_position_valid) {
+			    current_state->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -373,35 +374,36 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-		/* only check transition if the new state is actually different from the current one */
+	/* only check transition if the new state is actually different from the current one */
-		if (new_flighttermination_state == status->flighttermination_state) {
+	if (new_flighttermination_state == status->flighttermination_state) {
-			ret = TRANSITION_NOT_CHANGED;
+		ret = TRANSITION_NOT_CHANGED;
 
-		} else {
+	} else {
 
-			switch (new_flighttermination_state) {
+		switch (new_flighttermination_state) {
-			case FLIGHTTERMINATION_STATE_ON:
+		case FLIGHTTERMINATION_STATE_ON:
-				ret = TRANSITION_CHANGED;
+			ret = TRANSITION_CHANGED;
-				status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
+			status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
-				warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
+			warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
-				break;
+			break;
-			case FLIGHTTERMINATION_STATE_OFF:
-				ret = TRANSITION_CHANGED;
-				status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
-				break;
 
-			default:
+		case FLIGHTTERMINATION_STATE_OFF:
-				break;
+			ret = TRANSITION_CHANGED;
-			}
+			status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
+			break;
 
-			if (ret == TRANSITION_CHANGED) {
+		default:
-				flighttermination_state_changed = true;
+			break;
-				// TODO
-				//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
-			}
 		}
 
-		return ret;
+		if (ret == TRANSITION_CHANGED) {
+			flighttermination_state_changed = true;
+			// TODO
+			//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+		}
+	}
+
+	return ret;
 }
 
 

src/modules/navigator/navigator_main.cpp

@@ -172,7 +172,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;
 
@@ -508,16 +508,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);
@@ -534,6 +538,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);
@@ -576,11 +581,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");
@@ -665,8 +672,9 @@ Navigator::task_main()
 
 			/* Evaluate state machine from commander and set the navigator mode accordingly */
 			if (_vstatus.main_state == MAIN_STATE_AUTO &&
-					(_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) {
+			    (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) {
 				bool stick_mode = false;
+
 				if (!_vstatus.rc_signal_lost) {
 					/* RC signal available, use control switches to set mode */
 					/* RETURN switch, overrides MISSION switch */
@@ -674,21 +682,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);
@@ -714,15 +728,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:
@@ -735,6 +752,7 @@ Navigator::task_main()
 						if (myState == NAV_STATE_NONE) {
 							if (_mission.current_mission_available()) {
 								dispatch(EVENT_MISSION_REQUESTED);
+
 							} else {
 								dispatch(EVENT_LOITER_REQUESTED);
 							}
@@ -783,6 +801,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()) {
@@ -791,15 +810,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;
@@ -852,45 +871,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 / 1e7d, _global_pos.lat / 1e7d);
 		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
-			(double)_global_pos.alt, (double)_global_pos.relative_alt);
+		      (double)_global_pos.alt, (double)_global_pos.relative_alt);
 		warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f",
-			(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
+		      (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
 		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},
@@ -900,7 +926,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},
@@ -910,7 +936,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},
@@ -920,7 +946,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},
@@ -930,7 +956,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},
@@ -993,6 +1019,7 @@ Navigator::start_loiter()
 		if (_global_pos.alt < min_alt_amsl) {
 			_mission_item_triplet.current.altitude = min_alt_amsl;
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+
 		} else {
 			_mission_item_triplet.current.altitude = _global_pos.alt;
 			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
@@ -1043,27 +1070,28 @@ Navigator::set_mission_item()
 		add_home_pos_to_rtl(&_mission_item_triplet.current);
 
 		if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
-			_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
+		    _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
-			_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
+		    _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
-			_mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
+		    _mission_item_triplet.current.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_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
+				    _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF ||
-				_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
+				    _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT ||
-				_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
+				    _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
-				_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+				    _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+				    _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
+				    _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED
-				)) {
+			    )) {
 				/* do special TAKEOFF handling for VTOL */
 				_need_takeoff = false;
 
 				/* calculate desired takeoff altitude AMSL */
 				float takeoff_alt_amsl = _mission_item_triplet.current.altitude;
+
 				if (_mission_item_triplet.current.altitude_is_relative)
 					takeoff_alt_amsl += _home_pos.altitude;
 
@@ -1089,6 +1117,7 @@ Navigator::set_mission_item()
 					_mission_item_triplet.current.altitude = takeoff_alt_amsl;
 					_mission_item_triplet.current.altitude_is_relative = false;
 				}
+
 			} else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) {
 				/* will need takeoff after landing */
 				_need_takeoff = true;
@@ -1097,13 +1126,16 @@ Navigator::set_mission_item()
 
 		if (_do_takeoff) {
 			mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude);
+
 		} 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_triplet.current_valid = false;
@@ -1116,6 +1148,7 @@ Navigator::set_mission_item()
 		if (ret == OK) {
 			add_home_pos_to_rtl(&_mission_item_triplet.next);
 			_mission_item_triplet.next_valid = true;
+
 		} else {
 			/* this will fail for the last WP */
 			_mission_item_triplet.next_valid = false;
@@ -1129,17 +1162,21 @@ void
 Navigator::start_rtl()
 {
 	_do_takeoff = false;
+
 	if (_rtl_state == RTL_STATE_NONE) {
 		if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) {
 			_rtl_state = RTL_STATE_CLIMB;
+
 		} else {
 			_rtl_state = RTL_STATE_RETURN;
+
 			if (_reset_loiter_pos) {
 				_mission_item_triplet.current.altitude_is_relative = false;
 				_mission_item_triplet.current.altitude = _global_pos.alt;
 			}
 		}
 	}
+
 	_reset_loiter_pos = true;
 	mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
 	set_rtl_item();
@@ -1150,107 +1187,112 @@ Navigator::set_rtl_item()
 {
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-		memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
+			memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
-		_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
+			_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
-		_mission_item_triplet.current_valid = true;
+			_mission_item_triplet.current_valid = true;
-		_mission_item_triplet.next_valid = false;
+			_mission_item_triplet.next_valid = false;
 
-		float climb_alt = _home_pos.altitude + _parameters.rtl_alt;
+			float climb_alt = _home_pos.altitude + _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_triplet.current.altitude_is_relative = false;
+			_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
+			_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
+			_mission_item_triplet.current.altitude = climb_alt;
+			_mission_item_triplet.current.yaw = NAN;
+			_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
+			_mission_item_triplet.current.loiter_direction = 1;
+			_mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item_triplet.current.time_inside = 0.0f;
+			_mission_item_triplet.current.pitch_min = 0.0f;
+			_mission_item_triplet.current.autocontinue = true;
+			_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
+			break;
+		}
 
-		_mission_item_triplet.current.altitude_is_relative = false;
-		_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
-		_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
-		_mission_item_triplet.current.altitude = climb_alt;
-		_mission_item_triplet.current.yaw = NAN;
-		_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
-		_mission_item_triplet.current.loiter_direction = 1;
-		_mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF;
-		_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item_triplet.current.time_inside = 0.0f;
-		_mission_item_triplet.current.pitch_min = 0.0f;
-		_mission_item_triplet.current.autocontinue = true;
-		_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude);
-		break;
-	}
 	case RTL_STATE_RETURN: {
-		memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
+			memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
-		_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
+			_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
-		_mission_item_triplet.current_valid = true;
+			_mission_item_triplet.current_valid = true;
-		_mission_item_triplet.next_valid = false;
+			_mission_item_triplet.next_valid = false;
+
+			_mission_item_triplet.current.lat = _home_pos.lat;
+			_mission_item_triplet.current.lon = _home_pos.lon;
+			// don't change altitude setpoint
+			_mission_item_triplet.current.yaw = NAN;
+			_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
+			_mission_item_triplet.current.loiter_direction = 1;
+			_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item_triplet.current.time_inside = 0.0f;
+			_mission_item_triplet.current.pitch_min = 0.0f;
+			_mission_item_triplet.current.autocontinue = true;
+			_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
+			break;
+		}
 
-		_mission_item_triplet.current.lat = _home_pos.lat;
-		_mission_item_triplet.current.lon = _home_pos.lon;
-		// don't change altitude setpoint
-		_mission_item_triplet.current.yaw = NAN;
-		_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
-		_mission_item_triplet.current.loiter_direction = 1;
-		_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item_triplet.current.time_inside = 0.0f;
-		_mission_item_triplet.current.pitch_min = 0.0f;
-		_mission_item_triplet.current.autocontinue = true;
-		_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
-		break;
-	}
 	case RTL_STATE_DESCEND: {
-		memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
+			memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
-		_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
+			_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
-		_mission_item_triplet.current_valid = true;
+			_mission_item_triplet.current_valid = true;
-		_mission_item_triplet.next_valid = false;
+			_mission_item_triplet.next_valid = false;
 
-		float descend_alt = _home_pos.altitude + _parameters.land_alt;
+			float descend_alt = _home_pos.altitude + _parameters.land_alt;
+
+			_mission_item_triplet.current.altitude_is_relative = false;
+			_mission_item_triplet.current.lat = _home_pos.lat;
+			_mission_item_triplet.current.lon = _home_pos.lon;
+			_mission_item_triplet.current.altitude = descend_alt;
+			_mission_item_triplet.current.yaw = NAN;
+			_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
+			_mission_item_triplet.current.loiter_direction = 1;
+			_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item_triplet.current.time_inside = 0.0f;
+			_mission_item_triplet.current.pitch_min = 0.0f;
+			_mission_item_triplet.current.autocontinue = true;
+			_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
+			break;
+		}
 
-		_mission_item_triplet.current.altitude_is_relative = false;
-		_mission_item_triplet.current.lat = _home_pos.lat;
-		_mission_item_triplet.current.lon = _home_pos.lon;
-		_mission_item_triplet.current.altitude = descend_alt;
-		_mission_item_triplet.current.yaw = NAN;
-		_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
-		_mission_item_triplet.current.loiter_direction = 1;
-		_mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item_triplet.current.time_inside = 0.0f;
-		_mission_item_triplet.current.pitch_min = 0.0f;
-		_mission_item_triplet.current.autocontinue = true;
-		_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
-		mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude);
-		break;
-	}
 	case RTL_STATE_LAND: {
-		memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
+			memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
-		_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
+			_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
-		_mission_item_triplet.current_valid = true;
+			_mission_item_triplet.current_valid = true;
-		_mission_item_triplet.next_valid = false;
+			_mission_item_triplet.next_valid = false;
+
+			_mission_item_triplet.current.altitude_is_relative = false;
+			_mission_item_triplet.current.lat = _home_pos.lat;
+			_mission_item_triplet.current.lon = _home_pos.lon;
+			_mission_item_triplet.current.altitude = _home_pos.altitude;
+			_mission_item_triplet.current.yaw = NAN;
+			_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
+			_mission_item_triplet.current.loiter_direction = 1;
+			_mission_item_triplet.current.nav_cmd = NAV_CMD_LAND;
+			_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
+			_mission_item_triplet.current.time_inside = 0.0f;
+			_mission_item_triplet.current.pitch_min = 0.0f;
+			_mission_item_triplet.current.autocontinue = true;
+			_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
+			break;
+		}
 
-		_mission_item_triplet.current.altitude_is_relative = false;
-		_mission_item_triplet.current.lat = _home_pos.lat;
-		_mission_item_triplet.current.lon = _home_pos.lon;
-		_mission_item_triplet.current.altitude = _home_pos.altitude;
-		_mission_item_triplet.current.yaw = NAN;
-		_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
-		_mission_item_triplet.current.loiter_direction = 1;
-		_mission_item_triplet.current.nav_cmd = NAV_CMD_LAND;
-		_mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius;
-		_mission_item_triplet.current.time_inside = 0.0f;
-		_mission_item_triplet.current.pitch_min = 0.0f;
-		_mission_item_triplet.current.autocontinue = true;
-		_mission_item_triplet.current.origin = ORIGIN_ONBOARD;
-		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_mission_item_triplet();
@@ -1267,6 +1309,7 @@ Navigator::check_mission_item_reached()
 	if (_mission_item_triplet.current.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 */
@@ -1301,18 +1344,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_triplet.current.altitude;
+
 		if (_mission_item_triplet.current.altitude_is_relative)
 			_mission_item_triplet.current.altitude += _home_pos.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl,
-										  (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
+				(double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _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;
@@ -1324,9 +1369,11 @@ Navigator::check_mission_item_reached()
 		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) {
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item_triplet.current.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;
 		}
@@ -1336,6 +1383,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_triplet.current.time_inside > 0.01f) {
 				mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside);
 			}
@@ -1343,13 +1391,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_triplet.current.time_inside * 1e6)
-		     || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+		    || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
 			_time_first_inside_orbit = 0;
 			_waypoint_yaw_reached = false;
 			_waypoint_position_reached = false;
 			return true;
 		}
 	}
+
 	return false;
 
 }
@@ -1362,6 +1411,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();
@@ -1369,23 +1419,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);
@@ -1508,6 +1563,7 @@ Navigator::publish_control_mode()
 	case MAIN_STATE_AUTO:
 		_control_mode.flag_control_offboard_enabled = false;
 		_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;
@@ -1516,6 +1572,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;
@@ -1524,6 +1581,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_altitude_enabled = true;
 			_control_mode.flag_control_climb_rate_enabled = true;
 		}
+
 		break;
 
 	case MAIN_STATE_OFFBOARD:
@@ -1539,6 +1597,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_position_enabled = false;
 			_control_mode.flag_control_velocity_enabled = false;
 			break;
+
 		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
 			_control_mode.flag_control_rates_enabled = true;
 			_control_mode.flag_control_attitude_enabled = true;
@@ -1547,6 +1606,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_position_enabled = false;
 			_control_mode.flag_control_velocity_enabled = false;
 			break;
+
 		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
 			_control_mode.flag_control_rates_enabled = true;
 			_control_mode.flag_control_attitude_enabled = true;
@@ -1555,6 +1615,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_position_enabled = false;
 			_control_mode.flag_control_velocity_enabled = true;
 			break;
+
 		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
 			_control_mode.flag_control_rates_enabled = true;
 			_control_mode.flag_control_attitude_enabled = true;
@@ -1563,6 +1624,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_position_enabled = true;
 			_control_mode.flag_control_velocity_enabled = true;
 			break;
+
 		default:
 			_control_mode.flag_control_rates_enabled = false;
 			_control_mode.flag_control_attitude_enabled = false;
@@ -1572,6 +1634,7 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_velocity_enabled = false;
 			break;
 		}
+
 		break;
 
 	default:
@@ -1591,7 +1654,8 @@ Navigator::publish_control_mode()
 	}
 }
 
-bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
+bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b)
+{
 	if (a.altitude_is_relative == b.altitude_is_relative &&
 	    fabs(a.lat - b.lat) < FLT_EPSILON &&
 	    fabs(a.lon - b.lon) < FLT_EPSILON &&
@@ -1604,6 +1668,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const
 	    fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
 	    a.autocontinue == b.autocontinue) {
 		return true;
+
 	} else {
 		return false;
 	}
@@ -1664,8 +1729,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();