mirror of
https://github.com/bjoernellens1/cps_microros_imu.git
synced 2024-11-24 00:45:06 +00:00
400 lines
11 KiB
C++
400 lines
11 KiB
C++
// Copyright (c) 2021 Juan Miguel Jimeno
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#ifndef DEFAULT_IMU
|
|
#define DEFAULT_IMU
|
|
|
|
//include IMU base interface
|
|
#include "imu_interface.h"
|
|
|
|
//include sensor API headers
|
|
#include "I2Cdev.h"
|
|
#include "ADXL345.h"
|
|
#include "ITG3200.h"
|
|
#include "HMC5883L.h"
|
|
#include "MPU6050.h"
|
|
#include "MPU9150.h"
|
|
#include "MPU9250.h"
|
|
#include "MPU9250_WE.h"
|
|
|
|
class GY85IMU: public IMUInterface
|
|
{
|
|
private:
|
|
//constants specific to the sensor
|
|
const float accel_scale_ = 1 / 256.0;
|
|
const float gyro_scale_ = 1 / 14.375;
|
|
|
|
// driver objects to be used
|
|
ADXL345 accelerometer_;
|
|
ITG3200 gyroscope_;
|
|
|
|
// returned vector for sensor reading
|
|
geometry_msgs__msg__Vector3 accel_;
|
|
geometry_msgs__msg__Vector3 gyro_;
|
|
|
|
public:
|
|
GY85IMU()
|
|
{
|
|
// accel_cov_ = 0.001; //you can overwrite the convariance values here
|
|
// gyro_cov_ = 0.001; //you can overwrite the convariance values here
|
|
}
|
|
|
|
bool startSensor() override
|
|
{
|
|
// here you can override startSensor() function and use the sensor's driver API
|
|
// to initialize and test the sensor's connection during boot time
|
|
Wire.begin();
|
|
bool ret;
|
|
accelerometer_.initialize();
|
|
ret = accelerometer_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
gyroscope_.initialize();
|
|
ret = gyroscope_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readAccelerometer() override
|
|
{
|
|
// here you can override readAccelerometer function and use the sensor's driver API
|
|
// to grab the data from accelerometer and return as a Vector3 object
|
|
int16_t ax, ay, az;
|
|
|
|
accelerometer_.getAcceleration(&ax, &ay, &az);
|
|
|
|
accel_.x = ax * (double) accel_scale_ * g_to_accel_;
|
|
accel_.y = ay * (double) accel_scale_ * g_to_accel_;
|
|
accel_.z = az * (double) accel_scale_ * g_to_accel_;
|
|
|
|
return accel_;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readGyroscope() override
|
|
{
|
|
// here you can override readAccelerometer function and use the sensor's driver API
|
|
// to grab the data from gyroscope and return as a Vector3 object
|
|
int16_t gx, gy, gz;
|
|
|
|
gyroscope_.getRotation(&gx, &gy, &gz);
|
|
|
|
gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
|
|
|
|
return gyro_;
|
|
}
|
|
};
|
|
|
|
|
|
class MPU6050IMU: public IMUInterface
|
|
{
|
|
private:
|
|
const float accel_scale_ = 1 / 16384.0;
|
|
const float gyro_scale_ = 1 / 131.0;
|
|
|
|
MPU6050 accelerometer_;
|
|
MPU6050 gyroscope_;
|
|
|
|
geometry_msgs__msg__Vector3 accel_;
|
|
geometry_msgs__msg__Vector3 gyro_;
|
|
|
|
public:
|
|
MPU6050IMU()
|
|
{
|
|
}
|
|
|
|
bool startSensor() override
|
|
{
|
|
Wire.begin();
|
|
bool ret;
|
|
accelerometer_.initialize();
|
|
ret = accelerometer_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
gyroscope_.initialize();
|
|
ret = gyroscope_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readAccelerometer() override
|
|
{
|
|
int16_t ax, ay, az;
|
|
|
|
accelerometer_.getAcceleration(&ax, &ay, &az);
|
|
|
|
accel_.x = ax * (double) accel_scale_ * g_to_accel_;
|
|
accel_.y = ay * (double) accel_scale_ * g_to_accel_;
|
|
accel_.z = az * (double) accel_scale_ * g_to_accel_;
|
|
|
|
return accel_;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readGyroscope() override
|
|
{
|
|
int16_t gx, gy, gz;
|
|
|
|
gyroscope_.getRotation(&gx, &gy, &gz);
|
|
|
|
gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
|
|
|
|
return gyro_;
|
|
}
|
|
};
|
|
|
|
class MPU9150IMU: public IMUInterface
|
|
{
|
|
private:
|
|
const float accel_scale_ = 1 / 16384.0;
|
|
const float gyro_scale_ = 1 / 131.0;
|
|
|
|
MPU9150 accelerometer_;
|
|
MPU9150 gyroscope_;
|
|
|
|
geometry_msgs__msg__Vector3 accel_;
|
|
geometry_msgs__msg__Vector3 gyro_;
|
|
|
|
public:
|
|
MPU9150IMU()
|
|
{
|
|
}
|
|
|
|
bool startSensor() override
|
|
{
|
|
Wire.begin();
|
|
bool ret;
|
|
accelerometer_.initialize();
|
|
ret = accelerometer_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
gyroscope_.initialize();
|
|
ret = gyroscope_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readAccelerometer() override
|
|
{
|
|
int16_t ax, ay, az;
|
|
|
|
accelerometer_.getAcceleration(&ax, &ay, &az);
|
|
|
|
accel_.x = ax * (double) accel_scale_ * g_to_accel_;
|
|
accel_.y = ay * (double) accel_scale_ * g_to_accel_;
|
|
accel_.z = az * (double) accel_scale_ * g_to_accel_;
|
|
|
|
return accel_;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readGyroscope() override
|
|
{
|
|
int16_t gx, gy, gz;
|
|
|
|
gyroscope_.getRotation(&gx, &gy, &gz);
|
|
|
|
gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
|
|
|
|
return gyro_;
|
|
}
|
|
};
|
|
|
|
class MPU9250IMU: public IMUInterface
|
|
{
|
|
private:
|
|
const float accel_scale_ = 1 / 16384.0;
|
|
const float gyro_scale_ = 1 / 131.0;
|
|
|
|
MPU9250 accelerometer_;
|
|
MPU9250 gyroscope_;
|
|
|
|
geometry_msgs__msg__Vector3 accel_;
|
|
geometry_msgs__msg__Vector3 gyro_;
|
|
|
|
public:
|
|
MPU9250IMU()
|
|
{
|
|
}
|
|
|
|
bool startSensor() override
|
|
{
|
|
Wire.begin();
|
|
bool ret;
|
|
accelerometer_.initialize();
|
|
ret = accelerometer_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
gyroscope_.initialize();
|
|
ret = gyroscope_.testConnection();
|
|
if(!ret)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readAccelerometer() override
|
|
{
|
|
int16_t ax, ay, az;
|
|
|
|
accelerometer_.getAcceleration(&ax, &ay, &az);
|
|
|
|
accel_.x = ax * (double) accel_scale_ * g_to_accel_;
|
|
accel_.y = ay * (double) accel_scale_ * g_to_accel_;
|
|
accel_.z = az * (double) accel_scale_ * g_to_accel_;
|
|
|
|
return accel_;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readGyroscope() override
|
|
{
|
|
int16_t gx, gy, gz;
|
|
|
|
gyroscope_.getRotation(&gx, &gy, &gz);
|
|
|
|
gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
|
|
|
|
return gyro_;
|
|
}
|
|
};
|
|
|
|
class MPU9250IMU_WE: public IMUInterface
|
|
{
|
|
private:
|
|
const float accel_scale_ = 1 / 16384.0;
|
|
const float gyro_scale_ = 1 / 131.0;
|
|
|
|
MPU9250_WE accelerometer_;
|
|
MPU9250_WE gyroscope_;
|
|
|
|
geometry_msgs__msg__Vector3 accel_;
|
|
geometry_msgs__msg__Vector3 gyro_;
|
|
|
|
public:
|
|
MPU9250IMU_WE()
|
|
{
|
|
}
|
|
|
|
bool startSensor() override
|
|
{
|
|
Wire.begin();
|
|
bool ret;
|
|
accelerometer_.init();
|
|
ret = accelerometer_.init();
|
|
if(!ret)
|
|
return false;
|
|
|
|
gyroscope_.init();
|
|
ret = gyroscope_.init();
|
|
if(!ret)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readAccelerometer() override
|
|
{
|
|
//int16_t ax, ay, az;
|
|
|
|
xyzFloat xyz_Gval = accelerometer_.getGValues();
|
|
// ax = static_cast<int>(xyz_Gval.x); // Convert x to an integer
|
|
// ay = static_cast<int>(xyz_Gval.y); // Convert y to an integer
|
|
// az = static_cast<int>(xyz_Gval.z); // Convert z to an integer
|
|
|
|
float ax = xyz_Gval.x; // Convert x to an integer
|
|
float ay = xyz_Gval.y; // Convert y to an integer
|
|
float az = xyz_Gval.z; // Convert z to an integer
|
|
|
|
accel_.x = ax * (double) accel_scale_ * g_to_accel_;
|
|
accel_.y = ay * (double) accel_scale_ * g_to_accel_;
|
|
accel_.z = az * (double) accel_scale_ * g_to_accel_;
|
|
|
|
return accel_;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readGyroscope() override
|
|
{
|
|
//int16_t gx, gy, gz;
|
|
|
|
xyzFloat xyz_gyr = gyroscope_.getGyrValues();
|
|
// gx = static_cast<int>(xyz_gyr.x); // Convert x to an integer
|
|
// gy = static_cast<int>(xyz_gyr.y); // Convert y to an integer
|
|
// gz = static_cast<int>(xyz_gyr.z); // Convert z to an integer
|
|
|
|
float gx = xyz_gyr.x; // Convert x to an integer
|
|
float gy = xyz_gyr.y; // Convert y to an integer
|
|
float gz = xyz_gyr.z; // Convert z to an integer
|
|
|
|
gyro_.x = gx * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.y = gy * (double) gyro_scale_ * DEG_TO_RAD;
|
|
gyro_.z = gz * (double) gyro_scale_ * DEG_TO_RAD;
|
|
|
|
return gyro_;
|
|
}
|
|
};
|
|
|
|
class FakeIMU: public IMUInterface
|
|
{
|
|
private:
|
|
geometry_msgs__msg__Vector3 accel_;
|
|
geometry_msgs__msg__Vector3 gyro_;
|
|
|
|
public:
|
|
FakeIMU()
|
|
{
|
|
}
|
|
|
|
bool startSensor() override
|
|
{
|
|
return true;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readAccelerometer() override
|
|
{
|
|
return accel_;
|
|
}
|
|
|
|
geometry_msgs__msg__Vector3 readGyroscope() override
|
|
{
|
|
return gyro_;
|
|
}
|
|
};
|
|
|
|
#endif
|
|
//ADXL345 https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf
|
|
//HMC8553L https://cdn-shop.adafruit.com/datasheets/HMC5883L_3-Axis_Digital_Compass_IC.pdf
|
|
//ITG320 https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
|
|
|
|
|
|
//MPU9150 https://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
|
|
//MPU9250 https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf
|
|
//MPU6050 https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf
|
|
|
|
//http://www.sureshjoshi.com/embedded/invensense-imus-what-to-know/
|
|
//https://stackoverflow.com/questions/19161872/meaning-of-lsb-unit-and-unit-lsb
|