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
gxiang
Mobilebot
Commits
a8304ab2
Commit
a8304ab2
authored
Sep 19, 2021
by
jxchenlu
Browse files
gyrodometry sensor fusion
parent
aaf6fab6
Changes
1
Hide whitespace changes
Inline
Side-by-side
common/mb_odometry.c
View file @
a8304ab2
...
...
@@ -8,7 +8,8 @@
#include
"../mobilebot/mobilebot.h"
#include
"mb_defs.h"
#include
<math.h>
#include
<stdlib.h>
typedef
enum
{
F
,
T
}
boolean
;
#define PI 3.14159265358979323846
//float enc2meters = (WHEEL_DIAMETER * PI) / (GEAR_RATIO * ENCODER_RES);
/*******************************************************************************
...
...
@@ -35,7 +36,12 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
void
mb_update_odometry
(
mb_odometry_t
*
mb_odometry
,
mb_state_t
*
mb_state
){
// mb_odometry is the input of previenc2metersous veh's position and orientation
// Reference: https://www.hmc.edu/lair/ARW/ARW-Lecture01-Odometry.pdf P28
//del_thresh Needs to be tuned
//IF DON'T WANT GYRO, SET IT AS F!
float
enc2meters
=
(
WHEEL_DIAMETER
*
PI
)
/
(
GEAR_RATIO
*
ENCODER_RES
);
float
del_thesh
=
0
.
125
/
DT
;
boolean
Gyro
=
T
;
float
del_left
=
mb_state
->
left_encoder_delta
*
1
.
0
*
enc2meters
;
float
del_right
=
mb_state
->
right_encoder_delta
*
1
.
0
*
enc2meters
;
float
del_s
=
(
del_right
-
del_left
)
/
2
;
//float del_s = (del_left + del_right) / 2;
...
...
@@ -44,8 +50,19 @@ void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
float
del_y
=
del_s
*
sin
(
mb_odometry
->
theta
+
del_theta
/
2
);
mb_odometry
->
x
+=
del_x
;
mb_odometry
->
y
+=
del_y
;
mb_odometry
->
theta
+=
del_theta
;
mb_odometry
->
theta
=
mb_clamp_radians
(
mb_odometry
->
theta
);
// Gyro: we only need to use yaw
// Since each time step is small, no need to integrate
float
del_theta_gyro
=
mb_state
->
gyro
[
2
]
*
(
PI
/
180
.
0
)
*
DT
;
float
del_go
=
del_theta_gyro
-
del_theta
;
if
(
Gyro
==
T
){
if
(
abs
(
del_go
)
>
del_thesh
){
mb_odometry
->
theta
+=
del_theta_gyro
*
DT
;}
else
{
mb_odometry
->
theta
+=
del_theta
*
DT
;}
}
else
{
// this is after we update the del_x and y
mb_odometry
->
theta
+=
del_theta
;}
mb_odometry
->
theta
=
mb_clamp_radians
(
mb_odometry
->
theta
);
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment