Merge remote-tracking branch 'origin/master' into geo

src/modules/commander/accelerometer_calibration.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/accelerometer_calibration.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/airspeed_calibration.cpp

@@ -51,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <systemlib/airspeed.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -64,19 +65,17 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
 
-	const int calibration_count = 2000;
+	const unsigned calibration_count = 2000;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
 
-	int calibration_counter = 0;
 	float diff_pres_offset = 0.0f;
 
 	/* Reset sensor parameters */
 	struct airspeed_scale airscale = {
-		0.0f,
+		diff_pres_offset,
 		1.0f,
 	};
 
@@ -95,12 +94,29 @@ int do_airspeed_calibration(int mavlink_fd)
 	}
 
 	if (!paramreset_successful) {
-		warn("FAILED to set scale / offsets for airspeed");
-		mavlink_log_critical(mavlink_fd, "dpress reset failed");
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-		return ERROR;
+
+		/* only warn if analog scaling is zero */
+		float analog_scaling = 0.0f;
+		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
+		if (fabsf(analog_scaling) < 0.1f) {
+			mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
+			close(diff_pres_sub);
+			return ERROR;
+		}
+
+		/* set scaling offset parameter */
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			close(diff_pres_sub);
+			return ERROR;
+		}
 	}
 
+	unsigned calibration_counter = 0;
+
+	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+	usleep(500 * 1000);
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -112,11 +128,12 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
 			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
 			}
 
 		} else if (poll_ret == 0) {
@@ -131,6 +148,16 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	if (isfinite(diff_pres_offset)) {
 
+		int  fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
+		airscale.offset_pa = diff_pres_offset;
+		if (fd_scale > 0) {
+			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+				mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
+			}
+
+			close(fd_scale);
+		}
+
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			close(diff_pres_sub);
@@ -147,14 +174,91 @@ int do_airspeed_calibration(int mavlink_fd)
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-		tune_neutral(true);
-		close(diff_pres_sub);
-		return OK;
-
 	} else {
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		close(diff_pres_sub);
 		return ERROR;
 	}
+
+	mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
+
+	/* wait 500 ms to ensure parameter propagated through the system */
+	usleep(500 * 1000);
+
+	calibration_counter = 0;
+	const unsigned maxcount = 3000;
+
+	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
+	while (calibration_counter < maxcount) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+			calibration_counter++;
+
+			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
+				if (calibration_counter % 100 == 0) {
+					mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				}
+				continue;
+			}
+
+			/* do not allow negative values */
+			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
+				mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
+				close(diff_pres_sub);
+
+				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
+
+				diff_pres_offset = 0.0f;
+				if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+					mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+					close(diff_pres_sub);
+					return ERROR;
+				}
+
+				/* save */
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
+				(void)param_save_default();
+
+				close(diff_pres_sub);
+
+				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+				return ERROR;
+			} else {
+				mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				break;
+			}
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			close(diff_pres_sub);
+			return ERROR;
+		}
+	}
+
+	if (calibration_counter == maxcount) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		close(diff_pres_sub);
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+
+	mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	tune_neutral(true);
+	close(diff_pres_sub);
+	return OK;
 }

src/modules/commander/airspeed_calibration.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/baro_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/baro_calibration.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/calibration_messages.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/calibration_routines.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/calibration_routines.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/commander_helper.cpp

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,11 @@
 /**
  * @file commander_helper.cpp
  * Commander helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  */
 
 #include <stdio.h>
@@ -279,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 	}
 }
 
-float battery_remaining_estimate_voltage(float voltage, float discharged)
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
 {
 	float ret = 0;
 	static param_t bat_v_empty_h;
 	static param_t bat_v_full_h;
 	static param_t bat_n_cells_h;
 	static param_t bat_capacity_h;
-	static float bat_v_empty = 3.2f;
-	static float bat_v_full = 4.0f;
+	static param_t bat_v_load_drop_h;
+	static float bat_v_empty = 3.4f;
+	static float bat_v_full = 4.2f;
+	static float bat_v_load_drop = 0.06f;
 	static int bat_n_cells = 3;
 	static float bat_capacity = -1.0f;
 	static bool initialized = false;
@@ -295,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 
 	if (!initialized) {
 		bat_v_empty_h = param_find("BAT_V_EMPTY");
-		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_v_full_h = param_find("BAT_V_CHARGED");
 		bat_n_cells_h = param_find("BAT_N_CELLS");
 		bat_capacity_h = param_find("BAT_CAPACITY");
+		bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
 		initialized = true;
 	}
 
 	if (counter % 100 == 0) {
 		param_get(bat_v_empty_h, &bat_v_empty);
 		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_v_load_drop_h, &bat_v_load_drop);
 		param_get(bat_n_cells_h, &bat_n_cells);
 		param_get(bat_capacity_h, &bat_capacity);
 	}
 
 	counter++;
 
-	/* remaining charge estimate based on voltage */
-	float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
+	float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
+	float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
 
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */

src/modules/commander/commander_helper.h

@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
 /**
  * @file commander_helper.h
  * Commander helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef COMMANDER_HELPER_H_
@@ -77,8 +78,11 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
  * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
  * else use simple estimate based on voltage.
  *
+ * @param voltage the current battery voltage
+ * @param discharged the discharged capacity
+ * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(float voltage, float discharged);
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
 
 #endif /* COMMANDER_HELPER_H_ */

src/modules/commander/commander_params.c

@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
  *
  * @group Battery Calibration
  */
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
+
+/**
+ * Voltage drop per cell on 100% load
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio and assumes linearity.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
 /**
  * Number of cells.

src/modules/commander/commander_tests/state_machine_helper_test.cpp

@@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         
         // Safety switch arming tests
         
-        { "transition: init to standby, no safety switch",
+        { "transition: standby to armed, no safety switch",
             { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
             ARMING_STATE_ARMED,
             { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
         
-        { "transition: init to standby, safety switch off",
+        { "transition: standby to armed, safety switch off",
             { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
             ARMING_STATE_ARMED,
             { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         armed.ready_to_arm = test->current_state.ready_to_arm;
         
         // Attempt transition
-        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
 bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
-	struct vehicle_status_s current_state;
-	main_state_t new_main_state;
+	// This structure represent a single test case for testing Main State transitions.
+	typedef struct {
+		const char*     assertMsg;				// Text to show when test case fails
+		uint8_t		condition_bits;				// Bits for various condition_* values
+		main_state_t	from_state;				// State prior to transition request
+		main_state_t	to_state;				// State to transition to
+		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
+	} MainTransitionTest_t;
 	
-	// Identical states.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	new_main_state = MAIN_STATE_MANUAL;
-	ut_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+	// Bits for condition_bits
+	#define MTT_ALL_NOT_VALID		0
+	#define MTT_ROTARY_WING			1 << 0
+	#define MTT_LOC_ALT_VALID		1 << 1
+	#define MTT_LOC_POS_VALID		1 << 2
+	#define MTT_HOME_POS_VALID		1 << 3
+	#define MTT_GLOBAL_POS_VALID		1 << 4
+	
+	static const MainTransitionTest_t rgMainTransitionTests[] = {
+		
+		// TRANSITION_NOT_CHANGED tests
+		
+		{ "no transition: identical states",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+				
+		// TRANSITION_CHANGED tests
+		
+		{ "transition: MANUAL to ACRO",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
 
-	// AUTO to MANUAL.
-	current_state.main_state = MAIN_STATE_AUTO;
-	new_main_state = MAIN_STATE_MANUAL;
-	ut_assert("transition changed: auto to manual",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+		{ "transition: ACRO to MANUAL",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-	// MANUAL to ALTCTRL.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = true;
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("tranisition: manual to altctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
+		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
-	// MANUAL to ALTCTRL, invalid local altitude.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = false;
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("no transition: invalid local altitude",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-	// MANUAL to POSCTRL.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = true;
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("transition: manual to posctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
+		{ "transition: MANUAL to AUTO_LOITER - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
 
-	// MANUAL to POSCTRL, invalid local position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = false;
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("no transition: invalid position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+		{ "transition: AUTO_LOITER to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-	// MANUAL to AUTO.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = true;
-	new_main_state = MAIN_STATE_AUTO;
-	ut_assert("transition: manual to auto",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
+			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
+			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: ALTCTL to MANUAL - local altitude valid",
+			MTT_LOC_ALT_VALID,
+			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		// TRANSITION_DENIED tests
+
+		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
+			MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
+		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
+			MTT_ROTARY_WING,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+	};
+	
+	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
+	for (size_t i=0; i<cMainTransitionTests; i++) {
+		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
+
+		// Setup initial machine state
+		struct vehicle_status_s current_state;
+		current_state.main_state = test->from_state;
+		current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
+		current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		
+		// Attempt transition
+		transition_result_t result = main_state_transition(&current_state, test->to_state);
+		
+		// Validate result of transition
+		ut_assert(test->assertMsg, test->expected_transition_result == result);
+		if (test->expected_transition_result == result) {
+			if (test->expected_transition_result == TRANSITION_CHANGED) {
+				ut_assert(test->assertMsg, test->to_state == current_state.main_state);
+			} else {
+				ut_assert(test->assertMsg, test->from_state == current_state.main_state);
+			}
+		}
+	}
 
-	// MANUAL to AUTO, invalid global position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = false;
-	new_main_state = MAIN_STATE_AUTO;
-	ut_assert("no transition: invalid global position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	return true;
 }

src/modules/commander/gyro_calibration.cpp

@@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd)
 		unsigned poll_errcount = 0;
 
 		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
 		struct gyro_report gyro_report;
 
 		while (calibration_counter < calibration_count) {
@@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
 				gyro_scale.x_offset += gyro_report.x;
 				gyro_scale.y_offset += gyro_report.y;
 				gyro_scale.z_offset += gyro_report.z;

src/modules/commander/mag_calibration.cpp

@@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
 		struct mag_report mag;
 
 		/* limit update rate to get equally spaced measurements over time (in ms) */
@@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+				orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
 
 				x[calibration_counter] = mag.x;
 				y[calibration_counter] = mag.y;

src/modules/commander/mag_calibration.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/commander/px4_custom_mode.h

@@ -16,6 +16,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
 	PX4_CUSTOM_MAIN_MODE_ACRO,
+	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {

src/modules/commander/state_machine_helper.cpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -70,8 +71,6 @@
 #endif
 static const int ERROR = -1;
 
-static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
@@ -89,7 +88,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
-static const char *state_names[ARMING_STATE_MAX] = {
+static const char * const state_names[ARMING_STATE_MAX] = {
 	"ARMING_STATE_INIT",
 	"ARMING_STATE_STANDBY",
 	"ARMING_STATE_ARMED",
@@ -100,19 +99,20 @@ static const char *state_names[ARMING_STATE_MAX] = {
 };
 
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
-			const struct safety_s   *safety,            /// current safety settings
-			arming_state_t          new_arming_state,   /// arming state requested
-			struct actuator_armed_s *armed,             /// current armed status
-			const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
+arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
+			const struct safety_s   *safety,		///< current safety settings
+			arming_state_t          new_arming_state,	///< arming state requested
+			struct actuator_armed_s *armed,			///< current armed status
+			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
 {
 	// Double check that our static arrays are still valid
 	ASSERT(ARMING_STATE_INIT == 0);
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
-
 	arming_state_t current_arming_state = status->arming_state;
+	bool feedback_provided = false;
 
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == current_arming_state) {
@@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
@@ -157,13 +157,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
+						/* the prearm check already prints the reject reason */
+						feedback_provided = true;
 						valid_transition = false;
 
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
-
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						feedback_provided = true;
 						valid_transition = false;
 					}
 
@@ -173,16 +175,18 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 						// Fail transition if power is not good
 						if (!status->condition_power_input_valid) {
 
-							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							feedback_provided = true;
 							valid_transition = false;
 						}
 
 						// Fail transition if power levels on the avionics rail
 						// are measured but are insufficient
-						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
-							(status->avionics_power_rail_voltage < 4.9f)) {
+						if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
+							(status->avionics_power_rail_voltage < 4.9f))) {
 
-							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							feedback_provided = true;
 							valid_transition = false;
 						}
 					}
@@ -201,6 +205,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
 			valid_transition = false;
 		}
 
@@ -217,11 +223,14 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "INVAL: %s - %s";
+		const char * str = "INVAL: %s - %s";
+		/* only print to console here by default as this is too technical to be useful during operation */
+		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
 
-		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-
-		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+		/* print to MAVLink if we didn't provide any feedback yet */
+		if (!feedback_provided) {
+			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
+		}
 	}
 
 	return ret;
@@ -271,7 +280,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
-	case MAIN_STATE_AUTO_MISSION:
 	case MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
@@ -279,6 +287,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
+	case MAIN_STATE_AUTO_MISSION:
 	case MAIN_STATE_AUTO_RTL:
 		/* need global position and home position */
 		if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -286,6 +295,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
+	case MAIN_STATE_OFFBOARD:
+
+		/* need offboard signal */
+		if (!status->offboard_control_signal_lost) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
 	case MAIN_STATE_MAX:
 	default:
 		break;
@@ -584,6 +602,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		}
 		break;
 
+	case MAIN_STATE_OFFBOARD:
+		/* require offboard control, otherwise stay where you are */
+		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
+			status->failsafe = true;
+
+			status->nav_state = NAVIGATION_STATE_POSCTL;
+		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
+			status->failsafe = true;
+
+			if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_OFFBOARD;
+		}
 	default:
 		break;
 	}
@@ -594,19 +631,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 {
 	int ret;
+	bool failed = false;
 
 	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
 	if (fd < 0) {
-		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
-		ret = fd;
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
+		failed = true;
 		goto system_eval;
 	}
 
 	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
+		failed = true;
 		goto system_eval;
 	}
 
@@ -616,55 +655,44 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 
 	if (ret == sizeof(acc)) {
 		/* evaluate values */
-		float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
-		if (accel_scale < 9.78f || accel_scale > 9.83f) {
-			mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
-		}
-
-		if (accel_scale > 30.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
+		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
 			/* this is frickin' fatal */
-			ret = ERROR;
+			failed = true;
 			goto system_eval;
-		} else {
-			ret = OK;
 		}
 	} else {
-		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
 		/* this is frickin' fatal */
-		ret = ERROR;
+		failed = true;
 		goto system_eval;
 	}
 
-	if (!status->is_rotary_wing) {
-		int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
+		/* accel done, close it */
+		close(fd);
+		fd = orb_subscribe(ORB_ID(airspeed));
 
-		if (fd < 0) {
-			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
-			ret = fd;
+		struct airspeed_s airspeed;
+
+		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+			(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
+			failed = true;
 			goto system_eval;
 		}
 
-		struct differential_pressure_s diff_pres;
-
-		ret = read(fd, &diff_pres, sizeof(diff_pres));
-
-		if (ret == sizeof(diff_pres)) {
-			if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
-				mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
-				// XXX do not make this fatal yet
-				ret = OK;
-			}
-		} else {
-			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
-			/* this is frickin' fatal */
-			ret = ERROR;
-			goto system_eval;
+		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+			// XXX do not make this fatal yet
 		}
 	}
 
 system_eval:
 	close(fd);
-	return ret;
+	return (failed);
 }

src/modules/commander/state_machine_helper.h

@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,14 +34,14 @@
 /**
  * @file state_machine_helper.h
  * State machine helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef STATE_MACHINE_HELPER_H_
 #define STATE_MACHINE_HELPER_H_
 
-#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
-#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
@@ -59,7 +57,7 @@ typedef enum {
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
@@ -67,4 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+
 #endif /* STATE_MACHINE_HELPER_H_ */