commander: Improve gyro cal

1 files changed, 3 insertions, 129 deletions

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 47ee9a5e0..e2a7ef743 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -82,7 +82,7 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,
 	};
 
-	struct gyro_scale gyro_scale[max_gyros];
+	struct gyro_scale gyro_scale[max_gyros] = {};
 
 	int res = OK;
 
@@ -196,7 +196,7 @@ int do_gyro_calibration(int mavlink_fd)
 		float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
 
 		/* maximum allowable calibration error in radians */
-		const float maxoff = 0.01f;
+		const float maxoff = 0.002f;
 
 		if (!isfinite(gyro_scale[0].x_offset) ||
 		    !isfinite(gyro_scale[0].y_offset) ||
@@ -204,7 +204,7 @@ int do_gyro_calibration(int mavlink_fd)
 		    fabsf(xdiff) > maxoff ||
 		    fabsf(ydiff) > maxoff ||
 		    fabsf(zdiff) > maxoff) {
-			mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed");
+			mavlink_log_critical(mavlink_fd, "ERROR: Motion during calibration");
 			res = ERROR;
 		}
 	}
@@ -252,132 +252,6 @@ int do_gyro_calibration(int mavlink_fd)
 		}
 	}
 
-#if 0
-	/* beep on offset calibration end */
-	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
-	tune_neutral();
-
-	/* scale calibration */
-	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
-
-	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
-	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
-
-	/* apply new offsets */
-	fd = open(GYRO_DEVICE_PATH, 0);
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
-		warn("WARNING: failed to apply new offsets for gyro");
-	}
-
-	close(fd);
-
-
-	unsigned rotations_count = 30;
-	float gyro_integral = 0.0f;
-	float baseline_integral = 0.0f;
-
-	// XXX change to mag topic
-	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-
-	float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
-
-	if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }
-
-	if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }
-
-
-	uint64_t last_time = hrt_absolute_time();
-	uint64_t start_time = hrt_absolute_time();
-
-	while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
-
-		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
-		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
-		    && (hrt_absolute_time() - start_time > 5 * 1e6)) {
-			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
-			close(sub_sensor_combined);
-			return OK;
-		}
-
-		/* wait blocking for new data */
-		struct pollfd fds[1];
-		fds[0].fd = sub_sensor_combined;
-		fds[0].events = POLLIN;
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-
-			float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
-			last_time = hrt_absolute_time();
-
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-
-			// XXX this is just a proof of concept and needs world / body
-			// transformation and more
-
-			//math::Vector2f magNav(raw.magnetometer_ga);
-
-			// calculate error between estimate and measurement
-			// apply declination correction for true heading as well.
-			//float mag = -atan2f(magNav(1),magNav(0));
-			float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
-
-			if (mag > M_PI_F) { mag -= 2 * M_PI_F; }
-
-			if (mag < -M_PI_F) { mag += 2 * M_PI_F; }
-
-			float diff = mag - mag_last;
-
-			if (diff > M_PI_F) { diff -= 2 * M_PI_F; }
-
-			if (diff < -M_PI_F) { diff += 2 * M_PI_F; }
-
-			baseline_integral += diff;
-			mag_last = mag;
-			// Jump through some timing scale hoops to avoid
-			// 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", (double)baseline_integral, (double)gyro_integral);
-
-			// } 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;
-		}
-	}
-
-	float gyro_scale = baseline_integral / gyro_integral;
-
-	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_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
-		mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
-		close(sub_sensor_gyro);
-		mavlink_log_critical(mavlink_fd, "gyro calibration failed");
-		return ERROR;
-	}
-
-	/* beep on calibration end */
-	mavlink_log_info(mavlink_fd, "gyro scale calibration done");
-	tune_neutral();
-
-	if (res == OK) {
-		/* set scale parameters to new values */
-		if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
-		    || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale))
-		    || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
-			res = ERROR;
-		}
-	}
-
-#endif
-
 	if (res == OK) {
 		/* auto-save to EEPROM */
 		res = param_save_default();