From 3bdf24157fa93b3c1e749bc981a215bc21891770 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Tue, 8 Jun 2021 19:04:05 +0200
Subject: [PATCH] Added a function to set the units of the sensor
(accelerations and angular speeds).
---
include/bno055_imu_driver.h | 1 +
src/bno055_imu_driver.cpp | 37 +++++++++++++++++++++++++++++++++++++
2 files changed, 38 insertions(+)
diff --git a/include/bno055_imu_driver.h b/include/bno055_imu_driver.h
index a68d952..4ed36ab 100644
--- a/include/bno055_imu_driver.h
+++ b/include/bno055_imu_driver.h
@@ -83,6 +83,7 @@ class CBNO055IMUDriver
void get_remap_axis(std::vector<char> &new_x,std::vector<char> &new_y,std::vector<char> &new_z);
void set_data_rate(double rate_hz);
double get_data_rate(void);
+ void set_sensor_units(bool degrees_s,bool m_s_s);
std::string get_new_data_event_id(void);
/* accelerometer functions */
std::vector<double> get_raw_accelerometer(void);
diff --git a/src/bno055_imu_driver.cpp b/src/bno055_imu_driver.cpp
index 0fc0e58..0705885 100644
--- a/src/bno055_imu_driver.cpp
+++ b/src/bno055_imu_driver.cpp
@@ -524,6 +524,43 @@ void CBNO055IMUDriver::get_sensor_units(void)
}
}
+void CBNO055IMUDriver::set_sensor_units(bool degrees_s,bool m_s_s)
+{
+ unsigned char units=0x00;
+
+ try{
+ if(m_s_s)
+ {
+ units&=0xFE;
+ this->accel_config.units=accel_linear;
+ }
+ else
+ {
+ units|=0x01;
+ this->accel_config.units=accel_gravity;
+ }
+ if(degrees_s)
+ {
+ units&=0xF9;
+ this->gyro_config.units=gyro_deg;
+ this->euler_units=euler_deg;
+ }
+ else
+ {
+ units|=0x06;
+ this->gyro_config.units=gyro_rad;
+ this->euler_units=euler_rad;
+ }
+ this->imu_access.enter();
+ this->change_register_page(0x00);
+ this->write_registers(0x3B,1,&units);
+ this->imu_access.exit();
+ }catch(CException &e){
+ this->imu_access.exit();
+ throw;
+ }
+}
+
void CBNO055IMUDriver::get_sensor_configs(void)
{
unsigned char configs[4];
--
GitLab