sv/IronOS
1
0
Fork 0
forked from me/IronOS
Code Pull Requests Activity

Make all accelerometers I2C bus aware

Browse Source 
Update accelerometers_common.h
This commit is contained in:
Ben V. Brown
2023-07-22 18:18:07 +10:00
parent 964d37a8e8
commit 1396363eaa
7 changed files with 47 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
source/Core/Drivers/BMA223.cpp
View File

@@ -5,14 +5,16 @@
* Author: Ralim * Author: Ralim
*/ */
#include "accelerometers_common.h"
#include <BMA223.hpp> #include <BMA223.hpp>
#include <array> #include <array>
bool BMA223::detect() { bool BMA223::detect() {
if (FRToSI2C::probe(BMA223_ADDRESS)) { if (ACCEL_I2C_CLASS::probe(BMA223_ADDRESS)) {
// Read chip id to ensure its not an address collision // Read chip id to ensure its not an address collision
uint8_t id = 0; uint8_t id = 0;
if (FRToSI2C::Mem_Read(BMA223_ADDRESS, BMA223_BGW_CHIPID, &id, 1)) { if (ACCEL_I2C_CLASS::Mem_Read(BMA223_ADDRESS, BMA223_BGW_CHIPID, &id, 1)) {
return id == 0b11111000; return id == 0b11111000;
} }
} }
@@ -20,7 +22,7 @@ bool BMA223::detect() {
return false; return false;
} }
static const FRToSI2C::I2C_REG i2c_registers[] = { static const ACCEL_I2C_CLASS::I2C_REG i2c_registers[] = {
// //
// //
{BMA223_PMU_RANGE, 0b00000011, 0}, // 2G range {BMA223_PMU_RANGE, 0b00000011, 0}, // 2G range
@@ -44,7 +46,7 @@ bool BMA223::initalize() {
// Hysteresis is set to ~ 16 counts // Hysteresis is set to ~ 16 counts
// Theta blocking is set to 0b10 // Theta blocking is set to 0b10
return FRToSI2C::writeRegistersBulk(BMA223_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); return ACCEL_I2C_CLASS::writeRegistersBulk(BMA223_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
} }
void BMA223::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) { void BMA223::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
@@ -52,7 +54,7 @@ void BMA223::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
// And yet there are MSB and LSB registers _sigh_. // And yet there are MSB and LSB registers _sigh_.
uint8_t sensorData[6] = {0, 0, 0, 0, 0, 0}; uint8_t sensorData[6] = {0, 0, 0, 0, 0, 0};
if (FRToSI2C::Mem_Read(BMA223_ADDRESS, BMA223_ACCD_X_LSB, sensorData, 6) == false) { if (ACCEL_I2C_CLASS::Mem_Read(BMA223_ADDRESS, BMA223_ACCD_X_LSB, sensorData, 6) == false) {
x = y = z = 0; x = y = z = 0;
return; return;
} }

3
source/Core/Drivers/BMA223.hpp
View File

@@ -9,6 +9,7 @@
#define CORE_DRIVERS_BMA223_HPP_ #define CORE_DRIVERS_BMA223_HPP_
#include "BMA223_defines.h" #include "BMA223_defines.h"
#include "BSP.h" #include "BSP.h"
#include "accelerometers_common.h"
#include "I2C_Wrapper.hpp" #include "I2C_Wrapper.hpp"
class BMA223 { class BMA223 {
@@ -17,7 +18,7 @@ public:
static bool initalize(); static bool initalize();
// 1 = rh, 2,=lh, 8=flat // 1 = rh, 2,=lh, 8=flat
static Orientation getOrientation() { static Orientation getOrientation() {
uint8_t val = FRToSI2C::I2C_RegisterRead(BMA223_ADDRESS, BMA223_INT_STATUS_3); uint8_t val = ACCEL_I2C_CLASS::I2C_RegisterRead(BMA223_ADDRESS, BMA223_INT_STATUS_3);
val >>= 4; // we dont need high values val >>= 4; // we dont need high values
val &= 0b11; val &= 0b11;
if (val & 0b10) { if (val & 0b10) {

11
source/Core/Drivers/MMA8652FC.cpp
View File

@@ -8,9 +8,10 @@
#include <array> #include <array>
#include "MMA8652FC.hpp" #include "MMA8652FC.hpp"
#include "accelerometers_common.h"
#include "cmsis_os.h" #include "cmsis_os.h"
static const FRToSI2C::I2C_REG i2c_registers[] = { static const ACCEL_I2C_CLASS::I2C_REG i2c_registers[] = {
{CTRL_REG2, 0, 0}, // Normal mode {CTRL_REG2, 0, 0}, // Normal mode
{CTRL_REG2, 0x40, 2}, // Reset all registers to POR values {CTRL_REG2, 0x40, 2}, // Reset all registers to POR values
{FF_MT_CFG_REG, 0x78, 0}, // Enable motion detection for X, Y, Z axis, latch disabled {FF_MT_CFG_REG, 0x78, 0}, // Enable motion detection for X, Y, Z axis, latch disabled
@@ -26,11 +27,11 @@ static const FRToSI2C::I2C_REG i2c_registers[] = {
{CTRL_REG1, 0x19, 0} // ODR=12 Hz, Active mode {CTRL_REG1, 0x19, 0} // ODR=12 Hz, Active mode
}; };
bool MMA8652FC::initalize() { return FRToSI2C::writeRegistersBulk(MMA8652FC_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); } bool MMA8652FC::initalize() { return ACCEL_I2C_CLASS::writeRegistersBulk(MMA8652FC_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); }
Orientation MMA8652FC::getOrientation() { Orientation MMA8652FC::getOrientation() {
// First read the PL_STATUS register // First read the PL_STATUS register
uint8_t plStatus = FRToSI2C::I2C_RegisterRead(MMA8652FC_I2C_ADDRESS, PL_STATUS_REG); uint8_t plStatus = ACCEL_I2C_CLASS::I2C_RegisterRead(MMA8652FC_I2C_ADDRESS, PL_STATUS_REG);
if ((plStatus & 0b10000000) == 0b10000000) { if ((plStatus & 0b10000000) == 0b10000000) {
plStatus >>= 1; // We don't need the up/down bit plStatus >>= 1; // We don't need the up/down bit
plStatus &= 0x03; // mask to the two lower bits plStatus &= 0x03; // mask to the two lower bits
@@ -47,11 +48,11 @@ Orientation MMA8652FC::getOrientation() {
void MMA8652FC::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) { void MMA8652FC::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
std::array<int16_t, 3> sensorData; std::array<int16_t, 3> sensorData;
FRToSI2C::Mem_Read(MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, reinterpret_cast<uint8_t *>(sensorData.begin()), sensorData.size() * sizeof(int16_t)); ACCEL_I2C_CLASS::Mem_Read(MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, reinterpret_cast<uint8_t *>(sensorData.begin()), sensorData.size() * sizeof(int16_t));
x = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t *>(&sensorData[0]))); x = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t *>(&sensorData[0])));
y = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t *>(&sensorData[1]))); y = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t *>(&sensorData[1])));
z = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t *>(&sensorData[2]))); z = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t *>(&sensorData[2])));
} }
bool MMA8652FC::detect() { return FRToSI2C::probe(MMA8652FC_I2C_ADDRESS); } bool MMA8652FC::detect() { return ACCEL_I2C_CLASS::probe(MMA8652FC_I2C_ADDRESS); }

11
source/Core/Drivers/MSA301.cpp
View File

@@ -6,11 +6,12 @@
*/ */
#include "MSA301_defines.h" #include "MSA301_defines.h"
#include "accelerometers_common.h"
#include <MSA301.h> #include <MSA301.h>
#define MSA301_I2C_ADDRESS 0x26 << 1 #define MSA301_I2C_ADDRESS 0x26 << 1
bool MSA301::detect() { return FRToSI2C::probe(MSA301_I2C_ADDRESS); } bool MSA301::detect() { return ACCEL_I2C_CLASS::probe(MSA301_I2C_ADDRESS); }
static const FRToSI2C::I2C_REG i2c_registers[] = { static const ACCEL_I2C_CLASS::I2C_REG i2c_registers[] = {
// //
// //
{MSA301_REG_ODR, 0b00001000, 1}, // X/Y/Z enabled @ 250Hz {MSA301_REG_ODR, 0b00001000, 1}, // X/Y/Z enabled @ 250Hz
@@ -21,11 +22,11 @@ static const FRToSI2C::I2C_REG i2c_registers[] = {
}; };
bool MSA301::initalize() { return FRToSI2C::writeRegistersBulk(MSA301_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); } bool MSA301::initalize() { return ACCEL_I2C_CLASS::writeRegistersBulk(MSA301_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); }
Orientation MSA301::getOrientation() { Orientation MSA301::getOrientation() {
uint8_t temp = 0; uint8_t temp = 0;
FRToSI2C::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_ORIENT_STATUS, &temp, 1); ACCEL_I2C_CLASS::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_ORIENT_STATUS, &temp, 1);
switch (temp) { switch (temp) {
case 112: case 112:
return Orientation::ORIENTATION_LEFT_HAND; return Orientation::ORIENTATION_LEFT_HAND;
@@ -39,7 +40,7 @@ Orientation MSA301::getOrientation() {
void MSA301::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) { void MSA301::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
uint8_t temp[6]; uint8_t temp[6];
// Bulk read all 6 regs // Bulk read all 6 regs
FRToSI2C::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_OUT_X_L, temp, 6); ACCEL_I2C_CLASS::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_OUT_X_L, temp, 6);
x = int16_t(((int16_t)temp[1]) << 8 | temp[0]) >> 2; x = int16_t(((int16_t)temp[1]) << 8 | temp[0]) >> 2;
y = int16_t(((int16_t)temp[3]) << 8 | temp[2]) >> 2; y = int16_t(((int16_t)temp[3]) << 8 | temp[2]) >> 2;
z = int16_t(((int16_t)temp[5]) << 8 | temp[4]) >> 2; z = int16_t(((int16_t)temp[5]) << 8 | temp[4]) >> 2;

43
source/Core/Drivers/SC7A20.cpp
View File

@@ -6,6 +6,7 @@
*/ */
#include "LIS2DH12_defines.hpp" #include "LIS2DH12_defines.hpp"
#include "accelerometers_common.h"
#include <SC7A20.hpp> #include <SC7A20.hpp>
#include <SC7A20_defines.h> #include <SC7A20_defines.h>
#include <array> #include <array>
@@ -17,20 +18,20 @@ bool SC7A20::isInImitationMode;
*/ */
bool SC7A20::detect() { bool SC7A20::detect() {
if (FRToSI2C::probe(SC7A20_ADDRESS)) { if (ACCEL_I2C_CLASS::probe(SC7A20_ADDRESS)) {
// Read chip id to ensure its not an address collision // Read chip id to ensure its not an address collision
uint8_t id = 0; uint8_t id = 0;
if (FRToSI2C::Mem_Read(SC7A20_ADDRESS, SC7A20_WHO_AMI_I, &id, 1)) { if (ACCEL_I2C_CLASS::Mem_Read(SC7A20_ADDRESS, SC7A20_WHO_AMI_I, &id, 1)) {
if (id == SC7A20_WHO_AM_I_VALUE) { if (id == SC7A20_WHO_AM_I_VALUE) {
isInImitationMode = false; isInImitationMode = false;
return true; return true;
} }
} }
} }
if (FRToSI2C::probe(SC7A20_ADDRESS2)) { if (ACCEL_I2C_CLASS::probe(SC7A20_ADDRESS2)) {
// Read chip id to ensure its not an address collision // Read chip id to ensure its not an address collision
uint8_t id = 0; uint8_t id = 0;
if (FRToSI2C::Mem_Read(SC7A20_ADDRESS2, SC7A20_WHO_AMI_I, &id, 1)) { if (ACCEL_I2C_CLASS::Mem_Read(SC7A20_ADDRESS2, SC7A20_WHO_AMI_I, &id, 1)) {
if (id == SC7A20_WHO_AM_I_VALUE) { if (id == SC7A20_WHO_AM_I_VALUE) {
isInImitationMode = true; isInImitationMode = true;
return true; return true;
@@ -40,7 +41,7 @@ bool SC7A20::detect() {
return false; return false;
} }
static const FRToSI2C::I2C_REG i2c_registers[] = { static const ACCEL_I2C_CLASS::I2C_REG i2c_registers[] = {
// //
// //
{SC7A20_CTRL_REG1, 0b01100111, 0}, // 200Hz, XYZ enabled {SC7A20_CTRL_REG1, 0b01100111, 0}, // 200Hz, XYZ enabled
@@ -59,19 +60,19 @@ static const FRToSI2C::I2C_REG i2c_registers[] = {
// //
}; };
static const FRToSI2C::I2C_REG i2c_registers_alt[] = {{LIS_CTRL_REG1, 0b00110111, 0}, // 200Hz XYZ static const ACCEL_I2C_CLASS::I2C_REG i2c_registers_alt[] = {{LIS_CTRL_REG1, 0b00110111, 0}, // 200Hz XYZ
{LIS_CTRL_REG2, 0b00000000, 0}, // {LIS_CTRL_REG2, 0b00000000, 0}, //
{LIS_CTRL_REG3, 0b01100000, 0}, // Setup interrupt pins {LIS_CTRL_REG3, 0b01100000, 0}, // Setup interrupt pins
{LIS_CTRL_REG4, 0b00001000, 0}, // Block update mode off, HR on {LIS_CTRL_REG4, 0b00001000, 0}, // Block update mode off, HR on
{LIS_CTRL_REG5, 0b00000010, 0}, // {LIS_CTRL_REG5, 0b00000010, 0}, //
{LIS_CTRL_REG6, 0b01100010, 0}, {LIS_CTRL_REG6, 0b01100010, 0},
// Basically setup the unit to run, and enable 4D orientation detection // Basically setup the unit to run, and enable 4D orientation detection
{LIS_INT2_CFG, 0b01111110, 0}, // setup for movement detection {LIS_INT2_CFG, 0b01111110, 0}, // setup for movement detection
{LIS_INT2_THS, 0x28, 0}, // {LIS_INT2_THS, 0x28, 0}, //
{LIS_INT2_DURATION, 64, 0}, // {LIS_INT2_DURATION, 64, 0}, //
{LIS_INT1_CFG, 0b01111110, 0}, // {LIS_INT1_CFG, 0b01111110, 0}, //
{LIS_INT1_THS, 0x28, 0}, // {LIS_INT1_THS, 0x28, 0}, //
{LIS_INT1_DURATION, 64, 0}}; {LIS_INT1_DURATION, 64, 0}};
bool SC7A20::initalize() { bool SC7A20::initalize() {
// Setup acceleration readings // Setup acceleration readings
@@ -83,9 +84,9 @@ bool SC7A20::initalize() {
// Hysteresis is set to ~ 16 counts // Hysteresis is set to ~ 16 counts
// Theta blocking is set to 0b10 // Theta blocking is set to 0b10
if (isInImitationMode) { if (isInImitationMode) {
return FRToSI2C::writeRegistersBulk(SC7A20_ADDRESS2, i2c_registers_alt, sizeof(i2c_registers_alt) / sizeof(i2c_registers_alt[0])); return ACCEL_I2C_CLASS::writeRegistersBulk(SC7A20_ADDRESS2, i2c_registers_alt, sizeof(i2c_registers_alt) / sizeof(i2c_registers_alt[0]));
} else { } else {
return FRToSI2C::writeRegistersBulk(SC7A20_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); return ACCEL_I2C_CLASS::writeRegistersBulk(SC7A20_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
} }
} }
@@ -93,7 +94,7 @@ void SC7A20::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
// We can tell the accelerometer to output in LE mode which makes this simple // We can tell the accelerometer to output in LE mode which makes this simple
uint16_t sensorData[3] = {0, 0, 0}; uint16_t sensorData[3] = {0, 0, 0};
if (FRToSI2C::Mem_Read(isInImitationMode ? SC7A20_ADDRESS2 : SC7A20_ADDRESS, isInImitationMode ? SC7A20_OUT_X_L_ALT : SC7A20_OUT_X_L, (uint8_t *)sensorData, 6) == false) { if (ACCEL_I2C_CLASS::Mem_Read(isInImitationMode ? SC7A20_ADDRESS2 : SC7A20_ADDRESS, isInImitationMode ? SC7A20_OUT_X_L_ALT : SC7A20_OUT_X_L, (uint8_t *)sensorData, 6) == false) {
x = y = z = 0; x = y = z = 0;
return; return;
} }

4
source/Core/Drivers/SC7A20.hpp
View File

@@ -10,6 +10,8 @@
#include "BSP.h" #include "BSP.h"
#include "I2C_Wrapper.hpp" #include "I2C_Wrapper.hpp"
#include "SC7A20_defines.h" #include "SC7A20_defines.h"
#include "accelerometers_common.h"
class SC7A20 { class SC7A20 {
public: public:
@@ -17,7 +19,7 @@ public:
static bool initalize(); static bool initalize();
// 1 = rh, 2,=lh, 8=flat // 1 = rh, 2,=lh, 8=flat
static Orientation getOrientation() { static Orientation getOrientation() {
uint8_t val = ((FRToSI2C::I2C_RegisterRead(isInImitationMode ? SC7A20_ADDRESS2 : SC7A20_ADDRESS, SC7A20_INT2_SOURCE) >> 2) - 1); uint8_t val = ((ACCEL_I2C_CLASS::I2C_RegisterRead(isInImitationMode ? SC7A20_ADDRESS2 : SC7A20_ADDRESS, SC7A20_INT2_SOURCE) >> 2) - 1);
if (val == 1) { if (val == 1) {
#ifdef SC7_ORI_FLIP #ifdef SC7_ORI_FLIP
return Orientation::ORIENTATION_RIGHT_HAND; return Orientation::ORIENTATION_RIGHT_HAND;

2
source/Core/Drivers/accelerometers_common.h
View File

@@ -1,6 +1,6 @@
#ifndef CORE_DRIVERS_ACCELEROMTERS_COMMON_H_ #ifndef CORE_DRIVERS_ACCELEROMTERS_COMMON_H_
#define CORE_DRIVERS_ACCELEROMTERS_COMMON_H_ #define CORE_DRIVERS_ACCELEROMTERS_COMMON_H_
#include "configuration.h"
#if defined(ACCEL_I2CBB2) #if defined(ACCEL_I2CBB2)
#include "I2CBB2.hpp" #include "I2CBB2.hpp"
#define ACCEL_I2C_CLASS I2CBB2 #define ACCEL_I2C_CLASS I2CBB2