#include #include #include #include "mma7660_acc.h" #include "stm32l_adc.h" #include "stm32l_rtc.h" #include "parameters.h" #include "stats.h" volatile int g_movement, g_raw_movement; volatile int g_temperature, g_raw_temperature; int sensors_movement() { chSysLock(); int r = g_movement; chSysUnlock(); return r; } int sensors_raw_movement() { chSysLock(); int r = g_raw_movement; chSysUnlock(); return r; } int sensors_temperature() { chSysLock(); int r = g_temperature; chSysUnlock(); return r; } int sensors_raw_temperature() { chSysLock(); int r = g_raw_temperature; chSysUnlock(); return r; } /* We use 16 samples per second and apply filtering over the past * 5 seconds interval. */ #define BUFSIZE (5*16) static int g_samples_x[BUFSIZE]; static int g_samples_y[BUFSIZE]; static int g_samples_z[BUFSIZE]; static int g_bufpos; static int g_samplecount; /* Band-pass filter for 16 Hz; passes approx. 1 Hz - 5 Hz. */ static const int g_bandpass[BUFSIZE] = { 221, -2325, -2736, 3663, 5178, -1492, -2669, 262, -501, -493, 736, 238, -363, -579, -203, 124, 83, 709, 452, 15, -58, -1215, -427, 561, -589, 1256, 2350, -470, -80, 837, -1669, -1613, 1397, 903, -2542, -61, -260,-14154, -8621, 23905, 23905, -8621,-14154, -260, -61, -2542, 903, 1397, -1613, -1669, 837, -80, -470, 2350, 1256, -589, 561, -427, -1215, -58, 15, 452, 709, 83, 124, -203, -579, -363, 238, 736, -493, -501, 262, -2669, -1492, 5178, 3663, -2736, -2325, 221 }; /* Run a FIR filter over a set of samples */ static int fir(const int samples[], int pos, const int filter[], int buflen) { int64_t sum = 0; for (int i = 0; i < buflen; i++) { sum += samples[pos] * filter[i]; pos++; if (pos >= buflen) pos = 0; } return sum / 65536; } /* The results of filtering are kept as a moving average of squares. */ static int g_movement_RMS_average; /* Integer square root */ uint32_t isqrt(uint32_t n) { uint32_t root = 0, bit, trial; bit = (n >= 0x10000) ? 1<<30 : 1<<14; do { trial = root+bit; if (n >= trial) { n -= trial; root = trial+bit; } root >>= 1; bit >>= 2; } while (bit); return root; } /* Take one new accelerometer sample and update estimates. */ static void update_movement() { int x, y, z; if (mma7660_read(&x, &y, &z)) { g_samples_x[g_bufpos] = x; g_samples_y[g_bufpos] = y; g_samples_z[g_bufpos] = z; if (x*x + y*y + z*z < 100 * 100) { stats_add_freefall(1000 / 16); } if (g_samplecount > BUFSIZE) { int f_x = fir(g_samples_x, g_bufpos, g_bandpass, BUFSIZE); int f_y = fir(g_samples_x, g_bufpos, g_bandpass, BUFSIZE); int f_z = fir(g_samples_x, g_bufpos, g_bandpass, BUFSIZE); int total = f_x * f_x + f_y * f_y + f_z * f_z; g_movement_RMS_average = (total + g_movement_RMS_average * (MOVEMENT_DECAY_TIME - 1)) / MOVEMENT_DECAY_TIME; } g_samplecount++; g_bufpos++; if (g_bufpos >= BUFSIZE) g_bufpos = 0; } int rms = isqrt(g_movement_RMS_average); int estimate = rms - 40; // Need something more complex? if (estimate < 0) estimate = 0; if (estimate > 100) estimate = 100; chSysLock(); g_movement = estimate; g_raw_movement = rms; chSysUnlock(); } /* Temperature slope is calculated over the past 15 seconds using * a Savitzky-Golay filter. */ static int g_temperature_samples[15]; static int g_temperature_bufpos; static const int g_temperature_filter[15] = { -1638, -1404, -1170, -936, -702, -468, -234, -0, 234, 468, 702, 936, 1170, 1404, 1638 }; /* Maximum daily average outdoor temperatures by the month of the year. */ static const int g_outdoor_temperatures[12] = { 5, 3, 6, 16, 23, 26, 28, 23, 17, 13, 9, 7 }; /* Take one new temperature sample and update the estimates. */ static void update_temperature() { adc_on(); chThdSleepMilliseconds(10); int temperature = adc_temperature(); adc_off(); // Add new sample to buffer g_temperature_samples[g_temperature_bufpos] = temperature; // Calculate the current derivative (millicelsius / second). int derivative = fir(g_temperature_samples, g_temperature_bufpos, g_temperature_filter, 15); // Calculate the average int average = 0; for (int i = 0; i < 15; i++) average += g_temperature_samples[i]; average /= 15; // Increment the buffer position for next sample g_temperature_bufpos++; if (g_temperature_bufpos >= 15) g_temperature_bufpos = 0; // Estimate the current ambient temperature based on slope int difference = derivative * 1000000 / THERMAL_CONSTANT; int ambient = average + difference; // Then estimate the likelihood of this outdoor temperature at this time // of year. int outdoors_temp = 15000; time_t time; if (get_rtc(&time)) outdoors_temp = g_outdoor_temperatures[time.month - 1] * 1000; // 100% = outdoor temp or lower, 0% = 5°C higher difference = ambient - outdoors_temp; int likelihood = 100 - difference / 50; if (likelihood < 0) likelihood = 0; if (likelihood > 100) likelihood = 100; chSysLock(); g_temperature = likelihood; g_raw_temperature = ambient; chSysUnlock(); } static msg_t sensor_task(void *arg) { chRegSetThreadName("sensor"); memset(&g_samples_x, 0, sizeof(g_samples_x)); memset(&g_samples_y, 0, sizeof(g_samples_y)); memset(&g_samples_z, 0, sizeof(g_samples_z)); g_bufpos = 0; g_movement_RMS_average = 0; g_samplecount = 0; /* Initialize the temperature estimator */ adc_on(); int temp = adc_temperature(); adc_off(); for (int i = 0; i < 15; i++) g_temperature_samples[i] = temp; g_raw_temperature = temp; g_temperature_bufpos = 0; mma7660_poweron(MMA7660_SR_16); // Read accelerometer at 16 Hz and temperature at 1 Hz int temp_count = 16; while (!chThdShouldTerminate()) { mma7660_wait(); update_movement(); temp_count--; if (temp_count == 0) { temp_count = 16; update_temperature(); } } mma7660_poweroff(); return 0; } static Thread *sensorThread = 0; static WORKING_AREA(sensorThread_wa, 256); void sensors_start() { if (sensorThread != NULL) return; chSysLock(); g_movement = 0; g_temperature = 0; chSysUnlock(); sensorThread = chThdCreateStatic(sensorThread_wa, sizeof(sensorThread_wa), NORMALPRIO + 10, sensor_task, NULL); } void sensors_stop() { if (sensorThread) { chThdTerminate(sensorThread); chThdWait(sensorThread); sensorThread = NULL; } }