int file_no = 0;
#endif
+/* We'll have multiple calibration levels
+* so that we can provide an estimation as fast as possible
+*/
+static const float min_diffs[CAL_STEPS] = { 0.2, 0.4, 0.6, 1.0 };
+static const float max_sqr_errs[CAL_STEPS] = { 10.0, 8.0, 5.0, 3.5 };
+static const unsigned int lookback_counts[CAL_STEPS] = { 3, 4, 5, 6 };
/* reset calibration algorithm */
static void reset_sample (struct compass_cal* data)
return 1;
}
+static void scale_event (struct sensors_event_t* event)
+{
+ float sqr_norm = 0;
+ float sanity_norm = 0;
+ float scale = 1;
+
+ sqr_norm = (event->magnetic.x * event->magnetic.x +
+ event->magnetic.y * event->magnetic.y +
+ event->magnetic.z * event->magnetic.z);
+
+ sanity_norm = (sqr_norm < MAGNETIC_LOW) ? MAGNETIC_LOW : sanity_norm;
+ sanity_norm = (sqr_norm > MAGNETIC_HIGH) ? MAGNETIC_HIGH : sanity_norm;
+
+ if (sanity_norm && sqr_norm) {
+ scale = sanity_norm / sqr_norm;
+ scale = sqrt(scale);
+ event->magnetic.x = event->magnetic.x * scale;
+ event->magnetic.y = event->magnetic.y * scale;
+ event->magnetic.z = event->magnetic.z * scale;
+
+ }
+}
+
static void compass_compute_cal (struct sensors_event_t* event, struct sensor_info_t* info)
{
struct compass_cal* cal_data = (struct compass_cal*) info->cal_data;
+ double result[3][1], raw[3][1], diff[3][1];
if (!info->cal_level || cal_data == NULL)
return;
- double result[3][1], raw[3][1], diff[3][1];
-
raw[0][0] = event->magnetic.x;
raw[1][0] = event->magnetic.y;
raw[2][0] = event->magnetic.z;
event->magnetic.x = event->data[0] = result[0][0];
event->magnetic.y = event->data[1] = result[1][0];
event->magnetic.z = event->data[2] = result[2][0];
+
+ scale_event(event);
}
switch (cal_level) {
case 0:
+ scale_event(event);
event->magnetic.status = SENSOR_STATUS_UNRELIABLE;
break;