Init

2024-12-04 13:10:49 +03:00 · 2024-12-04 13:10:49 +03:00 · 55bcec9475
commit 55bcec9475
3 changed files with 330 additions and 0 deletions
--- a/Inc/Accel_QMA6100P.h
+++ b/Inc/Accel_QMA6100P.h
@ -0,0 +1,80 @@
+//
+// Created by cfif on 28.09.22.
+//
+
+#ifndef ACCEL_QMA6100P_H
+#define ACCEL_QMA6100P_H
+
+#include "SpiPort.h"
+#include "vector.h"
+
+#define QMA6100P_TIMEOUT  10
+#define QMA6100P_RESET 0xB6
+#define QMA6100P_ENABLE_ACTIVE_MODE 0b10000000
+#define QMA6100P_CIC_MULT8 0b00110000
+#define QMA6100P_ENABLE_INT1 0b10000
+#define QMA6100P_DATA_READY_ENABLE_INT1 0b10000
+#define QMA6100P_FULL_SCALE_RAGE 0b0001
+#define QMA6100P_DEFAULT 0b1000
+#define QMA6100P_DEFAULT_INT_CFG_VALUE 0b01001100
+#define QMA6100P_DEFAULT_MOT_CONF3_VALUE 0b01000000
+
+
+#define QMA6100P_REG_FSR 0x0F
+#define QMA6100P_REG_BW 0x10
+#define QMA6100P_REG_PM 0x11
+#define QMA6100P_REG_INT_MAP1 0x1A
+#define QMA6100P_REG_INT_EN1 0x17
+#define QMA6100P_REG_INT_CFG 0x21
+#define QMA6100P_REG_MOT_CONF3 0x2F
+#define QMA6100P_REG_RESET 0x36
+#define QMA6100P_REG_INTERNAL 0x33
+
+
+#define QMA6100P_REG_ACC_X_L 0x01
+#define QMA6100P_REG_ACC_X_H 0x02
+#define QMA6100P_REG_ACC_Y_L 0x03
+#define QMA6100P_REG_ACC_Y_H 0x04
+#define QMA6100P_REG_ACC_Z_L 0x05
+#define QMA6100P_REG_ACC_Z_H 0x06
+
+
+//Значения это LSB в микро g
+//Values is LSB in micro g
+typedef enum {
+    ACCEL_QMA6100P_RANGE_2G = 244,
+    ACCEL_QMA6100P_RANGE_4G = 488,
+    ACCEL_QMA6100P_RANGE_8G = 977,
+    ACCEL_QMA6100P_RANGE_16G = 1950,
+    ACCEL_QMA6100P_RANGE_32G = 3910
+} eAccelQma6100pRanges;
+
+typedef enum {
+    ACCEL_QMA6100P_RANGE_REG_2G = 0b0001,
+    ACCEL_QMA6100P_RANGE_REG_4G = 0b0010,
+    ACCEL_QMA6100P_RANGE_REG_8G = 0b0100,
+    ACCEL_QMA6100P_RANGE_REG_16G = 0b1000,
+    ACCEL_QMA6100P_RANGE_REG_32G = 0b1111,
+} eAccelQma6100pRangesReg;
+
+
+
+typedef struct {
+    tSpiPortIO *port;
+
+    //нельзя задавать ниже описанные значения напрямую
+    //только через setter
+    //private
+    eAccelQma6100pRanges range;
+} tAccelQma6100P;
+
+void AccelQma6100P_Init(tAccelQma6100P *env, tSpiPortIO *accelIO);
+
+bool AccelQma6100P_ConfigureDefaults(tAccelQma6100P *env);
+
+bool AccelQma6100P_SetRange(tAccelQma6100P *env, eAccelQma6100pRanges value);
+
+bool AccelQma6100P_Read(tAccelQma6100P *env, vector3 *value);
+
+
+#endif //ACCEL_QMA6100P_H
--- a/Src/Accel_QMA6100P.c
+++ b/Src/Accel_QMA6100P.c
@ -0,0 +1,228 @@
+//
+// 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))
+
+    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) в случае сбоя при записи конфигурации
+    return true;
+}
+
+
+bool AccelQma6100P_SetRange(tAccelQma6100P *env, eAccelQma6100pRanges value) {
+    uint8_t reg;
+    QMA6100P_RINO(vAccelReadReg(env, QMA6100P_REG_FSR, &reg, 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, &regLow, QMA6100P_TIMEOUT));
+    QMA6100P_RINO(vAccelReadReg(env, firstReg + 1, &regHigh, 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;
+
+    *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
--- a/modular.json
+++ b/modular.json
@ -0,0 +1,22 @@
+{
+  "dep": [
+    {
+      "type": "git",
+      "provider": "NAVIGATOR_UVEOS_NATION_TELIT",
+      "repo": "SpiPort"
+    },
+    {
+      "type": "git",
+      "provider": "NAVIGATOR_UVEOS_NATION_TELIT",
+      "repo": "VectorMath"
+    }
+  ],
+  "cmake": {
+    "inc_dirs": [
+      "Inc"
+    ],
+    "srcs": [
+      "Src/**.c"
+    ]
+  }
+}