1 files changed, 68 insertions, 81 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c0628a16..77d810a81 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
@@ -389,7 +389,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 {
 	/* result of the command */
 	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-	bool ret = false;
 
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
@@ -401,46 +400,40 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
-			uint8_t base_mode = (uint8_t) cmd->param1;
-			uint8_t custom_main_mode = (uint8_t) cmd->param2;
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+		uint8_t base_mode = (uint8_t) cmd->param1;
+		uint8_t custom_main_mode = (uint8_t) cmd->param2;
 
-			/* set HIL state */
-			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
+		/* set HIL state */
+		hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+		int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
-			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
-				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+		/* if HIL got enabled, reset battery status state */
+		if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
+			/* reset the arming mode to disarmed */
+			arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+		}
 
-				if (arming_res != TRANSITION_DENIED) {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+		// Transition the arming state
+		transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
-				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
-				}
-			}
+		/* set main state */
+		transition_result_t main_res = TRANSITION_DENIED;
 
-			if (hil_ret == OK) {
-				ret = true;
-			}
-
-			// Transition the arming state
-			arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+		if (hil_ret == OK) {
+			ret = true;
+		}
 
-			if (arming_res == TRANSITION_CHANGED) {
-				ret = true;
-			}
+		// Transition the arming state
+		arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
-			/* set main state */
-			transition_result_t main_res = TRANSITION_DENIED;
+		if (arming_res == TRANSITION_CHANGED) {
+			ret = true;
+		}
 
-			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-				/* use autopilot-specific mode */
-				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
-					/* MANUAL */
-					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+				/* AUTO */
+				main_res = main_state_transition(status, MAIN_STATE_AUTO);
+			}
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
@@ -459,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					main_res = main_state_transition(status, MAIN_STATE_ACRO);
 				}
 
-			} else {
+				} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
@@ -476,21 +469,22 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					}
 				}
 			}
+		}
 
-			if (main_res == TRANSITION_CHANGED) {
-				ret = true;
-			}
-
-			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+		if (main_res == TRANSITION_CHANGED) {
+			ret = true;
+		}
 
-			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			}
+		if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+			result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-			break;
+		} else {
+			result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 		}
 
+		break;
+	}
+
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
 			// We use an float epsilon delta to test float equality.
@@ -522,18 +516,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			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();
+				status->set_nav_state = NAVIGATION_STATE_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAV_STATE_MISSION;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
 
 			} else {
 				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
@@ -549,7 +539,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
 
 			} else {
 				/* reject parachute depoyment not armed */
@@ -693,8 +682,7 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAV_STATE_NONE;
-	status.set_nav_state_timestamp = 0;
+	status.set_nav_state = NAVIGATION_STATE_NONE;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 	status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -959,6 +947,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
 		bool eph_epv_good;
@@ -992,6 +988,10 @@ int commander_thread_main(int argc, char *argv[])
 			home.lon = global_position.lon;
 			home.alt = global_position.alt;
 
+			home.x = local_position.x;
+			home.y = local_position.y;
+			home.z = local_position.z;
+
 			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -1008,14 +1008,6 @@ int commander_thread_main(int argc, char *argv[])
 			tune_positive(true);
 		}
 
-		/* update local position estimate */
-		orb_check(local_position_sub, &updated);
-
-		if (updated) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-		}
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		/* hysteresis for EPH */
 		bool local_eph_good;
@@ -1039,13 +1031,10 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
-		static bool published_condition_landed_fw = false;
-
-		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
+		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
-				published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
 
 				if (status.condition_landed) {
 					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -1054,13 +1043,6 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
-
-		} else {
-			if (!published_condition_landed_fw) {
-				status.condition_landed = false; // Fixedwing does not have a landing detector currently
-				published_condition_landed_fw = true;
-				status_changed = true;
-			}
 		}
 
 		/* update battery status */
@@ -1286,27 +1268,24 @@ int commander_thread_main(int argc, char *argv[])
 			/* RETURN switch, overrides MISSION switch */
 			if (sp_man.return_switch == SWITCH_POS_ON) {
 				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAV_STATE_RTL;
-				status.set_nav_state_timestamp = hrt_absolute_time();
+				status.set_nav_state = NAVIGATION_STATE_RTL;
+
 
 			} else {
 
 				/* LOITER switch */
 				if (sp_man.loiter_switch == SWITCH_POS_ON) {
 					/* stick is in LOITER position */
-					status.set_nav_state = NAV_STATE_LOITER;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+					status.set_nav_state = NAVIGATION_STATE_LOITER;
 
 				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
 					/* stick is in MISSION position */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+					status.set_nav_state = NAVIGATION_STATE_MISSION;
 
 				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
 					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
 					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+					status.set_nav_state = NAVIGATION_STATE_MISSION;
 				}
 			}
 
@@ -1335,6 +1314,8 @@ int commander_thread_main(int argc, char *argv[])
 							/* LAND not allowed, set TERMINATION state */
 							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
+					} else {
+						status.set_nav_state = NAVIGATION_STATE_MISSION;
 					}
 
 				} else {
@@ -1354,6 +1335,8 @@ int commander_thread_main(int argc, char *argv[])
 							/* LAND not allowed, set TERMINATION state */
 							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
+					} else {
+						status.set_nav_state = NAVIGATION_STATE_RTL;
 					}
 				}
 
@@ -1407,6 +1390,10 @@ int commander_thread_main(int argc, char *argv[])
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
 
+				home.x = local_position.x;
+				home.y = local_position.y;
+				home.z = local_position.z;
+
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 				mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);