Added a function to set the units of the sensor (accelerations and angular speeds).

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);

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];