Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
labrobotica
drivers
bno055_imu_driver
Commits
cb443ded
Commit
cb443ded
authored
Mar 23, 2020
by
Sergi Hernandez
Browse files
Merge branch 'master' of
ssh://gitlab.iri.upc.edu:2202/labrobotica/drivers/bno055_imu_driver
parents
aa74d37d
5e42e3e4
Changes
10
Hide whitespace changes
Inline
Side-by-side
.gitignore
0 → 100644
View file @
cb443ded
bin
lib
build
Findbno055_imu_sim.cmake
deleted
100644 → 0
View file @
aa74d37d
#edit the following line to add the librarie's header files
FIND_PATH
(
bno055_imu_sim_INCLUDE_DIR bno055_imu_sim.h /usr/include/iridrivers /usr/local/include/iridrivers
)
FIND_LIBRARY
(
bno055_imu_sim_LIBRARY
NAMES bno055_imu_sim
PATHS /usr/lib /usr/local/lib /usr/local/lib/iridrivers
)
IF
(
bno055_imu_sim_INCLUDE_DIR AND bno055_imu_sim_LIBRARY
)
SET
(
bno055_imu_sim_FOUND TRUE
)
ENDIF
(
bno055_imu_sim_INCLUDE_DIR AND bno055_imu_sim_LIBRARY
)
IF
(
bno055_imu_sim_FOUND
)
IF
(
NOT bno055_imu_sim_FIND_QUIETLY
)
MESSAGE
(
STATUS
"Found bno055_imu_sim:
${
bno055_imu_sim_LIBRARY
}
"
)
ENDIF
(
NOT bno055_imu_sim_FIND_QUIETLY
)
ELSE
(
bno055_imu_sim_FOUND
)
IF
(
bno055_imu_sim_FIND_REQUIRED
)
MESSAGE
(
FATAL_ERROR
"Could not find bno055_imu_sim"
)
ENDIF
(
bno055_imu_sim_FIND_REQUIRED
)
ENDIF
(
bno055_imu_sim_FOUND
)
src/CMakeLists.txt
View file @
cb443ded
...
...
@@ -2,10 +2,6 @@
SET
(
sources bno055_imu_driver.cpp bno055_imu_exceptions.cpp
)
# application header files
SET
(
headers bno055_imu_driver.h bno055_imu_exceptions.h bno055_common.h
)
# simulator source files
SET
(
sources_sim bno055_imu_sim.cpp bno055_imu_exceptions.cpp
)
# application header files
SET
(
headers_sim bno055_imu_sim.h bno055_imu_exceptions.h bno055_common.h
)
# locate the necessary dependencies
FIND_PACKAGE
(
iriutils REQUIRED
)
FIND_PACKAGE
(
comm REQUIRED
)
...
...
@@ -19,23 +15,11 @@ ADD_LIBRARY(bno055_imu_driver SHARED ${sources})
TARGET_LINK_LIBRARIES
(
bno055_imu_driver
${
iriutils_LIBRARY
}
)
TARGET_LINK_LIBRARIES
(
bno055_imu_driver
${
comm_LIBRARY
}
)
# create the shared library
ADD_LIBRARY
(
bno055_imu_sim SHARED
${
sources_sim
}
)
# link necessary libraries
TARGET_LINK_LIBRARIES
(
bno055_imu_sim
${
iriutils_LIBRARY
}
)
TARGET_LINK_LIBRARIES
(
bno055_imu_sim
${
comm_LIBRARY
}
)
INSTALL
(
TARGETS bno055_imu_driver
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers
)
INSTALL
(
FILES
${
headers
}
DESTINATION include/iridrivers
)
INSTALL
(
TARGETS bno055_imu_sim
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
ARCHIVE DESTINATION lib/iridrivers
)
INSTALL
(
FILES
${
headers_sim
}
DESTINATION include/iridrivers
)
INSTALL
(
FILES ../Findbno055_imu_driver.cmake DESTINATION
${
CMAKE_ROOT
}
/Modules/
)
INSTALL
(
FILES ../Findbno055_imu_sim.cmake DESTINATION
${
CMAKE_ROOT
}
/Modules/
)
ADD_SUBDIRECTORY
(
examples
)
src/bno055_imu_driver.cpp
View file @
cb443ded
...
...
@@ -23,6 +23,10 @@ CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
this
->
info
.
soft_ver
=
0xFFFF
;
this
->
info
.
boot_ver
=
0xFF
;
this
->
current_page
=
0x00
;
this
->
accel_calibrated
=
false
;
this
->
mag_calibrated
=
false
;
this
->
gyro_calibrated
=
false
;
this
->
system_calibrated
=
false
;
/* initialize events */
this
->
event_server
=
CEventServer
::
instance
();
this
->
finish_thread_event_id
=
this
->
imu_name
+
"_finish_thread_event"
;
...
...
@@ -43,6 +47,7 @@ CBNO055IMUDriver::CBNO055IMUDriver(const std::string &name)
this
->
euler_angles_data
.
resize
(
3
,
0.0
);
this
->
linear_accel_data
.
resize
(
3
,
0.0
);
this
->
gravity_data
.
resize
(
3
,
0.0
);
this
->
temperature
=
0.0
;
}
}
...
...
@@ -84,10 +89,20 @@ void CBNO055IMUDriver::convert_gyro_data(unsigned char *data)
void
CBNO055IMUDriver
::
convert_quaternion_data
(
unsigned
char
*
data
)
{
this
->
quaternion_data
[
3
]
=
((
signed
short
int
)(
data
[
0
]
+
(
data
[
1
]
<<
8
)))
/
16384.0
;
this
->
quaternion_data
[
0
]
=
((
signed
short
int
)(
data
[
2
]
+
(
data
[
3
]
<<
8
)))
/
16384.0
;
this
->
quaternion_data
[
1
]
=
((
signed
short
int
)(
data
[
4
]
+
(
data
[
5
]
<<
8
)))
/
16384.0
;
this
->
quaternion_data
[
2
]
=
((
signed
short
int
)(
data
[
6
]
+
(
data
[
7
]
<<
8
)))
/
16384.0
;
if
(
this
->
op_mode
>=
imu_mode
)
{
this
->
quaternion_data
[
3
]
=
((
signed
short
int
)(
data
[
0
]
+
(
data
[
1
]
<<
8
)))
/
16384.0
;
this
->
quaternion_data
[
0
]
=
((
signed
short
int
)(
data
[
2
]
+
(
data
[
3
]
<<
8
)))
/
16384.0
;
this
->
quaternion_data
[
1
]
=
((
signed
short
int
)(
data
[
4
]
+
(
data
[
5
]
<<
8
)))
/
16384.0
;
this
->
quaternion_data
[
2
]
=
((
signed
short
int
)(
data
[
6
]
+
(
data
[
7
]
<<
8
)))
/
16384.0
;
}
else
{
this
->
quaternion_data
[
3
]
=
1.0
;
this
->
quaternion_data
[
0
]
=
0.0
;
this
->
quaternion_data
[
1
]
=
0.0
;
this
->
quaternion_data
[
2
]
=
0.0
;
}
}
void
CBNO055IMUDriver
::
convert_euler_angles_data
(
unsigned
char
*
data
)
...
...
@@ -132,6 +147,34 @@ void CBNO055IMUDriver::convert_gravity_data(unsigned char *data)
}
}
void
CBNO055IMUDriver
::
convert_temperature_data
(
unsigned
char
*
data
)
{
if
(
this
->
temp_units
==
temp_celcius
)
this
->
temperature
=
(
double
)
data
[
0
];
else
this
->
temperature
=
(
double
)(
data
[
0
]
*
2.0
);
}
void
CBNO055IMUDriver
::
convert_calibration_data
(
unsigned
char
*
data
)
{
if
((
data
[
0
]
&
0x03
)
==
0x03
)
this
->
mag_calibrated
=
true
;
else
this
->
mag_calibrated
=
false
;
if
((
data
[
0
]
&
0x0C
)
==
0x0C
)
this
->
accel_calibrated
=
true
;
else
this
->
accel_calibrated
=
false
;
if
((
data
[
0
]
&
0x30
)
==
0x30
)
this
->
gyro_calibrated
=
true
;
else
this
->
gyro_calibrated
=
false
;
if
((
data
[
0
]
&
0xC0
)
==
0xC0
)
this
->
system_calibrated
=
true
;
else
this
->
system_calibrated
=
false
;
}
void
CBNO055IMUDriver
::
change_register_page
(
unsigned
char
page_id
)
{
if
(
page_id
!=
0x01
&&
page_id
!=
0x00
)
...
...
@@ -149,45 +192,22 @@ void CBNO055IMUDriver::change_register_page(unsigned char page_id)
void
CBNO055IMUDriver
::
process_data
(
unsigned
char
*
data
)
{
this
->
data_access
.
enter
();
switch
(
this
->
op_mode
)
{
case
config_mode
:
break
;
case
acc_only_mode
:
this
->
convert_accel_data
(
data
);
break
;
case
mag_only_mode
:
this
->
convert_mag_data
(
data
);
break
;
case
gyro_only_mode
:
this
->
convert_gyro_data
(
data
);
break
;
case
acc_mag_mode
:
this
->
convert_accel_data
(
data
);
this
->
convert_mag_data
(
&
data
[
6
]);
break
;
case
acc_gyro_mode
:
this
->
convert_accel_data
(
data
);
this
->
convert_gyro_data
(
&
data
[
6
]);
break
;
case
mag_gyro_mode
:
this
->
convert_mag_data
(
data
);
this
->
convert_gyro_data
(
&
data
[
6
]);
break
;
case
acc_mag_gyro_mode
:
this
->
convert_accel_data
(
data
);
this
->
convert_mag_data
(
&
data
[
6
]);
this
->
convert_gyro_data
(
&
data
[
12
]);
break
;
case
imu_mode
:
case
compass_mode
:
case
m4g_mode
:
case
ndof_off_mode
:
case
ndof_mode
:
this
->
convert_euler_angles_data
(
data
);
this
->
convert_quaternion_data
(
&
data
[
6
]);
this
->
convert_linear_accel_data
(
&
data
[
14
]);
this
->
convert_gravity_data
(
&
data
[
20
]);
break
;
}
this
->
convert_accel_data
(
data
);
this
->
convert_mag_data
(
&
data
[
6
]);
this
->
convert_gyro_data
(
&
data
[
12
]);
this
->
convert_euler_angles_data
(
&
data
[
18
]);
this
->
convert_quaternion_data
(
&
data
[
24
]);
this
->
convert_linear_accel_data
(
&
data
[
32
]);
this
->
convert_gravity_data
(
&
data
[
38
]);
this
->
convert_temperature_data
(
&
data
[
44
]);
this
->
convert_calibration_data
(
&
data
[
45
]);
this
->
data_access
.
exit
();
}
void
*
CBNO055IMUDriver
::
data_thread
(
void
*
param
)
{
CBNO055IMUDriver
*
imu
=
(
CBNO055IMUDriver
*
)
param
;
unsigned
char
address
,
length
,
*
data
;
unsigned
char
address
=
0x08
,
length
=
46
,
data
[
46
]
;
bool
end
=
false
;
while
(
!
end
)
...
...
@@ -196,49 +216,7 @@ void *CBNO055IMUDriver::data_thread(void *param)
end
=
true
;
else
{
switch
(
imu
->
op_mode
)
{
case
config_mode
:
address
=
0x00
;
length
=
0x00
;
break
;
case
acc_only_mode
:
address
=
0x08
;
length
=
6
;
data
=
new
unsigned
char
[
length
];
break
;
case
mag_only_mode
:
address
=
0x0E
;
length
=
6
;
data
=
new
unsigned
char
[
length
];
break
;
case
gyro_only_mode
:
address
=
0x14
;
length
=
6
;
data
=
new
unsigned
char
[
length
];
break
;
case
acc_mag_mode
:
address
=
0x08
;
length
=
12
;
data
=
new
unsigned
char
[
length
];
break
;
case
acc_gyro_mode
:
address
=
0x08
;
length
=
18
;
data
=
new
unsigned
char
[
length
];
break
;
case
mag_gyro_mode
:
address
=
0x0E
;
length
=
12
;
data
=
new
unsigned
char
[
length
];
break
;
case
acc_mag_gyro_mode
:
address
=
0x08
;
length
=
18
;
data
=
new
unsigned
char
[
length
];
break
;
case
imu_mode
:
case
compass_mode
:
case
m4g_mode
:
case
ndof_off_mode
:
case
ndof_mode
:
address
=
0x1A
;
length
=
26
;
data
=
new
unsigned
char
[
length
];
break
;
}
if
(
length
>
0
)
if
(
imu
->
op_mode
!=
config_mode
)
{
try
{
imu
->
imu_access
.
enter
();
...
...
@@ -246,11 +224,8 @@ void *CBNO055IMUDriver::data_thread(void *param)
imu
->
read_registers
(
address
,
length
,
data
);
imu
->
imu_access
.
exit
();
imu
->
process_data
(
data
);
delete
[]
data
;
if
(
!
imu
->
event_server
->
event_is_set
(
imu
->
new_data_event_id
))
{
imu
->
event_server
->
set_event
(
imu
->
new_data_event_id
);
}
}
catch
(
CException
&
e
){
imu
->
imu_access
.
exit
();
std
::
cout
<<
e
.
what
()
<<
std
::
endl
;
...
...
@@ -464,6 +439,7 @@ void CBNO055IMUDriver::get_sensor_configs(void)
void
CBNO055IMUDriver
::
open
(
const
std
::
string
&
serial_dev
)
{
TRS232_config
config
;
unsigned
char
cal_data
;
try
{
this
->
imu_port
=
new
CRS232
(
this
->
imu_name
+
"_serial_port"
);
...
...
@@ -477,6 +453,8 @@ void CBNO055IMUDriver::open(const std::string &serial_dev)
this
->
detect_sensor
();
/* get the current state of the device */
this
->
read_registers
(
0x07
,
1
,
&
this
->
current_page
);
this
->
read_registers
(
0x35
,
1
,
&
cal_data
);
this
->
convert_calibration_data
(
&
cal_data
);
this
->
op_mode
=
this
->
get_operation_mode
();
this
->
set_operation_mode
(
config_mode
);
this
->
power_mode
=
this
->
get_power_mode
();
...
...
@@ -533,83 +511,38 @@ void CBNO055IMUDriver::set_operation_mode(op_mode_t op_mode)
{
if
(
this
->
op_mode
!=
op_mode
)
{
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
write_registers
(
0x3D
,
1
,(
unsigned
char
*
)
&
op_mode
);
this
->
imu_access
.
exit
();
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
op_mode
=
op_mode
;
if
(
this
->
op_mode
==
config_mode
)
if
(
op_mode
==
config_mode
)
{
if
(
this
->
thread_server
->
get_thread_state
(
this
->
data_thread_id
)
==
starting
||
this
->
thread_server
->
get_thread_state
(
this
->
data_thread_id
)
==
active
)
{
this
->
event_server
->
set_event
(
this
->
finish_thread_event_id
);
this
->
thread_server
->
end_thread
(
this
->
data_thread_id
);
this
->
event_server
->
reset_event
(
this
->
finish_thread_event_id
);
}
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
write_registers
(
0x3D
,
1
,(
unsigned
char
*
)
&
op_mode
);
this
->
imu_access
.
exit
();
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
op_mode
=
op_mode
;
}
else
{
switch
(
op_mode
)
{
case
config_mode
:
break
;
case
acc_only_mode
:
this
->
raw_mag_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
raw_gyro_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
mag_only_mode
:
this
->
raw_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
raw_gyro_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
gyro_only_mode
:
this
->
raw_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
raw_mag_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
acc_mag_mode
:
this
->
raw_gyro_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
acc_gyro_mode
:
this
->
raw_mag_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
mag_gyro_mode
:
this
->
raw_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
acc_mag_gyro_mode
:
this
->
quaternion_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
euler_angles_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
linear_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
gravity_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
case
imu_mode
:
case
compass_mode
:
case
m4g_mode
:
case
ndof_off_mode
:
case
ndof_mode
:
this
->
raw_accel_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
raw_mag_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
this
->
raw_gyro_data
=
std
::
vector
<
double
>
(
3
,
0.0
);
break
;
}
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
write_registers
(
0x3D
,
1
,(
unsigned
char
*
)
&
op_mode
);
this
->
imu_access
.
exit
();
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
op_mode
=
op_mode
;
if
(
this
->
thread_server
->
get_thread_state
(
this
->
data_thread_id
)
==
attached
)
this
->
thread_server
->
start_thread
(
this
->
data_thread_id
);
}
...
...
@@ -893,21 +826,13 @@ void CBNO055IMUDriver::get_gyro_config(unsigned char *range,unsigned char *bandw
/* temperature functions */
double
CBNO055IMUDriver
::
get_temperature
(
void
)
{
unsigned
char
temperature
;
double
data
;
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
read_registers
(
0x34
,
1
,
&
temperature
);
this
->
imu_access
.
exit
();
if
(
this
->
temp_units
==
temp_celcius
)
return
(
double
)
temperature
;
else
return
(
double
)(
temperature
*=
2.0
);
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
data_access
.
enter
();
data
=
this
->
temperature
;
this
->
data_access
.
exit
();
return
data
;
}
/* fused data functions */
...
...
@@ -961,28 +886,28 @@ void CBNO055IMUDriver::load_calibration(const std::string &filename)
unsigned
char
cal_data
[
22
];
std
::
fstream
cal_file
;
try
{
cal_file
.
open
(
filename
.
c_str
(),
std
::
ios
::
in
|
std
::
ios
::
binary
);
if
(
cal_file
.
is_open
())
cal_file
.
open
(
filename
.
c_str
(),
std
::
ios
::
in
|
std
::
ios
::
binary
);
if
(
cal_file
.
is_open
())
{
cal_file
.
read
((
char
*
)
cal_data
,
22
);
if
(
cal_file
.
gcount
()
!=
22
)
throw
CBNO055IMUException
(
_HERE_
,
"Impossibel to read all data. Invalid file format"
);
else
{
cal_file
.
read
((
char
*
)
cal_data
,
22
);
if
(
cal_file
.
gcount
()
!=
22
)
throw
CBNO055IMUException
(
_HERE_
,
"Impossibel to read all data. Invalid file format"
);
else
{
cal_file
.
close
();
cal_file
.
close
();
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
write_registers
(
0x55
,
22
,
cal_data
);
this
->
imu_access
.
exit
();
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
}
else
throw
CBNO055IMUException
(
_HERE_
,
"Impossible to open the desired file"
);
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
else
throw
CBNO055IMUException
(
_HERE_
,
"Impossible to open the desired file"
);
}
void
CBNO055IMUDriver
::
save_calibration
(
const
std
::
string
&
filename
)
...
...
@@ -1011,78 +936,46 @@ void CBNO055IMUDriver::save_calibration(const std::string &filename)
bool
CBNO055IMUDriver
::
is_accelerometer_calibrated
(
void
)
{
unsigned
char
calibration
;
bool
data
;
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
read_registers
(
0x35
,
1
,
&
calibration
);
this
->
imu_access
.
exit
();
if
((
calibration
&
0x0C
)
==
0x0C
)
return
true
;
else
return
false
;
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
data_access
.
enter
();
data
=
this
->
accel_calibrated
;
this
->
data_access
.
exit
();
return
data
;
}
bool
CBNO055IMUDriver
::
is_magnetometer_calibrated
(
void
)
{
unsigned
char
calibration
;
bool
data
;
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
read_registers
(
0x35
,
1
,
&
calibration
);
this
->
imu_access
.
exit
();
if
((
calibration
&
0x03
)
==
0x03
)
return
true
;
else
return
false
;
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
data_access
.
enter
();
data
=
this
->
mag_calibrated
;
this
->
data_access
.
exit
();
return
data
;
}
bool
CBNO055IMUDriver
::
is_gyroscope_calibrated
(
void
)
{
unsigned
char
calibration
;
bool
data
;
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
read_registers
(
0x35
,
1
,
&
calibration
);
this
->
imu_access
.
exit
();
if
((
calibration
&
0x30
)
==
0x30
)
return
true
;
else
return
false
;
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
data_access
.
enter
();
data
=
this
->
gyro_calibrated
;
this
->
data_access
.
exit
();
return
data
;
}
bool
CBNO055IMUDriver
::
is_imu_calibrated
(
void
)
{
unsigned
char
calibration
;
bool
data
;
try
{
this
->
imu_access
.
enter
();
this
->
change_register_page
(
0x00
);
this
->
read_registers
(
0x35
,
1
,
&
calibration
);
this
->
imu_access
.
exit
();
if
((
calibration
&
0xC0
)
==
0xC0
)
return
true
;
else
return
false
;
}
catch
(
CException
&
e
){
this
->
imu_access
.
exit
();
throw
;
}
this
->
data_access
.
enter
();
data
=
this
->
system_calibrated
;
this
->
data_access
.
exit
();
return
data
;
}
CBNO055IMUDriver
::~
CBNO055IMUDriver
()
...
...
src/bno055_imu_driver.h
View file @
cb443ded
...
...
@@ -28,6 +28,11 @@ class CBNO055IMUDriver
std
::
string
new_data_event_id
;
/* sensor data */
CMutex
data_access
;
/* calibration data */
bool
accel_calibrated
;
bool
mag_calibrated
;
bool
gyro_calibrated
;
bool
system_calibrated
;
/* accelerometer data */
std
::
vector
<
double
>
raw_accel_data
;
TBNO055AccelConfig
accel_config
;
...
...
@@ -39,6 +44,7 @@ class CBNO055IMUDriver
TBNO055GyroConfig
gyro_config
;
/* temperature data */
temp_units_t
temp_units
;
double
temperature
;
/* fusion data */
std
::
vector
<
double
>
quaternion_data
;
std
::
vector
<
double
>
euler_angles_data
;
...
...
@@ -53,6 +59,8 @@ class CBNO055IMUDriver
void
convert_euler_angles_data
(
unsigned
char
*
data
);
void
convert_linear_accel_data
(
unsigned
char
*
data
);
void
convert_gravity_data
(
unsigned
char
*
data
);
void
convert_temperature_data
(
unsigned
char
*
data
);
void
convert_calibration_data
(
unsigned
char
*
data
);
void
change_register_page
(
unsigned
char
page_id
);
void
process_data
(
unsigned
char
*
data
);
static
void
*
data_thread
(
void
*
param
);
...
...
src/bno055_imu_sim.cpp
deleted
100644 → 0
View file @
aa74d37d
#include
"bno055_imu_sim.h"
#include
"bno055_imu_exceptions.h"
#include
"eventexceptions.h"
#include
<iostream>
#include
<fstream>
#include
<math.h>
unsigned
char
page0_data
[]
=
{
0xA0
,
0xFB
,
0x32
,
0x0F
,
// chip ID's
0x08
,
0x03
,
0x00
,
// version ID's
0x00
,
//page 0 ID
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// acceleration data
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// magnetometer data
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// gyroscope data
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// euler data
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// quaternion data
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// linear acceleration data
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
0x00
,
// gravity data
0x00
,
// temperature
0x00
,
// calibration status
0x0F
,
// ST result