128 lines
2.8 KiB
C++
128 lines
2.8 KiB
C++
/*
|
|
* bma_accelerometer.h
|
|
*
|
|
* Created on: 08 íîÿá. 2015 ã.
|
|
* Author: Sergey
|
|
*/
|
|
|
|
#ifndef SRC_BMA_ACCELEROMETER_H_
|
|
#define SRC_BMA_ACCELEROMETER_H_
|
|
|
|
#include "stm32f1xx_hal.h"
|
|
#include "accelerometer.h"
|
|
extern "C"{
|
|
#include "bma2x2.h"
|
|
}
|
|
|
|
#define SPI_BUFFER_LEN 10
|
|
|
|
void swap_bytes(s16* value){
|
|
s8* bytes = (s8*)value;
|
|
auto tmp = bytes[0];
|
|
bytes[0] = bytes[1];
|
|
bytes[1] = tmp;
|
|
}
|
|
|
|
template<SPI_HandleTypeDef* spi>
|
|
class bma_accelerometer : public Accelerometer {
|
|
public:
|
|
bma_accelerometer(){
|
|
BMA_bind_SPI(&_bma2x2);
|
|
}
|
|
bool init(){
|
|
auto result = bma2x2_init(&_bma2x2);
|
|
result += bma2x2_set_bw(BMA2x2_BW_62_50HZ);
|
|
return result == 0;
|
|
}
|
|
bool set_power(bool on) {
|
|
return 0 == bma2x2_set_power_mode(on ? BMA2x2_MODE_NORMAL : BMA2x2_MODE_DEEP_SUSPEND);
|
|
}
|
|
|
|
float3 get_acceleration() override {
|
|
u8 range = 0;
|
|
if(0 != bma2x2_get_range(&range)){
|
|
return float3(0,0,0);
|
|
}
|
|
|
|
//14 bit resolution. Hardcoded for now.
|
|
constexpr float max_resolution_val = (float)0x1fff;
|
|
|
|
float range_mul = 2.0f;
|
|
if(range == BMA2x2_RANGE_4G){
|
|
range_mul = 4.0f;
|
|
}else if(range == BMA2x2_RANGE_8G){
|
|
range_mul = 8.0f;
|
|
}else if(range == BMA2x2_RANGE_16G){
|
|
range_mul = 16.0f;
|
|
}
|
|
|
|
//BMA2x2_RANGE_2G
|
|
bma2x2_accel_data sample_xyz;
|
|
|
|
|
|
if(0 == bma2x2_read_accel_xyz(&sample_xyz)){
|
|
return float3(
|
|
(float)(sample_xyz.x) / max_resolution_val,
|
|
(float)(sample_xyz.y) / max_resolution_val,
|
|
(float)(sample_xyz.z) / max_resolution_val) * range_mul;
|
|
}else{
|
|
return float3(0,0,0);
|
|
}
|
|
}
|
|
private:
|
|
static void SPI_begin(){
|
|
HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_RESET);
|
|
}
|
|
|
|
static void SPI_end(){
|
|
HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_SET);
|
|
}
|
|
|
|
static s8 SPI_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt){
|
|
u8 array[SPI_BUFFER_LEN]={0xFF};
|
|
u8 stringpos;
|
|
|
|
|
|
array[0] = reg_addr|0x80;
|
|
SPI_begin();
|
|
auto spi_result = HAL_SPI_TransmitReceive(spi, array, array, cnt+1, HAL_MAX_DELAY);
|
|
SPI_end();
|
|
for (stringpos = 0; stringpos < cnt; stringpos++) {
|
|
*(reg_data + stringpos) = array[stringpos+1];
|
|
}
|
|
return spi_result == HAL_OK ? 0 : -1;
|
|
}
|
|
|
|
static s8 SPI_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt){
|
|
u8 array[SPI_BUFFER_LEN * 2];
|
|
u8 stringpos = 0;
|
|
for (stringpos = 0; stringpos < cnt; stringpos++) {
|
|
|
|
array[stringpos * 2] = (reg_addr++) & 0x7F;
|
|
array[stringpos * 2 + 1] = *(reg_data + stringpos);
|
|
}
|
|
|
|
SPI_begin();
|
|
auto spi_result = HAL_SPI_Transmit(spi, array, cnt * 2, HAL_MAX_DELAY);
|
|
SPI_end();
|
|
|
|
return spi_result == HAL_OK ? 0 : -1;
|
|
}
|
|
|
|
static void delay_msek(u32 msek){
|
|
HAL_Delay(msek);
|
|
}
|
|
|
|
static s8 BMA_bind_SPI(bma2x2_t* bma) {
|
|
bma->bus_write = SPI_bus_write;
|
|
bma->bus_read = SPI_bus_read;
|
|
bma->delay_msec = delay_msek;
|
|
return 0;
|
|
}
|
|
|
|
bma2x2_t _bma2x2;
|
|
};
|
|
|
|
|
|
#endif /* SRC_BMA_ACCELEROMETER_H_ */
|