Merge branch 'release_v1.0.0'

msg/vehicle_status.msg

@@ -91,6 +91,7 @@ uint8 nav_state				# set navigation state machine to specified value
 uint8 arming_state			# current arming state
 uint8 hil_state				# current hil state
 bool failsafe				# true if system is in failsafe state
+bool calibration_enabled		# true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT.
 
 int32 system_type			# system type, inspired by MAVLink's VEHICLE_TYPE enum
 int32 system_id				# system id, inspired by MAVLink's system ID field

src/modules/commander/accelerometer_calibration.cpp

@@ -559,7 +559,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
 
 int do_level_calibration(int mavlink_fd) {
 	const unsigned cal_time = 5;
-	const unsigned cal_hz = 250;
+	const unsigned cal_hz = 100;
 	const unsigned settle_time = 30;
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	struct vehicle_attitude_s att;
@@ -614,6 +614,8 @@ int do_level_calibration(int mavlink_fd) {
 		pitch_mean /= counter;
 	} else {
 		mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
+		success = false;
+		goto out;
 	}
 
 	if (fabsf(roll_mean) > 0.8f ) {
@@ -628,6 +630,7 @@ int do_level_calibration(int mavlink_fd) {
 		success = true;
 	}
 
+out:
 	if (success) {
 		mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
 		return 0;

src/modules/commander/commander.cpp

@@ -1611,7 +1611,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* If in INIT state, try to proceed to STANDBY state */
-		if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
+		if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 			arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
 							     mavlink_fd);
 
@@ -2698,6 +2698,8 @@ void *commander_low_prio_loop(void *arg)
 							false /* fRunPreArmChecks */, mavlink_fd)) {
 						answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 						break;
+					} else {
+						status.calibration_enabled = true;
 					}
 
 					if ((int)(cmd.param1) == 1) {
@@ -2757,12 +2759,14 @@ void *commander_low_prio_loop(void *arg)
 							/* enable RC control input */
 							status.rc_input_blocked = false;
 							mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
-                            calib_ret = OK;
+							calib_ret = OK;
 						}
 						/* this always succeeds */
 						calib_ret = OK;
 					}
 
+					status.calibration_enabled = false;
+
 					if (calib_ret == OK) {
 						tune_positive(true);
 
@@ -2780,7 +2784,7 @@ void *commander_low_prio_loop(void *arg)
 
 						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 
-						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
+						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
 
 					} else {
 						tune_negative(true);

src/modules/commander/state_machine_helper.cpp

@@ -219,23 +219,13 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			valid_transition = true;
 		}
 
-		// Sensors need to be initialized for STANDBY state, except for HIL
-		if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
-			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
-			(!status->condition_system_sensors_initialized)) {
-			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
-			feedback_provided = true;
-			valid_transition = false;
-			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
-		}
-
 		// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
 		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
 
 			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
 				if (status->condition_system_sensors_initialized) {
-					mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+					mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming");
 				} else {
 					mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
 				}
@@ -243,11 +233,24 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 					status->condition_system_sensors_initialized) {
-				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
+				mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete");
 				feedback_provided = true;
 			} else {
-
+				// Silent ignore
+				feedback_provided = true;
 			}
+
+		// Sensors need to be initialized for STANDBY state, except for HIL
+		} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
+			(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 (!fRunPreArmChecks) {
+				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
+			}
+			feedback_provided = true;
+			valid_transition = false;
+			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 		}
 
 		// Finish up the state transition