1 files changed, 8 insertions, 0 deletions
diff --git a/firmware/os/drivers/st_mag40/st_mag40.c b/firmware/os/drivers/st_mag40/st_mag40.c
index 70242d17..bdc874aa 100644
--- a/firmware/os/drivers/st_mag40/st_mag40.c
+++ b/firmware/os/drivers/st_mag40/st_mag40.c
@@ -215,6 +215,7 @@ struct st_mag40_Task {
     uint8_t subState;
 
     /* sensor flags */
+    uint8_t samplesToDiscard;
     uint32_t rate;
     uint64_t latency;
     bool magOn;
@@ -484,6 +485,7 @@ static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
     mTask.currentODR = st_mag40_regVal[num];
     mTask.rate = rate;
     mTask.latency = latency;
+    mTask.samplesToDiscard = 2;
 
     if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
         mTask.subState = CONFIG_SET_RATE;
@@ -699,6 +701,12 @@ static void parseRawData(uint8_t *raw)
     float xi, yi, zi;
 #endif
 
+	/* Discard samples generated during sensor turn-on time */
+    if (mTask.samplesToDiscard > 0) {
+        mTask.samplesToDiscard--;
+        return;
+    }
+
     /* in uT */
     xs = (float)raw_x * kScale_mag;
     ys = (float)raw_y * kScale_mag;