Hi,
I have acceleration signal from adxl and ads1256. I send it to Python and it works well, I mean calculations as gRMS, Peak, mm/s, etc. But I want to calculate gRMS and Peak and mm/s with Teensy 4.0. g values seems ok but velocity mm/s is unstable like 0.5 - 2mm/s when sensor at rest. I need filter acceleration 500-10000Hz and velocity 10-1000Hz and have stable value. Any idea how to do it? Thanks.
My code:
I have acceleration signal from adxl and ads1256. I send it to Python and it works well, I mean calculations as gRMS, Peak, mm/s, etc. But I want to calculate gRMS and Peak and mm/s with Teensy 4.0. g values seems ok but velocity mm/s is unstable like 0.5 - 2mm/s when sensor at rest. I need filter acceleration 500-10000Hz and velocity 10-1000Hz and have stable value. Any idea how to do it? Thanks.
My code:
Code:
#include <Arduino.h>
#include <SPI.h>
// PIN assignments
#define CS_PIN 21
#define DRDY_PIN 22
#define RST_PIN 8
// SPI settings
#define SPISPEED 1950000
SPISettings spiSettings(SPISPEED, MSBFIRST, SPI_MODE1);
// ADS1256 registers
#define STATUS_REG 0x00
#define MUX_REG 0x01
#define ADCON_REG (0x02)
#define DRATE_REG 0x03
// ADS1256 commands
#define WAKEUP_CMD 0x00
#define RDATA_CMD 0x01
#define RREG_CMD 0x10
#define WREG_CMD 0x50
#define SELFCAL_CMD 0xF0
#define SYNC_CMD 0xFC
#define STANDBY_CMD 0xFD
#define RESET_CMD 0xFE
// Register values
#define STATUS_REG_VALUE (0x01) // MSB first, Auto-Cal Disabled, Input Buffer Disabled
#define ADCON_REG_VALUE (0x21) // Gain 2, Clock Out Freq = fCLKIN
#define DRATE_REG_VALUE (0xF0) // 30k SPS
#define MUX_REG_VALUE (0x01) // Differential AIN0 AIN1
// Constants
#define VREF (2.5f) // Reference voltage
#define SENSITIVITY_CORRECTION (0.0111f) // Calibrated sensitivity correction
const uint32_t numSamples = 32768; // 32k samples
float *raw_data; // Use dynamic allocation to prevent stack overflow
// High-pass filter parameters
float EMA_a_low = 0.00199; // 10Hz
float EMA_a_high = 0.666; // 500Hz
float EMA_S_low = 0;
float EMA_S_high = 0;
// DRDY flag
volatile bool dataReady = false;
void setup() {
Serial.begin(2000000);
while (!Serial); // Wait for Serial to initialize
setupPins();
resetADC();
configureADC();
attachInterrupt(digitalPinToInterrupt(DRDY_PIN), DRDY_Interrupt, FALLING);
// Allocate memory for raw_data
raw_data = (float *)malloc(numSamples * sizeof(float));
if (raw_data == NULL) {
Serial.println("Memory allocation failed!");
while (1); // Halt if memory allocation fails
}
}
void loop() {
readData();
applyHighPassFilter();
calculateGRMSandPeak();
calculateVelocityRMS();
}
// Interrupt Service Routine for DRDY
void DRDY_Interrupt() {
dataReady = true;
}
// Function to wait for DRDY signal with timeout
bool waitDRDY(uint32_t timeout_ms = 1000) {
uint32_t startTime = millis();
while (!dataReady) {
if (millis() - startTime > timeout_ms) {
return false; // Timeout occurred
}
yield(); // Allow other processes to run
}
dataReady = false;
return true;
}
// Function to read a single sample from the ADC
float READ_ADC() {
int32_t adc_raw = 0;
if (!waitDRDY()) {
Serial.println("DRDY wait timeout!");
return NAN; // Return NaN to indicate an error
}
digitalWriteFast(CS_PIN, LOW);
SPI.beginTransaction(spiSettings);
SPI.transfer(RDATA_CMD);
delayMicroseconds(5); // Small delay to ensure ADC is ready
// Read 24 bits of data
adc_raw = SPI.transfer(0);
adc_raw = (adc_raw << 8) | SPI.transfer(0);
adc_raw = (adc_raw << 8) | SPI.transfer(0);
SPI.endTransaction();
digitalWriteFast(CS_PIN, HIGH);
// Sign extension for 24-bit data
if (adc_raw & 0x800000) {
adc_raw |= 0xFF000000;
}
// Convert to voltage
float voltage = ((float)adc_raw * VREF) / (1 << 23);
voltage /= SENSITIVITY_CORRECTION;
return voltage;
}
// Function to read multiple samples from the ADC
void readData() {
for (uint32_t i = 0; i < numSamples; i++) {
raw_data[i] = READ_ADC();
if (isnan(raw_data[i])) {
Serial.println("Error reading ADC data!");
break;
}
}
}
// Function to apply a high-pass filter to the acceleration data
void applyHighPassFilter() {
for (uint32_t i = 0; i < numSamples; i++) {
float g = raw_data[i];
EMA_S_low = (EMA_a_low * g) + ((1 - EMA_a_low) * EMA_S_low);
EMA_S_high = (EMA_a_high * g) + ((1 - EMA_a_high) * EMA_S_high);
raw_data[i] = EMA_S_high - EMA_S_low;
}
}
// Function to calculate gRMS and Peak
void calculateGRMSandPeak() {
float sum = 0.0;
float peak = -INFINITY;
float mean = 0.0;
// Calculate mean
for (uint32_t i = 0; i < numSamples; i++) {
mean += raw_data[i];
}
mean /= numSamples;
// Remove mean and calculate gRMS and Peak
for (uint32_t i = 0; i < numSamples; i++) {
raw_data[i] -= mean;
sum += raw_data[i] * raw_data[i];
if (fabs(raw_data[i]) > peak) {
peak = fabs(raw_data[i]);
}
}
float gRMS = sqrt(sum / numSamples);
Serial.print("gRMS: ");
Serial.println(gRMS, 5);
Serial.print("Peak: ");
Serial.println(peak, 5);
}
// Function to calculate velocity RMS from acceleration data
void calculateVelocityRMS() {
float *velocity_data = (float *)malloc(numSamples * sizeof(float));
if (velocity_data == NULL) {
Serial.println("Memory allocation for velocity_data failed!");
while (1); // Halt if memory allocation fails
}
float dt = 1.0f / 31470.0f; // Time step based on sampling frequency (SPS_CORRECTION)
// Remove mean from acceleration data before integration
float mean_acc = 0.0;
for (uint32_t i = 0; i < numSamples; i++) {
mean_acc += raw_data[i];
}
mean_acc /= numSamples;
for (uint32_t i = 0; i < numSamples; i++) {
raw_data[i] -= mean_acc;
}
// Initialize velocity
velocity_data[0] = 0.0;
// Integrate raw acceleration to calculate velocity using the basic formula v = v0 + a * dt
for (uint32_t i = 1; i < numSamples; i++) {
velocity_data[i] = velocity_data[i - 1] + raw_data[i] * dt * 9.81f; // Convert to m/s
}
// Calculate mean of velocity
float mean_velocity = 0.0;
for (uint32_t i = 0; i < numSamples; i++) {
mean_velocity += velocity_data[i];
}
mean_velocity /= numSamples;
// Remove mean and calculate RMS of velocity
float sum = 0.0;
for (uint32_t i = 0; i < numSamples; i++) {
velocity_data[i] -= mean_velocity;
sum += velocity_data[i] * velocity_data[i];
}
float velocityRMS = sqrt(sum / numSamples) * 1000.0f; // Convert to mm/s
Serial.print("Velocity RMS (mm/s): ");
Serial.println(velocityRMS, 5);
// Free allocated memory
free(velocity_data);
}
// Function to set up pins
void setupPins() {
pinMode(CS_PIN, OUTPUT);
digitalWriteFast(CS_PIN, HIGH);
pinMode(DRDY_PIN, INPUT);
pinMode(RST_PIN, OUTPUT);
}
// Function to reset the ADC
void resetADC() {
digitalWriteFast(RST_PIN, LOW);
delayMicroseconds(10);
digitalWriteFast(RST_PIN, HIGH);
delay(5);
}
// Function to configure the ADC
void configureADC() {
SPI.begin();
SPI.beginTransaction(spiSettings);
digitalWriteFast(CS_PIN, LOW);
// Send RESET command
SPI.transfer(RESET_CMD);
delay(5);
// Write to registers
writeRegister(STATUS_REG, STATUS_REG_VALUE);
writeRegister(ADCON_REG, ADCON_REG_VALUE);
writeRegister(DRATE_REG, DRATE_REG_VALUE);
writeRegister(MUX_REG, MUX_REG_VALUE);
// Self-calibration
SPI.transfer(SELFCAL_CMD);
digitalWriteFast(CS_PIN, HIGH);
SPI.endTransaction();
// Wait for calibration to complete
waitDRDY(1000);
}
// Function to write to a register
void writeRegister(uint8_t address, uint8_t value) {
digitalWriteFast(CS_PIN, LOW);
SPI.transfer(WREG_CMD | (address & 0x0F));
SPI.transfer(0x00); // Write to a single register
SPI.transfer(value);
digitalWriteFast(CS_PIN, HIGH);
delayMicroseconds(10); // Small delay to ensure the command is processed
}