Commit 3bdf2415 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

parent f863e499
......@@ -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);
......
......@@ -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];
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment