From dccdc977d5be4f957bcaea036b66d0391b29fd2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Aug 2013 17:56:32 +0200 Subject: Made accel / gyro self tests aware of offsets and scales, added support to config command to call these --- src/modules/commander/accelerometer_calibration.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 6a304896a..fbb73d997 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -226,6 +226,12 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float if (orient < 0) return ERROR; + if (data_collected[orient]) { + sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + continue; + } + sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); @@ -380,6 +386,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + int errcount = 0; + while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); if (poll_ret == 1) { @@ -389,8 +397,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp accel_sum[i] += sensor.accelerometer_m_s2[i]; count++; } else { - return ERROR; + errcount++; + continue; } + + if (errcount > samples_num / 10) + return ERROR; } for (int i = 0; i < 3; i++) { -- cgit v1.2.3