Restore proper feedback (mavlink and tone) for calibration commands, etc

src/modules/commander/accelerometer_calibration.cpp

@@ -133,7 +133,7 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 int mat_invert3(float src[3][3], float dst[3][3]);
 int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
 
-void do_accel_calibration(int mavlink_fd) {
+int do_accel_calibration(int mavlink_fd) {
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
 
@@ -176,11 +176,11 @@ void do_accel_calibration(int mavlink_fd) {
 		}
 
 		mavlink_log_info(mavlink_fd, "accel calibration done");
-		tune_positive();
+		return OK;
 	} else {
 		/* measurements error */
 		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		tune_negative();
+		return ERROR;
 	}
 
 	/* exit accel calibration mode */

src/modules/commander/accelerometer_calibration.h

@@ -45,6 +45,6 @@
 
 #include <stdint.h>
 
-void do_accel_calibration(int mavlink_fd);
+int do_accel_calibration(int mavlink_fd);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */

src/modules/commander/airspeed_calibration.cpp

@@ -49,7 +49,13 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 
-void do_airspeed_calibration(int mavlink_fd)
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
@@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
-			return;
+			return ERROR;
 		}
 	}
 
@@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd)
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			return ERROR;
 		}
 
 		/* auto-save to EEPROM */
@@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
+			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			return ERROR;
 		}
 
 		//char buf[50];
@@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd)
 		//mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "airspeed calibration done");
 
-		tune_positive();
+		return OK;
 
 	} else {
 		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		return ERROR;
 	}
-
-	close(diff_pres_sub);
 }

src/modules/commander/airspeed_calibration.h

@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_airspeed_calibration(int mavlink_fd);
+int do_airspeed_calibration(int mavlink_fd);
 
-#endif /* AIRSPEED_CALIBRATION_H_ */
+#endif /* AIRSPEED_CALIBRATION_H_ */

src/modules/commander/baro_calibration.cpp

@@ -47,8 +47,14 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 
-void do_baro_calibration(int mavlink_fd)
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_baro_calibration(int mavlink_fd)
 {
 	// TODO implement this
-	return;
+	return ERROR;
 }

src/modules/commander/baro_calibration.h

@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_baro_calibration(int mavlink_fd);
+int do_baro_calibration(int mavlink_fd);
 
-#endif /* BARO_CALIBRATION_H_ */
+#endif /* BARO_CALIBRATION_H_ */

src/modules/commander/commander.cpp

@@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
+		/* try again later */
+		usleep(20000);
+		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+		if (mavlink_fd < 0) {
+			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
+		}
 	}
 
 	/* Main state machine */
@@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 	return res;
 }
 
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+		case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive();
+			break;
+		case VEHICLE_CMD_RESULT_DENIED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+			tune_negative();
+			break;
+		case VEHICLE_CMD_RESULT_FAILED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+			tune_negative();
+			break;
+		case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+			tune_negative();
+			break;
+		case VEHICLE_CMD_RESULT_UNSUPPORTED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+			tune_negative();
+			break;
+		default:
+			break;
+	}
+}
+
 void *commander_low_prio_loop(void *arg)
 {
 	/* Set thread name */
@@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg)
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM)
 			continue;
 
-		/* result of the command */
-		uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
 		/* only handle low-priority commands here */
 		switch (cmd.command) {
 
 		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-				if (is_safe(&status, &safety, &armed)) {
-
-					if (((int)(cmd.param1)) == 1) {
-						/* reboot */
-						systemreset(false);
-					} else if (((int)(cmd.param1)) == 3) {
-						/* reboot to bootloader */
-						systemreset(true);
-					} else {
-						result = VEHICLE_CMD_RESULT_DENIED;
-					}
-				
+			if (is_safe(&status, &safety, &armed)) {
+
+				if (((int)(cmd.param1)) == 1) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot */
+					systemreset(false);
+				} else if (((int)(cmd.param1)) == 3) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot to bootloader */
+					systemreset(true);
 				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 				}
-			break;
 
-		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+			} else {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+			}
+		break;
 
-				/* try to go to INIT/PREFLIGHT arming state */
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
-					result = VEHICLE_CMD_RESULT_DENIED;
-					break;
-				}
+			int calib_ret = ERROR;
 
-				if ((int)(cmd.param1) == 1) {
-					/* gyro calibration */
-					do_gyro_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			/* try to go to INIT/PREFLIGHT arming state */
 
-				} else if ((int)(cmd.param2) == 1) {
-					/* magnetometer calibration */
-					do_mag_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			// XXX disable interrupts in arming_state_transition
+			if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+				break;
+			}
 
-				} else if ((int)(cmd.param3) == 1) {
-					/* zero-altitude pressure calibration */
-					result = VEHICLE_CMD_RESULT_DENIED;
-					
-				} else if ((int)(cmd.param4) == 1) {
-					/* RC calibration */
-					result = VEHICLE_CMD_RESULT_DENIED;
-
-				} else if ((int)(cmd.param5) == 1) {
-					/* accelerometer calibration */
-					do_accel_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if ((int)(cmd.param1) == 1) {
+				/* gyro calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_gyro_calibration(mavlink_fd);
+
+			} else if ((int)(cmd.param2) == 1) {
+				/* magnetometer calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_mag_calibration(mavlink_fd);
+
+			} else if ((int)(cmd.param3) == 1) {
+				/* zero-altitude pressure calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+			} else if ((int)(cmd.param4) == 1) {
+				/* RC calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+			} else if ((int)(cmd.param5) == 1) {
+				/* accelerometer calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_accel_calibration(mavlink_fd);
+
+			} else if ((int)(cmd.param6) == 1) {
+				/* airspeed calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_airspeed_calibration(mavlink_fd);
+			}
 
-				} else if ((int)(cmd.param6) == 1) {
-					/* airspeed calibration */
-					do_airspeed_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-				}
+			if (calib_ret == OK)
+				tune_positive();
+			else
+				tune_negative();
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
-				break;
-			}
+			break;
+		}
 
 		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-				if (((int)(cmd.param1)) == 0) {
-					if (0 == param_load_default()) {
-						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if (((int)(cmd.param1)) == 0) {
+				if (0 == param_load_default()) {
+					mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
-						tune_error();
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+				}
 
-				} else if (((int)(cmd.param1)) == 1) {
-					if (0 == param_save_default()) {
-						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else if (((int)(cmd.param1)) == 1) {
+				if (0 == param_save_default()) {
+					mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
-						tune_error();
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 				}
-
-				break;
 			}
-
-		default:
 			break;
 		}
 
-		/* supported command handling stop */
-		if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-			tune_positive();
-
-		} else {
-			tune_negative();
-
-			if (result == VEHICLE_CMD_RESULT_DENIED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
-
-			} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
-
-			} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
-
-			} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
-			}
+		default:
+			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			break;
 		}
 
 		/* send any requested ACKs */

src/modules/commander/gyro_calibration.cpp

@@ -50,8 +50,13 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-void do_gyro_calibration(int mavlink_fd)
+int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
 
@@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-			return;
+			return ERROR;
 		}
 	}
 
@@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warnx("WARNING: auto-save of params to storage failed");
 			mavlink_log_critical(mavlink_fd, "gyro store failed");
-			// XXX negative tune
-			return;
+			return ERROR;
 		}
 
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
-		tune_positive();
+		tune_neutral();
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
-		return;
+		return ERROR;
 	}
 
 
@@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd)
 			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
 			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
 			mavlink_log_info(mavlink_fd, "gyro calibration done");
-			tune_positive();
-			return;
+			return OK;
 		}
 
 		/* wait blocking for new data */
@@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd)
 			// operating near the 1e6/1e8 max sane resolution of float.
 			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
 
-			warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
+//			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
 
 		// } else if (poll_ret == 0) {
 		// 	/* any poll failure for 1s is a reason to abort */
@@ -232,8 +235,8 @@ void do_gyro_calibration(int mavlink_fd)
 
 	float gyro_scale = baseline_integral / gyro_integral;
 	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
-	warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
-	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
+	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 
 
 	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
@@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd)
 		// mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
-		tune_positive();
 		/* third beep by cal end routine */
-
+		return OK;
 	} else {
 		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		return ERROR;
 	}
-
-	close(sub_sensor_combined);
 }

src/modules/commander/gyro_calibration.h

@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_gyro_calibration(int mavlink_fd);
+int do_gyro_calibration(int mavlink_fd);
 
-#endif /* GYRO_CALIBRATION_H_ */
+#endif /* GYRO_CALIBRATION_H_ */

src/modules/commander/mag_calibration.cpp

@@ -53,8 +53,13 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-void do_mag_calibration(int mavlink_fd)
+int do_mag_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
 
@@ -113,7 +118,7 @@ void do_mag_calibration(int mavlink_fd)
 		warnx("mag cal failed: out of memory");
 		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
 		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return;
+		return ERROR;
 	}
 
 	while (hrt_absolute_time() < calibration_deadline &&
@@ -252,6 +257,7 @@ void do_mag_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
 			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			return ERROR;
 		}
 
 		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
@@ -269,12 +275,11 @@ void do_mag_calibration(int mavlink_fd)
 
 		mavlink_log_info(mavlink_fd, "mag calibration done");
 
-		tune_positive();
+		return OK;
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+		return ERROR;
 	}
-
-	close(sub_mag);
-}
+}

src/modules/commander/mag_calibration.h

@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_mag_calibration(int mavlink_fd);
+int do_mag_calibration(int mavlink_fd);
 
-#endif /* MAG_CALIBRATION_H_ */
+#endif /* MAG_CALIBRATION_H_ */

src/modules/commander/rc_calibration.cpp

@@ -46,8 +46,13 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-void do_rc_calibration(int mavlink_fd)
+int do_rc_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "trim calibration starting");
 
@@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd)
 
 	if (save_ret != 0) {
 		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		return ERROR;
 	}
 
-	tune_positive();
-
 	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
+	return OK;
+}

src/modules/commander/rc_calibration.h

@@ -41,6 +41,6 @@
 
 #include <stdint.h>
 
-void do_rc_calibration(int mavlink_fd);
+int do_rc_calibration(int mavlink_fd);
 
-#endif /* RC_CALIBRATION_H_ */
+#endif /* RC_CALIBRATION_H_ */