Merged master into driver_framework

src/modules/commander/CMakeLists.txt

@@ -37,7 +37,7 @@ endif()
 px4_add_module(
 	MODULE modules__commander
 	MAIN commander
-	STACK 1200
+	STACK 4096
 	COMPILE_FLAGS
 		${MODULE_CFLAGS}
 		-Os

src/modules/commander/PreflightCheck.cpp

@@ -116,7 +116,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
 	return !calibration_found;
 }
 
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -127,8 +127,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -137,8 +139,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 	int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
 
 	if (ret) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -146,8 +150,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
 	ret = h.ioctl(MAGIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -157,7 +163,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -168,8 +174,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -178,8 +186,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 	int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
 
 	if (ret) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -187,8 +197,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 	ret = h.ioctl(ACCELIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -204,13 +216,17 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
+				if (report_fail) {
+					mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
+				}
 				/* this is frickin' fatal */
 				success = false;
 				goto out;
 			}
 		} else {
-			mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
+			}
 			/* this is frickin' fatal */
 			success = false;
 			goto out;
@@ -223,7 +239,7 @@ out:
 	return success;
 }
 
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -234,8 +250,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -244,8 +262,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
 
 	if (ret) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -253,8 +273,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	ret = h.ioctl(GYROIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		mavlink_and_console_log_critical(mavlink_fd,
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
+		}
 		success = false;
 		goto out;
 	}
@@ -264,7 +286,7 @@ out:
 	return success;
 }
 
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
 	bool success = true;
 
@@ -275,8 +297,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	if (!h.isValid()) {
 		if (!optional) {
-			mavlink_and_console_log_critical(mavlink_fd,
+			if (report_fail) {
+				mavlink_and_console_log_critical(mavlink_fd,
 							 "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
+			}
 		}
 
 		return false;
@@ -300,7 +324,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	return success;
 }
 
-static bool airspeedCheck(int mavlink_fd, bool optional)
+static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
 {
 	bool success = true;
 	int ret;
@@ -310,13 +334,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
 
 	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
 	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
-		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
+		}
 		success = false;
 		goto out;
 	}
 
 	if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
-		mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
+		}
 		// XXX do not make this fatal yet
 	}
 
@@ -325,7 +353,7 @@ out:
 	return success;
 }
 
-static bool gnssCheck(int mavlink_fd)
+static bool gnssCheck(int mavlink_fd, bool report_fail)
 {
 	bool success = true;
 
@@ -347,8 +375,10 @@ static bool gnssCheck(int mavlink_fd)
 	}
 
 	//Report failure to detect module
-	if(!success) {
-		mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+	if (!success) {
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+		}
 	}
 
 	px4_close(gpsSub);
@@ -356,7 +386,7 @@ static bool gnssCheck(int mavlink_fd)
 }
 
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
-		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
 {
 	bool failed = false;
 
@@ -371,7 +401,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_mag_count);
 			int device_id = -1;
 
-			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -382,7 +412,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 		/* check if the primary device is present */
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
+			}
 			failed = true;
 		}
 	}
@@ -398,7 +430,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_accel_count);
 			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -409,7 +441,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 		/* check if the primary device is present */
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
+			}
 			failed = true;
 		}
 	}
@@ -425,7 +459,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_gyro_count);
 			int device_id = -1;
 
-			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -436,7 +470,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 		/* check if the primary device is present */
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
+			}
 			failed = true;
 		}
 	}
@@ -452,7 +488,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 			bool required = (i < max_mandatory_baro_count);
 			int device_id = -1;
 
-			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
 				failed = true;
 			}
 
@@ -464,29 +500,33 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		// TODO there is no logic in place to calibrate the primary baro yet
 		// // check if the primary device is present 
 		if (!prime_found && prime_id != 0) {
-			mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			}
 			failed = true;
 		}
 	}
 
 	/* ---- AIRSPEED ---- */
 	if (checkAirspeed) {
-		if (!airspeedCheck(mavlink_fd, true)) {
+		if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
 			failed = true;
 		}
 	}
 
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
-		if (rc_calibration_check(mavlink_fd) != OK) {
-			mavlink_log_critical(mavlink_fd, "RC calibration check failed");
+		if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
+			}
 			failed = true;
 		}
 	}
 
 	/* ---- Global Navigation Satellite System receiver ---- */
 	if (checkGNSS) {
-		if(!gnssCheck(mavlink_fd)) {
+		if (!gnssCheck(mavlink_fd, reportFailures)) {
 			failed = true;
 		}
 	}

src/modules/commander/PreflightCheck.h

@@ -67,7 +67,7 @@ namespace Commander
 *   true if the GNSS receiver should be checked
 **/
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
-    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
+    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);
 
 const unsigned max_mandatory_gyro_count = 1;
 const unsigned max_optional_gyro_count = 3;

src/modules/commander/airspeed_calibration.cpp

@@ -68,7 +68,7 @@ static const char *sensor_name = "dpress";
 static void feedback_calibration_failed(int mavlink_fd)
 {
 	sleep(5);
-	mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+	mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 }
 
 int do_airspeed_calibration(int mavlink_fd)
@@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	const unsigned maxcount = 2400;
 
 	/* give directions */
-	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	const unsigned calibration_count = (maxcount * 2) / 3;
 
@@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd)
 			paramreset_successful = true;
 
 		} else {
-			mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
 		}
 
 		px4_close(fd);
@@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd)
 		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, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
 			goto error_return;
 		}
 
 		/* set scaling offset parameter */
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
 			goto error_return;
 		}
 	}
 
-	mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
 	usleep(500 * 1000);
 
 	while (calibration_counter < calibration_count) {
@@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd)
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
-				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
+				mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
 			}
 
 		} else if (poll_ret == 0) {
@@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd)
 		airscale.offset_pa = diff_pres_offset;
 		if (fd_scale > 0) {
 			if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
-				mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
 			}
 
 			px4_close(fd_scale);
 		}
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
 			goto error_return;
 		}
 
@@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 			goto error_return;
 		}
 
@@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd)
 		goto error_return;
 	}
 
-	mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
 
 	/* wait 500 ms to ensure parameter propagated through the system */
 	usleep(500 * 1000);
 
-	mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
+	mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
 
 	calibration_counter = 0;
 
@@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
 				if (calibration_counter % 500 == 0) {
-					mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
+					mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
 				}
 				continue;
@@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
 
 			/* do not allow negative values */
 			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
-				mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
 						(int)diff_pres.differential_pressure_raw_pa);
-				mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
 
 				/* 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_ERROR_SET_PARAMS_MSG);
+					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
 					goto error_return;
 				}
 
 				/* save */
-				mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
+				mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
 				(void)param_save_default();
 
 				feedback_calibration_failed(mavlink_fd);
 				goto error_return;
 			} else {
-				mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
+				mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
 					(int)diff_pres.differential_pressure_raw_pa);
 				break;
 			}
@@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd)
 		goto error_return;
 	}
 
-	mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
 
-	mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	tune_neutral(true);
 
 normal_return:

src/modules/commander/commander.cpp

@@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
+#define HOTPLUG_SENS_TIMEOUT		(8 * 1000 * 1000)	/**< wait for hotplug sensors to come online for upto 10 seconds */
+
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	10000000
 
@@ -299,7 +301,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = px4_task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3500,
+					     3600,
 					     commander_thread_main,
 					     (char * const *)&argv[0]);
 
@@ -360,6 +362,8 @@ int commander_main(int argc, char *argv[])
 				calib_ret = do_level_calibration(mavlink_fd);
 			} else if (!strcmp(argv[2], "esc")) {
 				calib_ret = do_esc_calibration(mavlink_fd, &armed);
+			} else if (!strcmp(argv[2], "airspeed")) {
+				calib_ret = do_airspeed_calibration(mavlink_fd);
 			} else {
 				warnx("argument %s unsupported.", argv[2]);
 			}
@@ -377,23 +381,31 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "check")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		int checkres = prearm_check(&status, mavlink_fd_local);
+		int checkres = 0;
+		checkres = preflight_check(&status, mavlink_fd_local, false, true);
+		warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
+		checkres = preflight_check(&status, mavlink_fd_local, true, true);
+		warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
 		px4_close(mavlink_fd_local);
-		warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "arm")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		arm_disarm(true, mavlink_fd_local, "command line");
-		warnx("note: not updating home position on commandline arming!");
+		if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) {
+			warnx("note: not updating home position on commandline arming!");
+		} else {
+			warnx("arming failed");
+		}
 		px4_close(mavlink_fd_local);
 		return 0;
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		arm_disarm(false, mavlink_fd_local, "command line");
+		if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) {
+			warnx("rejected disarm");
+		}
 		px4_close(mavlink_fd_local);
 		return 0;
 	}
@@ -891,6 +903,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* not yet initialized */
 	commander_initialized = false;
 
+	bool sensor_fail_tune_played = false;
 	bool arm_tune_played = false;
 	bool was_landed = true;
 	bool was_armed = false;
@@ -1020,6 +1033,9 @@ int commander_thread_main(int argc, char *argv[])
 
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
+	
+	status.condition_system_prearm_error_reported = false;
+	status.condition_system_hotplug_timeout = false;
 
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
@@ -1101,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
-	rc_calibration_check(mavlink_fd);
+	rc_calibration_check(mavlink_fd, true);
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -1243,6 +1259,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	// Run preflight check
 	int32_t rc_in_off = 0;
+	bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 	param_get(_param_autostart_id, &autostart_id);
 	param_get(_param_rc_in_off, &rc_in_off);
 	status.rc_input_mode = rc_in_off;
@@ -1251,14 +1268,10 @@ int commander_thread_main(int argc, char *argv[])
 		status.condition_system_sensors_initialized = false;
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	} else {
-		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
-			checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
-		if (!status.condition_system_sensors_initialized) {
-			set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
-		}
-		else {
+			// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
+			status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
+			checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
 			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
-		}
 	}
 
 	commander_boot_timestamp = hrt_absolute_time();
@@ -1291,7 +1304,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* initialize low priority thread */
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2600);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2880);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -1354,11 +1367,11 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
 					status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
 				}
 
 				/* re-check RC calibration */
-				rc_calibration_check(mavlink_fd);
+				rc_calibration_check(mavlink_fd, true);
 			}
 
 			/* Safety parameters */
@@ -1446,6 +1459,8 @@ int commander_thread_main(int argc, char *argv[])
 				    !armed.armed) {
 
 				    	bool chAirspeed = false;
+				    	bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+				    	
 					/* 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) {
@@ -1456,11 +1471,11 @@ int commander_thread_main(int argc, char *argv[])
 					if (is_hil_setup(autostart_id)) {
 						/* HIL configuration: check only RC input */
 						(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
-								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true);
 					} else {
 						/* check sensors also */
 						(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
-								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
 					}
 				}
 
@@ -1686,10 +1701,10 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				if (status.condition_landed) {
-					mavlink_log_critical(mavlink_fd, "LANDING DETECTED");
+					mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED");
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
+					mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED");
 				}
 			}
 
@@ -2177,11 +2192,17 @@ int commander_thread_main(int argc, char *argv[])
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
-					/* only report a regain */
+					/* report a regain */
 					if (telemetry_last_dl_loss[i] > 0) {
 						mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
+					} else if (telemetry_last_dl_loss[i] == 0) {
+						/* new link */
 					}
 
+					/* got link again or new */
+					status.condition_system_prearm_error_reported = false;
+					status_changed = true;
+
 					telemetry_lost[i] = false;
 					have_link = true;
 
@@ -2414,7 +2435,7 @@ int commander_thread_main(int argc, char *argv[])
 		} else {
 			set_tune(TONE_STOP_TUNE);
 		}
-
+		
 		/* reset arm_tune_played when disarmed */
 		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 
@@ -2425,6 +2446,21 @@ int commander_thread_main(int argc, char *argv[])
 
 			arm_tune_played = false;
 		}
+		
+		/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
+		hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+		
+		if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) {
+			set_tune_override(TONE_GPS_WARNING_TUNE);
+			sensor_fail_tune_played = true;
+			status_changed = true;
+		}
+		
+		/* update timeout flag */
+		if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) {
+			status.condition_system_hotplug_timeout = hotplug_timeout;
+			status_changed = true;
+		}
 
 		counter++;
 
@@ -2482,6 +2518,8 @@ get_circuit_breaker_params()
 {
 	status.circuit_breaker_engaged_power_check =
 		circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+	status.cb_usb =
+		circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
 	status.circuit_breaker_engaged_airspd_check =
 		circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
 	status.circuit_breaker_engaged_enginefailure_check =
@@ -2508,19 +2546,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 	/* driving rgbled */
 	if (changed) {
 		bool set_normal_color = false;
-
+		bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+		
 		/* set mode */
 		if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) {
+		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
 		} else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
 			set_normal_color = true;
+		
+		} else if (!status.condition_system_sensors_initialized && !hotplug_timeout) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
 
 		} else {	// STANDBY_ERROR and other states
 			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
@@ -2536,7 +2579,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
-				if (status_local->condition_home_position_valid) {
+				if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
 
 				} else {
@@ -3197,6 +3240,7 @@ void *commander_low_prio_loop(void *arg)
 						// so this would be prone to false negatives.
 
 						bool checkAirspeed = false;
+						bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 						/* 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) {
@@ -3204,7 +3248,7 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
-							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
 
 						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
 

src/modules/commander/commander_params.c

@@ -55,6 +55,7 @@
  * @group Radio Calibration
  * @min -0.25
  * @max 0.25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 
@@ -69,6 +70,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
  * @group Radio Calibration
  * @min -0.25
  * @max 0.25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 
@@ -83,6 +85,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
  * @group Radio Calibration
  * @min -0.25
  * @max 0.25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
 
@@ -93,6 +96,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
  *
  * @group Battery Calibration
  * @unit V
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
 
@@ -103,6 +107,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
  *
  * @group Battery Calibration
  * @unit V
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
 
@@ -115,6 +120,8 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
  * @group Battery Calibration
  * @unit V
  * @min 0.0
+ * @max 1.5
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
@@ -137,6 +144,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
  *
  * @group Battery Calibration
  * @unit mA
+ * @decimal 0
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
 
@@ -184,6 +192,7 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
  * @group Commander
  * @min 0.0
  * @max 1.0
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
 
@@ -196,6 +205,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
  * @min 0.0
  * @max 30.0
  * @unit ampere
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
 
@@ -209,6 +219,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
  * @unit second
  * @min 0.0
  * @max 60.0
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
 
@@ -221,6 +232,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
  * @unit second
  * @min 0
  * @max 35
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
 
@@ -233,6 +245,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
  * @unit meter
  * @min 2
  * @max 15
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
 
@@ -245,6 +258,7 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
  * @unit meter
  * @min 5
  * @max 25
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
 

src/modules/commander/gyro_calibration.cpp

@@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 				}
 				
 				if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
-					mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
+					mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
 				}
 			}
 			
@@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
 		}
 		
 		if (poll_errcount > 1000) {
-			mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
 			return calibrate_return_error;
 		}
 	}
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
-			mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
+			mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
 			return calibrate_return_error;
 		}
 
@@ -155,7 +155,7 @@ int do_gyro_calibration(int mavlink_fd)
 	int			res = OK;
 	gyro_worker_data_t	worker_data = {};
 
-	mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
+	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
 	worker_data.mavlink_fd = mavlink_fd;
 	
@@ -179,7 +179,7 @@ int do_gyro_calibration(int mavlink_fd)
 		(void)sprintf(str, "CAL_GYRO%u_ID", s);
 		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
+			mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
 			return ERROR;
 		}
 		
@@ -193,7 +193,7 @@ int do_gyro_calibration(int mavlink_fd)
 			px4_close(fd);
 
 			if (res != OK) {
-				mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
+				mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 				return ERROR;
 			}
 		}
@@ -238,7 +238,7 @@ int do_gyro_calibration(int mavlink_fd)
 			float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
 
 			/* maximum allowable calibration error in radians */
-			const float maxoff = 0.0055f;
+			const float maxoff = 0.01f;
 
 			if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
 			    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
@@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd)
 				px4_close(fd);
 
 				if (res != OK) {
-					mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);
+					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
 				}
 			}
 		}
@@ -325,7 +325,7 @@ int do_gyro_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
 		}
 	}
 
@@ -333,9 +333,9 @@ int do_gyro_calibration(int mavlink_fd)
 	usleep(200000);
 
 	if (res == OK) {
-		mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
 	} else {
-		mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
 	}
 
 	/* give this message enough time to propagate */

src/modules/commander/state_machine_helper.cpp

@@ -97,8 +97,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_IN_AIR_RESTORE",
 };
 
-static bool sensor_feedback_provided = false;
-
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
 			const struct safety_s   *safety,		///< current safety settings
@@ -126,10 +124,20 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		 */
 		int prearm_ret = OK;
 
-		/* only perform the check if we have to */
+		/* only perform the pre-arm check if we have to */
 		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
-			prearm_ret = prearm_check(status, mavlink_fd);
+
+			prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
+		}
+		/* re-run the pre-flight check as long as sensors are failing */
+		if (!status->condition_system_sensors_initialized 
+				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED ||
+				    new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
+             			&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+
+            		prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
+			status->condition_system_sensors_initialized = !prearm_ret;
 		}
 
 		/*
@@ -175,7 +183,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
 						feedback_provided = true;
 						valid_transition = false;
 					}
@@ -200,10 +208,10 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 								feedback_provided = true;
 								valid_transition = false;
 							} else if (status->avionics_power_rail_voltage < 4.9f) {
-								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
 								feedback_provided = true;
 							} else if (status->avionics_power_rail_voltage > 5.4f) {
-								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
 								feedback_provided = true;
 							}
 						}
@@ -247,9 +255,11 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
 			(!status->condition_system_sensors_initialized)) {
-			if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			if ((!status->condition_system_prearm_error_reported && 
+			      status->condition_system_hotplug_timeout) || 
+			     (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
 				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
-				sensor_feedback_provided = true;
+				status->condition_system_prearm_error_reported = true;
 			}
 			feedback_provided = true;
 			valid_transition = false;
@@ -265,8 +275,9 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		/* reset feedback state */
 		if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
+			status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
 			valid_transition) {
-			sensor_feedback_provided = false;
+			status->condition_system_prearm_error_reported = false;
 		}
 
 		/* end of atomic state update */
@@ -384,7 +395,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 		switch (new_state) {
 		case vehicle_status_s::HIL_STATE_OFF:
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
+			mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)");
 			ret = TRANSITION_DENIED;
 			break;
 
@@ -457,7 +468,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 					closedir(d);
 
 					ret = TRANSITION_CHANGED;
-					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+					mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
 
 				} else {
 					/* failed opening dir */
@@ -518,11 +529,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 				}
 
 				ret = TRANSITION_CHANGED;
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+				mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
 #endif
 
 			} else {
-				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed");
 				ret = TRANSITION_DENIED;
 			}
 			break;
@@ -759,24 +770,35 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	return status->nav_state != nav_state_old;
 }
 
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report)
 {
-	/* at this point this equals the preflight check, but might add additional
-	 * quantities later.
+	/* 
 	 */
+	bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
+		status->condition_system_hotplug_timeout);
+	
 	bool checkAirspeed = false;
 	/* 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) {
+	if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
 		checkAirspeed = true;
 	}
 
-	bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
-
-	if (status->usb_connected) {
-		prearm_ok = false;
-		mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+	bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true,
+				checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF),
+				!status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
+		
+	if (!status->cb_usb && status->usb_connected && prearm) {
+		preflight_ok = false;
+		if (reportFailures) {
+			mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+		}
 	}
 
-	return !prearm_ok;
+	/* report once, then set the flag */
+	if (mavlink_fd >= 0 && reportFailures && !preflight_ok) {
+		status->condition_system_prearm_error_reported = true;
+	}
+
+	return !preflight_ok;
 }

src/modules/commander/state_machine_helper.h

@@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
 
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
 
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false);
 
 #endif /* STATE_MACHINE_HELPER_H_ */