mirror of
https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git
synced 2025-05-20 19:41:47 +02:00
113 lines
3.4 KiB
C++
113 lines
3.4 KiB
C++
#include "MotionDetection.h"
|
|
#include <cmath.h>
|
|
#include <limits.h>
|
|
MotionDetection::MotionDetection(){//:handler(FSPI){
|
|
handler = new SPIClass(FSPI);
|
|
};
|
|
|
|
void MotionDetection::begin(void){
|
|
pinMode(34,OUTPUT);
|
|
digitalWrite(34,HIGH);
|
|
handler->begin(36,37,35,34);
|
|
|
|
handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
|
|
digitalWrite(34,LOW);
|
|
// set Accel and Gyroscop to Low Noise
|
|
handler->transfer(0x1F);
|
|
handler->transfer(0x0F);
|
|
//busy Wait for startup
|
|
delayMicroseconds(200);
|
|
digitalWrite(34,HIGH);
|
|
handler->endTransaction();
|
|
};
|
|
void MotionDetection::end(void){
|
|
handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
|
|
digitalWrite(34,LOW);
|
|
handler->transfer(cmdWrite(PWR_MGMT0));
|
|
//turn Accel and Gyroscope off
|
|
handler->transfer(0x00);
|
|
digitalWrite(34,HIGH);
|
|
handler->end();
|
|
};
|
|
IMUResult MotionDetection::getAcceleration(){
|
|
IMUResult result;
|
|
result.x = readRegister(ACCEL_DATA_X_HIGH)<<8;
|
|
result.x |= readRegister(ACCEL_DATA_X_LOW);
|
|
result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8;
|
|
result.y |= readRegister(ACCEL_DATA_Y_LOW);
|
|
result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8;
|
|
result.z |= readRegister(ACCEL_DATA_Z_LOW);
|
|
return result;
|
|
};
|
|
IMUResult MotionDetection::getRotation(){
|
|
IMUResult result;
|
|
result.x = readRegister(GYRO_DATA_X_HIGH) <<8;
|
|
result.x |= readRegister(GYRO_DATA_X_LOW);
|
|
result.y = readRegister(GYRO_DATA_Y_HIGH)<<8;
|
|
result.y |= readRegister(GYRO_DATA_Y_LOW);
|
|
result.z = readRegister(GYRO_DATA_Z_HIGH)<<8;
|
|
result.z |= readRegister(GYRO_DATA_Z_LOW);
|
|
return result;
|
|
};
|
|
float MotionDetection::getTemperature(){
|
|
int16_t raw_temperatur = readRegister(REG_TEMP_HIGH)<<8;
|
|
raw_temperatur |= readRegister(REG_TEMP_LOW);
|
|
return raw_temperatur/128 +25;
|
|
};
|
|
|
|
int8_t MotionDetection::getWhoAmI(){
|
|
return readRegister(WHO_AM_I);
|
|
};
|
|
|
|
bool MotionDetection::isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis){
|
|
|
|
};
|
|
|
|
void MotionDetection::calibrateZAxis(uint gforceValue){
|
|
this->gForceCalib = gforceValue;
|
|
};
|
|
|
|
Orientation MotionDetection::getTilt(){
|
|
uint tolerance = 200;
|
|
IMUResult reading = this->getAcceleration();
|
|
bool flipped = reading.z < 0;
|
|
float accelrationStrenght = sqrt(reading.x*reading.x+reading.y*reading.y+reading.z*reading.z);
|
|
//check if reading is valid
|
|
if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
|
|
//total accelration is not gravitational force, error
|
|
return Orientation{xRotation = UINT_MAX,yRotation = UINT_MAX};
|
|
}
|
|
|
|
//calculates the angle between the two axis, therefore value between 0-90
|
|
uint yAngle;
|
|
uint xAngle;
|
|
if(result.z != 0){
|
|
yAngle = atan((result.x)/result.z)+0.5;
|
|
xAngle = atan(float(result.y)/result.z)+0.5;
|
|
} else {
|
|
yAngle = 90;
|
|
xAngle = 90;
|
|
}
|
|
|
|
|
|
//shift quadrants depending on signum
|
|
|
|
};
|
|
|
|
uint8_t MotionDetection::cmdRead(uint8_t reg){
|
|
return (CMD_READ | (reg & ADDR_MASK));
|
|
};
|
|
uint8_t MotionDetection::cmdWrite(uint8_t reg){
|
|
return (CMD_WRITE | (reg & ADDR_MASK));
|
|
};
|
|
|
|
int8_t MotionDetection::readRegister(uint8_t reg){
|
|
handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
|
|
digitalWrite(34,LOW);
|
|
uint8_t result;
|
|
result = handler->transfer(cmdRead(reg));
|
|
result = handler->transfer(0x00);
|
|
digitalWrite(34,HIGH);
|
|
handler->endTransaction();
|
|
return result;
|
|
}; |