forked from Akcelerometry_drgania_WMT/PI_mikrokontroler
243 lines
7.2 KiB
C++
243 lines
7.2 KiB
C++
#include "ADXL345FreshSPI.h"
|
||
|
||
static const char *TAG_FRESH = "ADXLFRESH";
|
||
|
||
static inline int16_t u8pair_to_i16(uint8_t lo, uint8_t hi) {
|
||
return (int16_t)((hi << 8) | lo);
|
||
}
|
||
|
||
bool ADXL345FreshSPI::begin(SPIClass* s, uint8_t csPin, uint32_t clockHz) {
|
||
spi = s; cs = csPin; spiHz = clockHz;
|
||
pinMode(cs, OUTPUT); digitalWrite(cs, HIGH);
|
||
//spi->begin(); // tymczas
|
||
delay(1);
|
||
|
||
if (!ping()) return false;
|
||
|
||
// 1. Wymuś STANDBY przed jakąkolwiek konfiguracją
|
||
write8(ADXL345_REG_POWER_CTL, 0x00);
|
||
delay(1);
|
||
|
||
// 2. Skonfiguruj format danych (Range i Full_Res)
|
||
if (!setRange(Range::G16, true)) { // było if (!setRange(Range::G2, true)) return false;
|
||
ESP_LOGI(TAG_FRESH, "Range G16 ERROR!");
|
||
return false;
|
||
}
|
||
// 3. Skonfiguruj ODR
|
||
if (!setODR_Hz(100.0f)) return false; // domyślnie 100 Hz
|
||
// 4. Dopiero teraz włącz pomiar
|
||
if (!configurePowerMeasure()) return false;
|
||
showRangeFull();
|
||
return true;
|
||
}
|
||
|
||
|
||
bool ADXL345FreshSPI::ping() {
|
||
uint8_t id=0; if (!read8(ADXL345_REG_DEVID, id)) return false;
|
||
return id == 0xE5;
|
||
}
|
||
|
||
bool ADXL345FreshSPI::configurePowerMeasure() {
|
||
return write8(ADXL345_REG_POWER_CTL, ADXL345_POWER_MEASURE);
|
||
}
|
||
|
||
|
||
bool ADXL345FreshSPI::setRange(Range r, bool fullRes_) {
|
||
fullRes = fullRes_;
|
||
// Wymuś STANDBY (MEASURE=0) – kluczowe dla zmiany RANGE/FULL_RES
|
||
write8(ADXL345_REG_POWER_CTL, 0x00);
|
||
delayMicroseconds(5);
|
||
|
||
uint8_t fmt = 0;
|
||
if (!read8(ADXL345_REG_DATA_FORMAT, fmt)) return false;
|
||
|
||
fmt &= ~ADXL345_DATA_FORMAT_RANGE_MASK; // bity 1:0
|
||
fmt |= (uint8_t)r;
|
||
|
||
if (fullRes) fmt |= ADXL345_DATA_FORMAT_FULL_RES; // bit 3
|
||
else fmt &= ~ADXL345_DATA_FORMAT_FULL_RES;
|
||
|
||
if (!write8(ADXL345_REG_DATA_FORMAT, fmt)) return false;
|
||
|
||
// Kontrola: odczyt po zapisie
|
||
//uint8_t verify = 0;
|
||
//read8(ADXL345_REG_DATA_FORMAT, verify);
|
||
// tu możesz logować verify
|
||
|
||
// Wróć do MEASURE
|
||
write8(ADXL345_REG_POWER_CTL, ADXL345_POWER_MEASURE);
|
||
scale_g_per_lsb = fullRes ? 0.0039f : (1.0f/256.0f) * (2 << (uint8_t)r);
|
||
return true;
|
||
}
|
||
|
||
|
||
uint8_t ADXL345FreshSPI::odrCodeFromHz(float hz) {
|
||
struct { uint8_t code; float f; } map[] = {
|
||
{0x06, 6.25f},{0x07,12.5f},{0x08,25.f},{0x09,50.f},{0x0A,100.f},
|
||
{0x0B,200.f},{0x0C,400.f},{0x0D,800.f},{0x0E,1600.f},{0x0F,3200.f}
|
||
};
|
||
uint8_t best=0x0A; float bestErr=1e9f;
|
||
for (auto &e: map){ float err=fabsf(e.f-hz); if (err<bestErr){bestErr=err; best=e.code;}}
|
||
return best;
|
||
}
|
||
|
||
bool ADXL345FreshSPI::setODR_Hz(float hz) {
|
||
return write8(ADXL345_REG_BW_RATE, odrCodeFromHz(hz));
|
||
}
|
||
|
||
bool ADXL345FreshSPI::enableFIFO(FIFOmode mode, uint8_t triggerLevel) {
|
||
triggerLevel = constrain(triggerLevel, (uint8_t)1, (uint8_t)32);
|
||
uint8_t m = ADXL345_FIFO_BYPASS;
|
||
switch(mode){
|
||
case FIFOmode::BYPASS: m = ADXL345_FIFO_BYPASS; break;
|
||
case FIFOmode::FIFO: m = ADXL345_FIFO_FIFO; break;
|
||
case FIFOmode::STREAM: m = ADXL345_FIFO_STREAM; break;
|
||
case FIFOmode::TRIGGER: m = ADXL345_FIFO_TRIGGER;break;
|
||
}
|
||
return write8(ADXL345_REG_FIFO_CTL, (uint8_t)(m | ((triggerLevel-1) & 0x1F)));
|
||
}
|
||
|
||
bool ADXL345FreshSPI::enableDataReadyInterrupt(bool enable) {
|
||
uint8_t ie=0; if (!read8(ADXL345_REG_INT_ENABLE, ie)) return false;
|
||
if (enable) ie |= ADXL345_DATA_READY_BIT;
|
||
else ie &= ~ADXL345_DATA_READY_BIT;
|
||
return write8(ADXL345_REG_INT_ENABLE, ie);
|
||
}
|
||
|
||
bool ADXL345FreshSPI::available() {
|
||
uint8_t src=0; if (!read8(ADXL345_REG_INT_SOURCE, src)) return false;
|
||
return (src & ADXL345_DATA_READY_BIT) != 0;
|
||
}
|
||
|
||
bool ADXL345FreshSPI::readFresh(SampleI16& out, uint32_t timeout_ms) {
|
||
uint32_t start = millis();
|
||
while (!available()) {
|
||
if ((millis() - start) > timeout_ms) return false;
|
||
delayMicroseconds(200);
|
||
}
|
||
uint8_t buf[6];
|
||
if (!readMulti(ADXL345_REG_DATAX0, buf, 6)) return false;
|
||
out.x = u8pair_to_i16(buf[0], buf[1]);
|
||
out.y = u8pair_to_i16(buf[2], buf[3]);
|
||
out.z = u8pair_to_i16(buf[4], buf[5]);
|
||
out.ts_us = micros();
|
||
return true;
|
||
}
|
||
|
||
bool ADXL345FreshSPI::readFresh(SampleSI& out, uint32_t timeout_ms) {
|
||
SampleI16 raw;
|
||
if (!readFresh(raw, timeout_ms)) return false;
|
||
countsToSI(raw, out);
|
||
return true;
|
||
}
|
||
|
||
size_t ADXL345FreshSPI::readFIFOBurst(SampleI16* buf, size_t maxCount) {
|
||
if (!buf || maxCount==0) return 0;
|
||
uint8_t status=0; if (!read8(ADXL345_REG_FIFO_STATUS, status)) return 0;
|
||
uint8_t entries = status & 0x3F; // 0..32
|
||
size_t n = min<size_t>(entries, maxCount);
|
||
for (size_t i=0;i<n;i++){
|
||
uint8_t d[6];
|
||
if (!readMulti(ADXL345_REG_DATAX0, d, 6)) return i;
|
||
buf[i].x = u8pair_to_i16(d[0], d[1]);
|
||
buf[i].y = u8pair_to_i16(d[2], d[3]);
|
||
buf[i].z = u8pair_to_i16(d[4], d[5]);
|
||
buf[i].ts_us = micros();
|
||
}
|
||
return n;
|
||
}
|
||
|
||
size_t ADXL345FreshSPI::readFIFOBurst(SampleSI* buf, size_t maxCount) {
|
||
if (!buf || maxCount==0) return 0;
|
||
size_t n = 0;
|
||
while (n < maxCount) {
|
||
uint8_t status=0; if (!read8(ADXL345_REG_FIFO_STATUS, status)) break;
|
||
uint8_t entries = status & 0x3F;
|
||
if (entries == 0) break;
|
||
uint8_t d[6];
|
||
uint32_t t = micros();
|
||
if (!readMulti(ADXL345_REG_DATAX0, d, 6)) break;
|
||
SampleI16 s;
|
||
s.x = u8pair_to_i16(d[0], d[1]);
|
||
s.y = u8pair_to_i16(d[2], d[3]);
|
||
s.z = u8pair_to_i16(d[4], d[5]);
|
||
s.ts_us = t;
|
||
countsToSI(s, buf[n]);
|
||
n++;
|
||
}
|
||
return n;
|
||
}
|
||
|
||
void ADXL345FreshSPI::countsToSI(const SampleI16& in, SampleSI& out) {
|
||
out.ax_g = in.x * scale_g_per_lsb;
|
||
out.ay_g = in.y * scale_g_per_lsb;
|
||
out.az_g = in.z * scale_g_per_lsb;
|
||
out.ax_ms2 = out.ax_g * g_ms2;
|
||
out.ay_ms2 = out.ay_g * g_ms2;
|
||
out.az_ms2 = out.az_g * g_ms2;
|
||
out.ts_us = in.ts_us;
|
||
}
|
||
|
||
// ---------- Low-level SPI -----------
|
||
bool ADXL345FreshSPI::write8(uint8_t reg, uint8_t val) {
|
||
spi->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE3));
|
||
spiSelect();
|
||
spi->transfer(reg & 0x3F); // write, single
|
||
spi->transfer(val);
|
||
spiDeselect();
|
||
spi->endTransaction();
|
||
return true;
|
||
}
|
||
|
||
bool ADXL345FreshSPI::read8(uint8_t reg, uint8_t& val) {
|
||
spi->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE3));
|
||
spiSelect();
|
||
spi->transfer(0x80 | (reg & 0x3F)); // read, single
|
||
val = spi->transfer(0x00);
|
||
spiDeselect();
|
||
spi->endTransaction();
|
||
return true;
|
||
}
|
||
|
||
bool ADXL345FreshSPI::readMulti(uint8_t reg, uint8_t *dst, size_t n) {
|
||
if (n==0) return true;
|
||
spi->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE3));
|
||
spiSelect();
|
||
spi->transfer(0xC0 | (reg & 0x3F)); // read, multi (MB=1, R/W=1)
|
||
for (size_t i=0;i<n;i++) dst[i] = spi->transfer(0x00);
|
||
spiDeselect();
|
||
spi->endTransaction();
|
||
return true;
|
||
}
|
||
|
||
void ADXL345FreshSPI::spiSelect() { digitalWrite(cs, LOW); }
|
||
void ADXL345FreshSPI::spiDeselect() { digitalWrite(cs, HIGH); }
|
||
|
||
|
||
uint8_t ADXL345FreshSPI::getADXLRange() {
|
||
uint8_t format = 0;
|
||
if (!read8(0x31, format)) return 255; // albo 0
|
||
switch (format & 0x03) {
|
||
case 0: return 2;
|
||
case 1: return 4;
|
||
case 2: return 8;
|
||
case 3: return 16;
|
||
}
|
||
return 255;
|
||
}
|
||
|
||
|
||
bool ADXL345FreshSPI::getADXLFullRes() {
|
||
uint8_t format = 0;
|
||
if (!read8(0x31, format)) return false;
|
||
//bool ok = read8(0x31, format);
|
||
//ESP_LOGI(TAG_FRESH, "read8 ok=%d DATA_FORMAT=0x%02X", ok, format);
|
||
return (format & 0x08) != 0;
|
||
}
|
||
|
||
void ADXL345FreshSPI::showRangeFull(String txt) {
|
||
uint8_t format = getADXLRange();
|
||
bool full_res = getADXLFullRes();
|
||
ESP_LOGI(TAG_FRESH, "%s ADXL345: RANGE=%dG FULL_RES=%d", txt, format, full_res);
|
||
}
|