QMA6100P 加速度计传感器驱动实现(nRF52832平台)-CSDN博客

参数	规格	优势
量程	±2g/±4g/±8g/±16g	宽量程适应不同应用
分辨率	14位ADC	高精度测量 (±0.1mg)
噪声密度	100 μg/√Hz	低噪声输出
带宽	1kHz	捕捉快速运动
非线性度	<0.5% FS	高线性度输出
零g偏移	±30mg	出厂校准精度高

1.2 功耗特性

工作模式	电流消耗	唤醒时间
正常工作	150μA @ 100Hz	-
低功耗模式	20μA @ 1Hz	5ms
睡眠模式	2μA	50ms
待机模式	0.5μA	100ms

1.3 . 数字接口

I²C接口：标准400kHz，兼容1.8V/3.3V系统
中断输出：可配置多种中断源
FIFO缓冲区：32级数据缓存，减少主机负载

1.4 灵敏度系数表

量程	灵敏度 (LSB/g)	分辨率 (mg/LSB)
±2g	4096	0.244
±4g	2048	0.488
±8g	1024	0.976
±16g	512	1.953

1. 5 典型应用场景

1）可穿戴设备

计步算法：精准步数统计，误差<3%

活动识别：步行/跑步/骑行模式自动识别

睡眠质量：翻身次数和深度睡眠分析

2）物联网设备

资产追踪：震动事件检测

智能家居：门窗开合状态监测

工业监控：设备振动分析

3）移动设备

屏幕旋转：自动横竖屏切换

游戏控制：体感操作

防抖系统：相机电子防抖

1.6 关键控制寄存器

2 驱动程序实现

2.1 完整驱动程序代码

#include "nrf_drv_twi.h"
#include "nrf_delay.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "math.h"

// QMA6100P I2C地址 (SA0=0)
#define QMA6100P_ADDR        0x12

// QMA6100P寄存器定义
#define QMA6100P_CHIP_ID     0x00
#define QMA6100P_XOUT_L      0x01
#define QMA6100P_XOUT_H      0x02
#define QMA6100P_YOUT_L      0x03
#define QMA6100P_YOUT_H      0x04
#define QMA6100P_ZOUT_L      0x05
#define QMA6100P_ZOUT_H      0x06
#define QMA6100P_STATUS      0x09
#define QMA6100P_CTRL1       0x0A
#define QMA6100P_CTRL2       0x0B
#define QMA6100P_CTRL3       0x0C
#define QMA6100P_POWER_MODE  0x11
#define QMA6100P_RESET       0x36

// 量程配置
typedef enum {
    QMA6100P_RANGE_2G = 0,   // ±2g
    QMA6100P_RANGE_4G,       // ±4g
    QMA6100P_RANGE_8G,       // ±8g
    QMA6100P_RANGE_16G       // ±16g
} qma6100p_range_t;

// 输出数据速率 (ODR)
typedef enum {
    QMA6100P_ODR_0_78HZ = 0,
    QMA6100P_ODR_1_56HZ,
    QMA6100P_ODR_3_12HZ,
    QMA6100P_ODR_6_25HZ,
    QMA6100P_ODR_12_5HZ,
    QMA6100P_ODR_25HZ,
    QMA6100P_ODR_50HZ,
    QMA6100P_ODR_100HZ,
    QMA6100P_ODR_200HZ,
    QMA6100P_ODR_400HZ,
    QMA6100P_ODR_500HZ,
    QMA6100P_ODR_1000HZ
} qma6100p_odr_t;

// 传感器数据结构
typedef struct {
    int16_t x;
    int16_t y;
    int16_t z;
    float x_g;
    float y_g;
    float z_g;
} qma6100p_data_t;

// 全局变量
static const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(0);
static qma6100p_range_t m_range = QMA6100P_RANGE_8G;
static float m_sensitivity = 1024.0f; // ±8g时的灵敏度 (LSB/g)

// I2C初始化
void twi_init(void) {
    ret_code_t err_code;
    
    const nrf_drv_twi_config_t twi_config = {
        .scl                = ARDUINO_SCL_PIN, // 根据实际硬件修改
        .sda                = ARDUINO_SDA_PIN, // 根据实际硬件修改
        .frequency          = NRF_DRV_TWI_FREQ_400K,
        .interrupt_priority = APP_IRQ_PRIORITY_LOW,
        .clear_bus_init     = false
    };
    
    err_code = nrf_drv_twi_init(&m_twi, &twi_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);
    
    nrf_drv_twi_enable(&m_twi);
}

// 写入寄存器
ret_code_t qma6100p_write_reg(uint8_t reg, uint8_t data) {
    ret_code_t err_code;
    uint8_t buffer[2] = {reg, data};
    
    err_code = nrf_drv_twi_tx(&m_twi, QMA6100P_ADDR, buffer, sizeof(buffer), false);
    return err_code;
}

// 读取寄存器
ret_code_t qma6100p_read_reg(uint8_t reg, uint8_t *data) {
    ret_code_t err_code;
    
    err_code = nrf_drv_twi_tx(&m_twi, QMA6100P_ADDR, &reg, 1, true);
    if (err_code != NRF_SUCCESS) return err_code;
    
    return nrf_drv_twi_rx(&m_twi, QMA6100P_ADDR, data, 1);
}

// 读取多个寄存器
ret_code_t qma6100p_read_regs(uint8_t start_reg, uint8_t *data, uint8_t length) {
    ret_code_t err_code;
    
    err_code = nrf_drv_twi_tx(&m_twi, QMA6100P_ADDR, &start_reg, 1, true);
    if (err_code != NRF_SUCCESS) return err_code;
    
    return nrf_drv_twi_rx(&m_twi, QMA6100P_ADDR, data, length);
}

// 重置传感器
ret_code_t qma6100p_reset(void) {
    // 发送复位命令
    ret_code_t err_code = qma6100p_write_reg(QMA6100P_RESET, 0xB6);
    if (err_code != NRF_SUCCESS) return err_code;
    
    // 等待复位完成
    nrf_delay_ms(20);
    return NRF_SUCCESS;
}

// 初始化传感器
ret_code_t qma6100p_init(qma6100p_range_t range, qma6100p_odr_t odr) {
    ret_code_t err_code;
    uint8_t chip_id;
    
    // 读取芯片ID
    err_code = qma6100p_read_reg(QMA6100P_CHIP_ID, &chip_id);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("Failed to read chip ID");
        return err_code;
    }
    
    if (chip_id != 0x41) {
        NRF_LOG_ERROR("Invalid chip ID: 0x%02X", chip_id);
        return NRF_ERROR_NOT_FOUND;
    }
    
    NRF_LOG_INFO("QMA6100P detected, Chip ID: 0x%02X", chip_id);
    
    // 重置传感器
    err_code = qma6100p_reset();
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("Reset failed");
        return err_code;
    }
    
    // 设置量程和ODR
    uint8_t ctrl1 = ((range & 0x03) << 4) | (odr & 0x0F);
    err_code = qma6100p_write_reg(QMA6100P_CTRL1, ctrl1);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("Failed to set CTRL1");
        return err_code;
    }
    
    // 设置滤波器带宽 (设置为ODR/2)
    err_code = qma6100p_write_reg(QMA6100P_CTRL2, 0x00);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("Failed to set CTRL2");
        return err_code;
    }
    
    // 设置正常模式
    err_code = qma6100p_write_reg(QMA6100P_POWER_MODE, 0x00);
    if (err_code != NRF_SUCCESS) {
        NRF_LOG_ERROR("Failed to set power mode");
        return err_code;
    }
    
    // 保存量程配置
    m_range = range;
    
    // 根据量程设置灵敏度
    switch (range) {
        case QMA6100P_RANGE_2G:
            m_sensitivity = 4096.0f; // LSB/g
            break;
        case QMA6100P_RANGE_4G:
            m_sensitivity = 2048.0f; // LSB/g
            break;
        case QMA6100P_RANGE_8G:
            m_sensitivity = 1024.0f; // LSB/g
            break;
        case QMA6100P_RANGE_16G:
            m_sensitivity = 512.0f;  // LSB/g
            break;
    }
    
    NRF_LOG_INFO("QMA6100P initialized. Range: %d g, ODR: %d", 
                1 << (range + 1), 
                100 * (1 << (odr > 7 ? odr - 4 : odr))); // 近似ODR值
    
    return NRF_SUCCESS;
}

// 检查新数据是否就绪
bool qma6100p_data_ready(void) {
    uint8_t status;
    ret_code_t err_code = qma6100p_read_reg(QMA6100P_STATUS, &status);
    
    if (err_code == NRF_SUCCESS) {
        return (status & 0x01) != 0; // 检查DRDY位
    }
    return false;
}

// 读取原始加速度数据
ret_code_t qma6100p_read_raw(qma6100p_data_t *data) {
    uint8_t raw_data[6];
    ret_code_t err_code = qma6100p_read_regs(QMA6100P_XOUT_L, raw_data, 6);
    
    if (err_code != NRF_SUCCESS) {
        return err_code;
    }
    
    // 组合14位数据 (左对齐，右移2位)
    data->x = (int16_t)((raw_data[1] << 8) | raw_data[0]) >> 2;
    data->y = (int16_t)((raw_data[3] << 8) | raw_data[2]) >> 2;
    data->z = (int16_t)((raw_data[5] << 8) | raw_data[4]) >> 2;
    
    return NRF_SUCCESS;
}

// 读取加速度数据 (g单位)
ret_code_t qma6100p_read(qma6100p_data_t *data) {
    ret_code_t err_code = qma6100p_read_raw(data);
    
    if (err_code != NRF_SUCCESS) {
        return err_code;
    }
    
    // 转换为g单位
    data->x_g = (float)data->x / m_sensitivity;
    data->y_g = (float)data->y / m_sensitivity;
    data->z_g = (float)data->z / m_sensitivity;
    
    return NRF_SUCCESS;
}

// 计算倾角 (单位：度)
void qma6100p_calculate_tilt(qma6100p_data_t *data, float *roll, float *pitch) {
    // 计算滚转角 (绕X轴旋转)
    *roll = atan2(data->y_g, data->z_g) * 180.0f / M_PI;
    
    // 计算俯仰角 (绕Y轴旋转)
    *pitch = atan2(-data->x_g, sqrt(data->y_g * data->y_g + data->z_g * data->z_g)) * 180.0f / M_PI;
}

// 进入低功耗模式
ret_code_t qma6100p_sleep(void) {
    return qma6100p_write_reg(QMA6100P_POWER_MODE, 0x03); // 睡眠模式
}

// 唤醒传感器
ret_code_t qma6100p_wakeup(void) {
    return qma6100p_write_reg(QMA6100P_POWER_MODE, 0x00); // 正常模式
}

// 设置中断配置
ret_code_t qma6100p_config_interrupt(uint8_t config) {
    return qma6100p_write_reg(QMA6100P_CTRL3, config);
}

2.2 驱动使用示例

2.2.1 初始化与数据读取

void qma6100p_example(void) {
    qma6100p_data_t sensor_data;
    float roll, pitch;
    
    // 初始化I2C
    twi_init();
    
    // 初始化QMA6100P (范围±8g，ODR=100Hz)
    ret_code_t err_code = qma6100p_init(QMA6100P_RANGE_8G, QMA6100P_ODR_100HZ);
    APP_ERROR_CHECK(err_code);
    
    while (1) {
        // 检查数据是否就绪
        if (qma6100p_data_ready()) {
            // 读取加速度数据
            err_code = qma6100p_read(&sensor_data);
            if (err_code == NRF_SUCCESS) {
                // 计算倾角
                qma6100p_calculate_tilt(&sensor_data, &roll, &pitch);
                
                // 打印结果
                NRF_LOG_INFO("Accel: X=%.2fg, Y=%.2fg, Z=%.2fg", 
                            sensor_data.x_g, sensor_data.y_g, sensor_data.z_g);
                NRF_LOG_INFO("Tilt: Roll=%.1f°, Pitch=%.1f°", roll, pitch);
            }
        }
        
        // 低功耗延迟 (10ms)
        nrf_delay_ms(10);
    }
}

2.2.2 低功耗应用示例

void low_power_activity_monitor(void) {
    qma6100p_data_t sensor_data;
    uint32_t last_activity_time = 0;
    bool active = false;
    
    // 初始化
    twi_init();
    qma6100p_init(QMA6100P_RANGE_4G, QMA6100P_ODR_12_5HZ);
    
    // 配置活动检测中断
    qma6100p_config_interrupt(0x08); // 启用数据就绪中断
    
    while (1) {
        if (qma6100p_data_ready()) {
            qma6100p_read(&sensor_data);
            
            // 计算加速度矢量幅度
            float magnitude = sqrt(sensor_data.x_g * sensor_data.x_g +
                                  sensor_data.y_g * sensor_data.y_g +
                                  sensor_data.z_g * sensor_data.z_g);
            
            // 检测活动 (阈值1.2g)
            if (magnitude > 1.2f) {
                last_activity_time = nrf_rtc_counter_get();
                if (!active) {
                    active = true;
                    NRF_LOG_INFO("Activity detected");
                }
            }
            // 检测静止
            else if (active && (nrf_rtc_counter_get() - last_activity_time > 3000)) {
                active = false;
                NRF_LOG_INFO("Inactive for 3 seconds");
            }
        }
        
        // 每100ms检查一次
        nrf_delay_ms(100);
        
        // 长时间无活动时进入深度睡眠
        if (!active && (nrf_rtc_counter_get() - last_activity_time > 30000)) {
            qma6100p_sleep();
            enter_deep_sleep();
            qma6100p_wakeup();
        }
    }
}

2.3 关键功能

1) 数据读取与转换

// 读取原始数据 (14位)
data->x = (int16_t)((raw_data[1] << 8) | raw_data[0]) >> 2;

// 转换为g单位
data->x_g = (float)data->x / m_sensitivity;

灵敏度系数表：

量程	灵敏度 (LSB/g)	分辨率 (mg/LSB)
±2g	4096	0.244
±4g	2048	0.488
±8g	1024	0.976
±16g	512	1.953

2) 倾角计算算法

// 滚转角计算 (绕X轴)
roll = atan2(y_g, z_g) * 180.0f / M_PI;

// 俯仰角计算 (绕Y轴)
pitch = atan2(-x_g, sqrt(y_g*y_g + z_g*z_g)) * 180.0f / M_PI;

3)运动检测算法

bool detect_motion(float x, float y, float z) {
    static float last_x = 0, last_y = 0, last_z = 0;
    const float threshold = 0.2f; // 运动阈值 (g)
    
    // 计算加速度变化量
    float delta = fabs(x - last_x) + 
                 fabs(y - last_y) + 
                 fabs(z - last_z);
    
    // 更新历史值
    last_x = x;
    last_y = y;
    last_z = z;
    
    return delta > threshold;
}

4) 姿态解算

void calculate_orientation(float x, float y, float z, float *roll, float *pitch) {
    // 计算滚转角 (X轴旋转)
    *roll = atan2(y, z) * 180.0 / M_PI;
    
    // 计算俯仰角 (Y轴旋转)
    *pitch = atan2(-x, sqrt(y*y + z*z)) * 180.0 / M_PI;
}

2.4 低功耗优化策略

1) 动态功耗管理

void adaptive_power_management(void) {
    static qma6100p_odr_t current_odr = QMA6100P_ODR_100HZ;
    
    if (is_motion_detected()) {
        // 高动态活动 - 使用高ODR
        if (current_odr != QMA6100P_ODR_200HZ) {
            qma6100p_set_odr(QMA6100P_ODR_200HZ);
            current_odr = QMA6100P_ODR_200HZ;
        }
    }
    else if (is_low_activity()) {
        // 低活动 - 使用中等ODR
        if (current_odr != QMA6100P_ODR_25HZ) {
            qma6100p_set_odr(QMA6100P_ODR_25HZ);
            current_odr = QMA6100P_ODR_25HZ;
        }
    }
    else {
        // 静止状态 - 使用低ODR
        if (current_odr != QMA6100P_ODR_1_56HZ) {
            qma6100p_set_odr(QMA6100P_ODR_1_56HZ);
            current_odr = QMA6100P_ODR_1_56HZ;
        }
    }
}

2) 睡眠模式集成

void enter_sleep_cycle(void) {
    // 1. 设置传感器睡眠
    qma6100p_sleep();
    
    // 2. 配置RTC唤醒
    NRF_RTC0->CC[0] = NRF_RTC0->COUNTER + 32768; // 1秒后唤醒
    
    // 3. 进入System OFF模式
    NRF_POWER->SYSTEMOFF = 1;
    
    // 4. 唤醒后恢复
    qma6100p_wakeup();
}

3) 功耗数据对比

工作模式	电流消耗	唤醒时间	适用场景
正常工作	150-200μA	-	连续监测
睡眠模式	10-20μA	5ms	间歇采样
深度睡眠	1-2μA	50ms	长期待机
关闭模式	0.1μA	100ms	电池保存

4）智能采样技术

void adaptive_sampling(void) {
    static qma6100p_odr_t current_odr = ODR_100HZ;
    
    if (high_activity_detected()) {
        set_odr(ODR_200HZ); // 高活动状态
    } 
    else if (low_activity_detected()) {
        set_odr(ODR_25HZ);  // 低活动状态
    }
    else {
        set_odr(ODR_1HZ);   // 静止状态
    }
}

5）运动激活唤醒

void motion_activated_wakeup(void) {
    // 配置唤醒阈值
    set_wakeup_threshold(0.2f); // 0.2g阈值
    
    // 进入低功耗模式
    enter_sleep_mode();
    
    // 等待运动中断
    while(!motion_interrupt());
    
    // 恢复正常采样
    set_odr(ODR_100HZ);
}

3 调试与校准

3.1 传感器校准

1) 平均值方法

void calibrate_qma6100p(void) {
    qma6100p_data_t data;
    float offset_x = 0, offset_y = 0, offset_z = 0;
    const uint16_t samples = 100;
    
    // 采集样本
    for (int i = 0; i < samples; i++) {
        qma6100p_read(&data);
        offset_x += data.x_g;
        offset_y += data.y_g;
        offset_z += data.z_g;
        nrf_delay_ms(10);
    }
    
    // 计算平均偏移
    offset_x /= samples;
    offset_y /= samples;
    offset_z = (offset_z / samples) - 1.0f; // 减去重力
    
    // 保存校准值
    save_calibration(offset_x, offset_y, offset_z);
    
    NRF_LOG_INFO("Calibration complete: X=%.4fg, Y=%.4fg, Z=%.4fg", 
                offset_x, offset_y, offset_z);
}

// 应用校准
void apply_calibration(qma6100p_data_t *data) {
    calibration_t cal = load_calibration();
    data->x_g -= cal.offset_x;
    data->y_g -= cal.offset_y;
    data->z_g -= cal.offset_z;
}

2）六点校准法

void six_point_calibration(void) {
    // 六面校准数据采集
    float positions[6][3] = {
        // +X, +Y, +Z, -X, -Y, -Z
        {1.0, 0.0, 0.0}, {-1.0, 0.0, 0.0},
        {0.0, 1.0, 0.0}, {0.0, -1.0, 0.0},
        {0.0, 0.0, 1.0}, {0.0, 0.0, -1.0}
    };
    
    float offsets[3] = {0};
    float scales[3] = {0};
    
    // 采集每个位置的100个样本
    for (int i = 0; i < 6; i++) {
        qma6100p_data_t avg = {0};
        for (int j = 0; j < 100; j++) {
            qma6100p_data_t data;
            qma6100p_read(&data);
            avg.x_g += data.x_g;
            avg.y_g += data.y_g;
            avg.z_g += data.z_g;
            delay(10);
        }
        avg.x_g /= 100;
        avg.y_g /= 100;
        avg.z_g /= 100;
        
        // 计算偏差
        offsets[0] += avg.x_g - positions[i][0];
        offsets[1] += avg.y_g - positions[i][1];
        offsets[2] += avg.z_g - positions[i][2];
        
        // 计算比例因子
        scales[0] += fabs(positions[i][0]);
        scales[1] += fabs(positions[i][1]);
        scales[2] += fabs(positions[i][2]);
    }
    
    // 保存校准参数
    save_calibration(offsets, scales);
}

3）温度补偿

void temperature_compensation(float temp) {
    // 温度补偿系数 (每℃变化)
    const float temp_coeff[3] = {0.0005f, 0.0006f, 0.0007f};
    static float ref_temp = 25.0f;
    
    // 计算温度偏移
    float delta_temp = temp - ref_temp;
    
    // 应用补偿
    calibration.x_offset += temp_coeff[0] * delta_temp;
    calibration.y_offset += temp_coeff[1] * delta_temp;
    calibration.z_offset += temp_coeff[2] * delta_temp;
}

3.2 数据可视化调试

void real_time_data_monitor(void) {
    qma6100p_data_t data;
    
    while (1) {
        if (qma6100p_data_ready()) {
            qma6100p_read(&data);
            
            // 发送到上位机 (例如通过BLE)
            send_ble_packet(&data);
            
            // 控制台输出
            NRF_LOG_RAW_INFO("%.3f,%.3f,%.3f\n", 
                            data.x_g, data.y_g, data.z_g);
        }
        nrf_delay_ms(10);
    }
}

3.3 读取寄存器的值

*** Booting nRF Connect SDK v2.9.0-7787b2649840 ***
*** Using Zephyr OS v3.7.99-1f8f3dc29142 ***
reg[00], B_val: 90,  A_val: 90
reg[01], B_val: 91,  A_val: 00
reg[02], B_val: F8,  A_val: F7
reg[03], B_val: 49,  A_val: 8D
reg[04], B_val: FF,  A_val: FF
reg[05], B_val: A9,  A_val: A9
reg[06], B_val: 1E,  A_val: 1E
reg[07], B_val: 00,  A_val: 00
reg[08], B_val: 00,  A_val: 00
reg[09], B_val: 00,  A_val: 00
reg[0A], B_val: 00,  A_val: 00
reg[0B], B_val: 00,  A_val: 00
reg[0C], B_val: 00,  A_val: 00
reg[0D], B_val: 00,  A_val: 00
reg[0E], B_val: 01,  A_val: 01
reg[0F], B_val: 02,  A_val: 02
reg[10], B_val: 63,  A_val: 63
reg[11], B_val: 87,  A_val: 87
reg[12], B_val: 94,  A_val: 94
reg[13], B_val: 7F,  A_val: 7F
reg[14], B_val: 0B,  A_val: 0B
reg[15], B_val: 0C,  A_val: 0C
reg[16], B_val: 08,  A_val: 08
reg[17], B_val: 00,  A_val: 00
reg[18], B_val: 00,  A_val: 00
reg[19], B_val: 00,  A_val: 00
reg[1A], B_val: 00,  A_val: 00
reg[1B], B_val: 00,  A_val: 00
reg[1C], B_val: 00,  A_val: 00
reg[1D], B_val: 00,  A_val: 00
reg[1E], B_val: 08,  A_val: 08
reg[1F], B_val: 00,  A_val: 00
reg[20], B_val: 05,  A_val: 05
reg[21], B_val: 00,  A_val: 00
reg[22], B_val: D8,  A_val: D8
reg[23], B_val: 7C,  A_val: 7C
reg[24], B_val: 00,  A_val: 00
reg[25], B_val: 81,  A_val: 81
reg[26], B_val: 02,  A_val: 02
reg[27], B_val: 00,  A_val: 00
reg[28], B_val: 00,  A_val: 00
reg[29], B_val: 00,  A_val: 00
reg[2A], B_val: 05,  A_val: 05
reg[2B], B_val: CD,  A_val: CD
reg[2C], B_val: 00,  A_val: 00
reg[2D], B_val: 00,  A_val: 00
reg[2E], B_val: 00,  A_val: 00
reg[2F], B_val: 00,  A_val: 00
reg[30], B_val: 3F,  A_val: 3F
reg[31], B_val: 00,  A_val: 00
reg[32], B_val: 00,  A_val: 00
reg[33], B_val: 0D,  A_val: 0D
reg[34], B_val: 9D,  A_val: 9D
reg[35], B_val: 66,  A_val: 66
reg[36], B_val: 00,  A_val: 00
reg[37], B_val: E7,  A_val: E7
reg[38], B_val: 63,  A_val: 63
reg[39], B_val: F2,  A_val: F2
reg[3A], B_val: 83,  A_val: 83
reg[3B], B_val: 3F,  A_val: 3F
reg[3C], B_val: 43,  A_val: 43
reg[3D], B_val: 96,  A_val: 96
reg[3E], B_val: 07,  A_val: 07
reg[3F], B_val: 65,  A_val: 49
reg[40], B_val: 01,  A_val: 01
reg[41], B_val: 0F,  A_val: 0F
reg[42], B_val: 12,  A_val: 12
reg[43], B_val: D6,  A_val: D6
reg[44], B_val: 09,  A_val: 09
reg[45], B_val: C0,  A_val: C0
reg[46], B_val: 0F,  A_val: 0F
reg[47], B_val: 42,  A_val: 42
reg[48], B_val: 96,  A_val: 96
reg[49], B_val: 00,  A_val: 00
reg[4A], B_val: 20,  A_val: 20
reg[4B], B_val: 05,  A_val: 05
reg[4C], B_val: A5,  A_val: A5
reg[4D], B_val: 00,  A_val: 00
reg[4E], B_val: 00,  A_val: 00
reg[4F], B_val: 00,  A_val: 00
reg[50], B_val: 00,  A_val: 00
reg[51], B_val: 00,  A_val: 00
reg[52], B_val: 00,  A_val: 00
reg[53], B_val: FF,  A_val: FF
reg[54], B_val: FF,  A_val: FF
reg[55], B_val: FF,  A_val: FF
reg[56], B_val: 01,  A_val: 01
reg[57], B_val: 00,  A_val: 00
reg[58], B_val: 00,  A_val: 00
reg[59], B_val: 61,  A_val: 61
reg[5A], B_val: 29,  A_val: 29
reg[5B], B_val: 29,  A_val: 29
reg[5C], B_val: 10,  A_val: 10
reg[5D], B_val: 51,  A_val: 51
reg[5E], B_val: 03,  A_val: 03
reg[5F], B_val: 00,  A_val: 00
reg[60], B_val: 00,  A_val: 00
reg[61], B_val: 00,  A_val: 00
reg[62], B_val: 00,  A_val: 00
reg[63], B_val: 00,  A_val: 00
reg[64], B_val: 00,  A_val: 00
reg[65], B_val: 00,  A_val: 00
reg[66], B_val: 00,  A_val: 00
reg[67], B_val: 00,  A_val: 00
reg[68], B_val: 00,  A_val: 00
reg[69], B_val: 00,  A_val: 00
reg[6A], B_val: 00,  A_val: 00
reg[6B], B_val: 00,  A_val: 00
reg[6C], B_val: 00,  A_val: 00
reg[6D], B_val: 00,  A_val: 00
reg[6E], B_val: 00,  A_val: 00
reg[6F], B_val: 00,  A_val: 00
Bluetooth initialized
 x = 65029,y = 65493, z = 1989, step = 0  +ACC: OK