237 lines
7.0 KiB
C
237 lines
7.0 KiB
C
//
|
||
// Created by cfif on 16.09.22.
|
||
//
|
||
#include "Accel_QMA6100P.h"
|
||
#include <math.h>
|
||
#include "SystemDelayInterface.h"
|
||
//#include "AsciiStringAssmeblingUtils.h"
|
||
|
||
// Return If Not Ok
|
||
#define QMA6100P_RINO(FUNC){bool fres = FUNC; if(!fres){return false;}}
|
||
|
||
|
||
static bool vAccelWriteReg(tAccelQma6100P *env, uint8_t reg, uint8_t value, uint32_t timeout) {
|
||
uint16_t back;
|
||
|
||
uint16_t adr_reg = reg;
|
||
uint16_t value_reg = value;
|
||
|
||
QMA6100P_RINO(SpiPortChipSelect(env->port, timeout))
|
||
|
||
QMA6100P_RINO(SpiPortTransmit(env->port, &adr_reg, timeout))
|
||
QMA6100P_RINO(SpiPortTransmit(env->port, &value_reg, timeout))
|
||
|
||
QMA6100P_RINO(SpiPortChipRelease(env->port, timeout))
|
||
SystemDelayMs(50);
|
||
return true;
|
||
}
|
||
|
||
|
||
static bool vAccelReadReg(tAccelQma6100P *env, uint8_t reg, uint8_t *data, uint32_t timeout) {
|
||
uint16_t back;
|
||
uint16_t adr_reg = reg | 0x80;
|
||
|
||
QMA6100P_RINO(SpiPortChipSelect(env->port, timeout))
|
||
|
||
QMA6100P_RINO(SpiPortTransmit(env->port, &adr_reg, timeout))
|
||
QMA6100P_RINO(SpiPortReceive(env->port, &back, timeout))
|
||
|
||
QMA6100P_RINO(SpiPortChipRelease(env->port, timeout))
|
||
|
||
*data = back;
|
||
|
||
return true;
|
||
}
|
||
|
||
void AccelQma6100P_Init(tAccelQma6100P *env, tSpiPortIO *accelIO) {
|
||
env->port = accelIO;
|
||
}
|
||
|
||
typedef enum {
|
||
QMA_6100P_MODE_STANDBY = 0,
|
||
QMA_6100P_MODE_WAKE = 1,
|
||
} eAccelQma6100P_Mode;
|
||
|
||
//void AccelQma6100P_SetMode(tAccelQma6100P *env, eAccelQma6100P_Mode mode) {
|
||
// uint8_t reg;
|
||
// uint16_t err;
|
||
//
|
||
// reg = vAccelReadReg(env, 0x11, &err, 1);
|
||
//
|
||
// if (mode == QMA_6100P_MODE_WAKE)
|
||
// reg = (reg & 0x7f) | 0x80;
|
||
// else if (mode == QMA_6100P_MODE_STANDBY)
|
||
// reg = (reg & 0x7f);
|
||
// else {
|
||
// return;
|
||
// }
|
||
//
|
||
// vAccelWriteReg(env, 0x11, reg, QMA6100P_TIMEOUT);
|
||
//}
|
||
|
||
bool AccelQma6100P_ConfigureDefaults(tAccelQma6100P *env) {
|
||
|
||
// // Сброс
|
||
// vAccelWriteReg(env, QMA6100P_REG_RESET, QMA6100P_RESET, QMA6100P_TIMEOUT);
|
||
// SystemDelayMs(10);
|
||
// vAccelWriteReg(env, QMA6100P_REG_RESET, 0, QMA6100P_TIMEOUT);
|
||
// SystemDelayMs(10);
|
||
//
|
||
// // Перевод микросхемы в активное состояние
|
||
// vAccelWriteReg(env,
|
||
// QMA6100P_REG_PM,
|
||
// QMA6100P_ENABLE_ACTIVE_MODE | QMA6100P_CIC_MULT8, QMA6100P_TIMEOUT
|
||
// );
|
||
//
|
||
// // Сброс значения регистров по умолчанию
|
||
// SystemDelayMs(10);
|
||
// vAccelWriteReg(env, QMA6100P_REG_INTERNAL, QMA6100P_DEFAULT, QMA6100P_TIMEOUT);
|
||
// SystemDelayMs(10);
|
||
//
|
||
// // Перевод микросхемы в активное состояние
|
||
// vAccelWriteReg(env, QMA6100P_REG_PM, QMA6100P_ENABLE_ACTIVE_MODE, QMA6100P_TIMEOUT);
|
||
//
|
||
// // Установка полной шкалы измерения
|
||
// vAccelWriteReg(env, QMA6100P_REG_FSR, QMA6100P_FULL_SCALE_RAGE, QMA6100P_TIMEOUT);
|
||
//
|
||
// // Включение прерывания по готовности данных
|
||
// vAccelWriteReg(env, QMA6100P_REG_INT_EN1, QMA6100P_DATA_READY_ENABLE_INT1, QMA6100P_TIMEOUT);
|
||
// vAccelWriteReg(env, QMA6100P_REG_INT_MAP1, QMA6100P_ENABLE_INT1, QMA6100P_TIMEOUT);
|
||
//
|
||
//
|
||
// vAccelWriteReg(env, QMA6100P_REG_INT_CFG, QMA6100P_DEFAULT_INT_CFG_VALUE, QMA6100P_TIMEOUT);
|
||
// vAccelWriteReg(env, QMA6100P_REG_MOT_CONF3, QMA6100P_DEFAULT_MOT_CONF3_VALUE, QMA6100P_TIMEOUT);
|
||
|
||
|
||
|
||
vAccelWriteReg(env, QMA6100P_REG_RESET, QMA6100P_RESET, QMA6100P_TIMEOUT);
|
||
SystemDelayMs(50);
|
||
vAccelWriteReg(env, QMA6100P_REG_RESET, 0, QMA6100P_TIMEOUT);
|
||
SystemDelayMs(55);
|
||
//
|
||
//
|
||
|
||
vAccelWriteReg(env, 0x4A, 0x20, QMA6100P_TIMEOUT);//2w spi
|
||
vAccelWriteReg(env, 0x56, 0x01, QMA6100P_TIMEOUT);//2w spi
|
||
vAccelWriteReg(env, 0x5F, 0x80, QMA6100P_TIMEOUT);//2w spi
|
||
SystemDelayMs(1);
|
||
vAccelWriteReg(env, 0x5F, 0x0, QMA6100P_TIMEOUT);//2w spi
|
||
|
||
// vAccelWriteReg(env, 0x11, 0x4, QMA6100P_TIMEOUT);//2w spi
|
||
//
|
||
//
|
||
// uint8_t id;
|
||
// vAccelReadReg(env, 0x00, &id, QMA6100P_TIMEOUT);
|
||
//
|
||
// bool is_qma = (id & 0xF0) == 90;
|
||
|
||
|
||
vAccelWriteReg(env, 0x20, 0x05, QMA6100P_TIMEOUT);//2w spi
|
||
vAccelWriteReg(env, 0x11, 0x84, QMA6100P_TIMEOUT);//mode active, mclk 51khz
|
||
|
||
vAccelWriteReg(env, 0x0F, 0x01, QMA6100P_TIMEOUT);//hpf,2g (244ug/LSB)
|
||
AccelQma6100P_SetRange(env, ACCEL_QMA6100P_RANGE_16G);
|
||
|
||
vAccelWriteReg(env, 0x21, 0x61, QMA6100P_TIMEOUT);//int, disable i2c
|
||
|
||
vAccelWriteReg(env, 0x10, (1 << 6) | (1 << 5), QMA6100P_TIMEOUT);//bandwidth settings
|
||
|
||
vAccelWriteReg(env, 0x2C, 0x01, QMA6100P_TIMEOUT);
|
||
vAccelWriteReg(env, 0x2E, 0x03, QMA6100P_TIMEOUT);//motion interrupt
|
||
vAccelWriteReg(env, 0x2F, 0x40, QMA6100P_TIMEOUT);
|
||
|
||
vAccelWriteReg(env, 0x18, 0x07, QMA6100P_TIMEOUT);//enable interrupt
|
||
vAccelWriteReg(env, 0x1A, 0x01, QMA6100P_TIMEOUT);//interrupt map
|
||
|
||
// vAccelWriteReg(env, 0x09, 0x01, QMA6100P_TIMEOUT);
|
||
|
||
|
||
// uint16_t err = 0x00;
|
||
// uint8_t res = vAccelReadReg(env, 0x00, &err, QMA6100P_TIMEOUT);
|
||
//
|
||
//
|
||
// vAccelWriteReg(env, 0x20, 0x05, QMA6100P_TIMEOUT);
|
||
//
|
||
//
|
||
//// vAccelWriteReg(env, 0x09, 0x01, QMA6100P_TIMEOUT);
|
||
|
||
|
||
|
||
//todo return on error, сделать вывод с ошибкой (false) в случае сбоя при записи конфигурации
|
||
SystemDelayMs(200);
|
||
return true;
|
||
}
|
||
|
||
|
||
bool AccelQma6100P_SetRange(tAccelQma6100P *env, eAccelQma6100pRanges value) {
|
||
uint8_t reg;
|
||
QMA6100P_RINO(vAccelReadReg(env, QMA6100P_REG_FSR, ®, QMA6100P_TIMEOUT));
|
||
|
||
uint8_t regValue;
|
||
|
||
#define V2R(T) case ACCEL_QMA6100P_RANGE_##T: regValue = ACCEL_QMA6100P_RANGE_REG_##T; break;
|
||
|
||
switch (value) {
|
||
V2R(2G)
|
||
V2R(4G)
|
||
V2R(8G)
|
||
V2R(16G)
|
||
V2R(32G)
|
||
default:
|
||
return false;
|
||
}
|
||
|
||
#undef V2R
|
||
|
||
reg = (reg & 0b0000) | regValue;
|
||
|
||
QMA6100P_RINO(vAccelWriteReg(env, QMA6100P_REG_FSR, reg, QMA6100P_TIMEOUT));
|
||
|
||
env->range = value;
|
||
|
||
return true;
|
||
}
|
||
|
||
bool AccelQma6100P_ReadOne(tAccelQma6100P *env, uint8_t firstReg, float *value) {
|
||
|
||
uint8_t regLow;
|
||
uint8_t regHigh;
|
||
|
||
QMA6100P_RINO(vAccelReadReg(env, firstReg, ®Low, QMA6100P_TIMEOUT));
|
||
QMA6100P_RINO(vAccelReadReg(env, firstReg + 1, ®High, QMA6100P_TIMEOUT));
|
||
|
||
uint16_t rawRegs = regHigh;
|
||
rawRegs <<= 8;
|
||
rawRegs |= regLow;
|
||
|
||
// uint16_t rawRegs;
|
||
// AccelReadAutoIncAdrReg(env->port, firstReg, (uint8_t *)&rawRegs, 2, QMA6100P_TIMEOUT);
|
||
|
||
int16_t rowValue = ((int16_t) rawRegs) >> 2;
|
||
|
||
int32_t valueMicroG = rowValue * env->range;
|
||
|
||
if (firstReg == QMA6100P_REG_ACC_X_L)
|
||
env->xyz.x = valueMicroG;
|
||
|
||
if (firstReg == QMA6100P_REG_ACC_Y_L)
|
||
env->xyz.y = valueMicroG;
|
||
|
||
if (firstReg == QMA6100P_REG_ACC_Z_L)
|
||
env->xyz.z = valueMicroG;
|
||
|
||
*value = roundf(valueMicroG) / 1000000.f;
|
||
|
||
return true;
|
||
}
|
||
|
||
bool AccelQma6100P_Read(tAccelQma6100P *env, vector3 *value) {
|
||
|
||
QMA6100P_RINO(AccelQma6100P_ReadOne(env, QMA6100P_REG_ACC_X_L, &value->x));
|
||
QMA6100P_RINO(AccelQma6100P_ReadOne(env, QMA6100P_REG_ACC_Y_L, &value->y));
|
||
QMA6100P_RINO(AccelQma6100P_ReadOne(env, QMA6100P_REG_ACC_Z_L, &value->z));
|
||
|
||
return true;
|
||
}
|
||
|
||
#undef QMA6100P_RINO |