You need to check that the sensor data is being updated properly. Adding a time of reading would be helpful.
Here is a demo sketch that shows data sharing with an accelerometer. The shared variable dataT is the time in milis() when the data was acquired.
Note the mutex, volatile shared vatiables, and the use of temp variables to limit the time shared data is locked.
// Simple demo of daata sharing.
// Adafruit ADXL335 - 5V ready triple-axis accelerometer.
#include <ChibiOS_AVR.h>
const uint8_t X_PIN = 0;
const uint8_t Y_PIN = 1;
const uint8_t Z_PIN = 2;
// approximate zero g value.
const int dataOffset = 338;
//------------------------------------------------------------------------------
// Shared data, use volatile to insure correct access.
// Mutex for atomic access to data.
MUTEX_DECL(dataMutex);
// Time data was read.
volatile uint32_t dataT;
// Data X value.
volatile int dataX;
// Data Y value.
volatile int dataY;
// Data Z value.
volatile int dataZ;
//------------------------------------------------------------------------------
// Thread 1, high priority to read accelerometer.
// 64 byte stack beyond task switch and interrupt needs.
static WORKING_AREA(waThread1, 64);
static msg_t Thread1(void *arg) {
// Read data every 10 ms.
systime_t wakeTime = chTimeNow();
while (1) {
// Add ticks for 10 ms.
wakeTime += MS2ST(10);
chThdSleepUntil(wakeTime);
// Use temp variables to acquire data.
uint32_t tmpT = millis();
int tmpX = analogRead(X_PIN);
int tmpY = analogRead(Y_PIN);
int tmpZ = analogRead(Z_PIN);
// Lock access to data.
chMtxLock(&dataMutex);
// Copy tmp variables to shared variables.
dataT = tmpT;
dataX = tmpX;
dataY = tmpY;
dataZ = tmpX;
// Unlock data access.
chMtxUnlock();
}
return 0;
}
//------------------------------------------------------------------------------
// thread 2 - print data every second.
// 100 byte stack beyond task switch and interrupt needs
static WORKING_AREA(waThread2, 100);
static msg_t Thread2(void *arg) {
// print count every second
systime_t wakeTime = chTimeNow();
while (1) {
// Sleep for one second.
wakeTime += MS2ST(1000);
chThdSleepUntil(wakeTime);
// Lock access to data.
chMtxLock(&dataMutex);
// Copy shared data to tmp variables.
uint32_t tmpT = dataT;
int tmpX = dataX;
int tmpY = dataY;
int tmpZ = dataZ;
// Unlock data access.
chMtxUnlock();
Serial.print(F("dataAge: "));
Serial.print(millis() - tmpT);
Serial.print(F(" ms, dataX: "));
Serial.print(tmpX -dataOffset);
Serial.print(F(", dataY: "));
Serial.print(tmpY - dataOffset);
Serial.print(F(", dataZ: "));
Serial.println(tmpZ - dataOffset);
}
}
//------------------------------------------------------------------------------
void setup() {
Serial.begin(9600);
// wait for USB Serial
while (!Serial) {}
// read any input
delay(200);
while (Serial.read() >= 0) {}
chBegin(mainThread);
// chBegin never returns, main thread continues with mainThread()
while(1) {}
}
//------------------------------------------------------------------------------
// main thread runs at NORMALPRIO
void mainThread() {
// start blink thread
chThdCreateStatic(waThread1, sizeof(waThread1),
NORMALPRIO + 2, Thread1, NULL);
// start print thread
chThdCreateStatic(waThread2, sizeof(waThread2),
NORMALPRIO + 1, Thread2, NULL);
}
//------------------------------------------------------------------------------
void loop() {
// not used
}
Here is output from the sketch. Note the data is never older than 10 milliseconds.
dataAge: 4 ms, dataX: 0, dataY: 1, dataZ: 100
dataAge: 8 ms, dataX: -1, dataY: 1, dataZ: 99
dataAge: 3 ms, dataX: 4, dataY: -13, dataZ: 104
dataAge: 7 ms, dataX: 83, dataY: -5, dataZ: 183
dataAge: 2 ms, dataX: 90, dataY: -54, dataZ: 190
dataAge: 6 ms, dataX: 19, dataY: 65, dataZ: 119
dataAge: 1 ms, dataX: 112, dataY: 93, dataZ: 212