AP_Compass: HMC5843: use common method to accumulate samples

amitendra-git · Oct 15, 2018 · c0bccda · c0bccda
1 parent 3295163
commit c0bccda
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 58 deletions.
diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
@@ -233,47 +233,19 @@ void AP_Compass_HMC5843::_timer()
         return;
     }
 
-    // the _mag_N values are in the range -2048 to 2047, so we can
-    // accumulate up to 15 of them in an int16_t. Let's make it 14
-    // for ease of calculation. We expect to do reads at 10Hz, and
-    // we get new data at most 75Hz, so we don't expect to
-    // accumulate more than 8 before a read
     // get raw_field - sensor frame, uncorrected
     Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
     raw_field *= _gain_scale;
-    
+
     // rotate to the desired orientation
     if (is_external(_compass_instance)) {
         raw_field.rotate(ROTATION_YAW_90);
     }
 
-    // rotate raw_field from sensor frame to body frame
-    rotate_field(raw_field, _compass_instance);
-
-    // publish raw_field (uncorrected point sample) for calibration use
-    publish_raw_field(raw_field, _compass_instance);
-
-    // correct raw_field for known errors
-    correct_field(raw_field, _compass_instance);
-
-    if (!field_ok(raw_field)) {
-        return;
-    }
-
-    if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return; 
-    }
-    _mag_x_accum += raw_field.x;
-    _mag_y_accum += raw_field.y;
-    _mag_z_accum += raw_field.z;
-    _accum_count++;
-    if (_accum_count == 14) {
-        _mag_x_accum /= 2;
-        _mag_y_accum /= 2;
-        _mag_z_accum /= 2;
-        _accum_count = 7;
-    }
-    _sem->give();
+    // We expect to do reads at 10Hz, and  we get new data at most 75Hz, so we
+    // don't expect to accumulate more than 8 before a read; let's make it
+    // 14 to give more room for the initialization phase
+    accumulate_sample(raw_field, _compass_instance, 14);
 }
 
 /*
@@ -291,26 +263,7 @@ void AP_Compass_HMC5843::read()
         return;
     }
 
-    if (!_sem->take_nonblocking()) {
-        return;
-    }
-
-    if (_accum_count == 0) {
-        _sem->give();
-        return;
-    }
-
-    Vector3f field(_mag_x_accum * _scaling[0],
-                   _mag_y_accum * _scaling[1],
-                   _mag_z_accum * _scaling[2]);
-    field /= _accum_count;
-
-    _accum_count = 0;
-    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
-
-    _sem->give();
-
-    publish_filtered_field(field, _compass_instance);
+    drain_accumulated_samples(_compass_instance, &_scaling);
 }
 
 bool AP_Compass_HMC5843::_setup_sampling_mode()

diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h
@@ -47,16 +47,12 @@ class AP_Compass_HMC5843 : public AP_Compass_Backend
 
     AP_HMC5843_BusDriver *_bus;
 
-    float _scaling[3];
+    Vector3f _scaling;
     float _gain_scale;
 
     int16_t _mag_x;
     int16_t _mag_y;
     int16_t _mag_z;
-    int16_t _mag_x_accum;
-    int16_t _mag_y_accum;
-    int16_t _mag_z_accum;
-    uint8_t _accum_count;
 
     uint8_t _compass_instance;