Commander: Preflight check reporting cleanup, add USB breaker

src/modules/commander/PreflightCheck.cpp

@@ -206,7 +206,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 				goto out;
 			}
 		} else {
-			if(report_fail) 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;
@@ -229,9 +231,11 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	if (fd < 0) {
 		if (!optional) {
-			if(report_fail) 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;
 	}
@@ -239,8 +243,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
 
 	if (ret) {
-		if(report_fail) 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;
 	}
@@ -248,8 +254,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 	ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
 
 	if (ret != OK) {
-		if(report_fail) 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;
 	}
@@ -269,9 +277,11 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
 
 	if (fd < 0) {
 		if (!optional) {
-			if(report_fail) 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;
 	}
@@ -304,13 +314,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
 
 	if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
 	    (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
-		if(report_fail) 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) {
-		if(report_fail) 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
 	}
 
@@ -342,7 +356,9 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
 
 	//Report failure to detect module
 	if (!success) {
-		if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+		if (report_fail) {
+			mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
+		}
 	}
 
 	px4_close(gpsSub);
@@ -376,7 +392,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) {
-			if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary compass not found");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
+			}
 			failed = true;
 		}
 	}
@@ -403,7 +421,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) {
-			if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
+			}
 			failed = true;
 		}
 	}
@@ -430,7 +450,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) {
-			if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary gyro not found");
+			if (reportFailures) {
+				mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
+			}
 			failed = true;
 		}
 	}
@@ -458,7 +480,9 @@ 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) {
-			if(reportFailures) 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;
 		}
 	}
@@ -473,7 +497,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
 		if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
-			if(reportFailures) mavlink_log_critical(mavlink_fd, "RC calibration check failed");
+			if (reportFailures) {
+				mavlink_log_critical(mavlink_fd, "RC calibration check failed");
+			}
 			failed = true;
 		}
 	}

src/modules/commander/commander.cpp

@@ -390,15 +390,20 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "arm")) {
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
-		arm_disarm(true, mavlink_fd_local, "command line");
+		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;
 	}
@@ -2513,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 =

src/modules/commander/state_machine_helper.cpp

@@ -177,7 +177,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;
 					}
@@ -202,10 +202,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;
 							}
 						}
@@ -392,7 +392,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;
 
@@ -465,7 +465,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 */
@@ -502,11 +502,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,9 +759,9 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd,
 
 	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->usb_connected && prearm) {
+	if (!status->cb_usb && status->usb_connected && prearm) {
 		preflight_ok = false;
-		if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+		if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
 	}
 
 	return !preflight_ok;