diff --git a/include/bno055_imu_driver.h b/include/bno055_imu_driver.h
index a68d95249b31fd6de6dc18f7e1a300b6275ea9ae..4ed36abc17826df8aa1d57511d3e35b78de24e0f 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 0fc0e589b79d0998606ee71c337916788b303fbe..070588526f33b564e56d778501a79372314edd3f 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];