Toggle navigation
Toggle navigation
This project
Loading...
Sign in
2021-1-capstone-design1
/
JJS_Project2
Go to a project
Toggle navigation
Toggle navigation pinning
Projects
Groups
Snippets
Help
Project
Activity
Repository
Pipelines
Graphs
Issues
0
Merge Requests
0
Wiki
Snippets
Network
Create a new issue
Builds
Commits
Issue Boards
Authored by
이충섭
2021-06-15 00:38:41 +0900
Browse Files
Options
Browse Files
Download
Email Patches
Plain Diff
Commit
9dfacf6c224dbb40305e72adbe73b21465e9817a
9dfacf6c
1 parent
f8ff5ebd
feat : detect abnormal gps
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
23 additions
and
13 deletions
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/modules/ekf2/EKF2.hpp
home/Firmware/src/modules/ekf2/EKF2.cpp
View file @
9dfacf6
...
...
@@ -1251,6 +1251,8 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
_ekf
.
setGpsData
(
gps_msg
);
if
(
hold
!=
int
(
gps_msg
.
time_usec
/
1000000
))
{
if
(
!
check_warning
())
{
dm_read
(
DM_KEY_MISSION_STATE
,
0
,
&
mission
,
sizeof
(
mission_s
));
hold
=
int
(
gps_msg
.
time_usec
/
1000000
);
if
(
hold
%
2
==
0
)
...
...
@@ -1258,7 +1260,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
calculate_inclination_target
();
print_target_gps
();
//print_current_gps(gps_msg);
check_inclination
(
calculate_inclination_current
(
gps_msg
));
double
check
=
calculate_inclination_current
(
gps_msg
);
check_inclination
(
check
);
}
}
}
_gps_time_usec
=
gps_msg
.
time_usec
;
...
...
@@ -1269,15 +1273,26 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
void
EKF2
::
check_inclination
(
double
check
)
{
double
origin
=
inclination
[
int
(
mission
.
current_seq
)
-
1
];
std
::
cout
<<
"
origin : "
<<
origin
<<
"
\t
check : "
<<
check
<<
std
::
endl
;
std
::
cout
<<
"
(origin : "
<<
origin
<<
" | check : "
<<
check
<<
")"
;
if
(
origin
-
1
<=
check
&&
check
<=
origin
+
1
)
{
std
::
cout
<<
"*----Good moving----*"
<<
std
::
endl
;
std
::
cout
<<
"
\t
--- Good moving"
<<
std
::
endl
;
warning_count
=
0
;
}
else
{
std
::
cout
<<
"*----Bad moving----*"
<<
std
::
endl
;
std
::
cout
<<
"
\t
--- Bad moving"
<<
std
::
endl
;
warning_count
++
;
}
}
bool
EKF2
::
check_warning
()
{
if
(
warning_count
>=
15
)
{
std
::
cout
<<
"* [abnormal moving detected] *"
<<
std
::
endl
;
return
true
;
}
return
false
;
}
void
EKF2
::
print_current_gps
(
gps_message
gps_msg
)
{
...
...
@@ -1292,8 +1307,6 @@ void EKF2::calculate_inclination_target()
struct
mission_item_s
next_mission_item
{};
dm_read
((
dm_item_t
)
mission
.
dataman_id
,
i
,
&
mission_item
,
sizeof
(
mission_item_s
));
dm_read
((
dm_item_t
)
mission
.
dataman_id
,
i
+
1
,
&
next_mission_item
,
sizeof
(
mission_item_s
));
//std:: cout << "next : "<<next_mission_item.lat<<std::endl;
//std:: cout << "curr : "<<mission_item.lat<<std::endl;
x
=
next_mission_item
.
lat
-
mission_item
.
lat
;
y
=
next_mission_item
.
lon
-
mission_item
.
lon
;
std
::
cout
.
setf
(
std
::
ios
::
fixed
);
...
...
@@ -1305,9 +1318,6 @@ void EKF2::calculate_inclination_target()
{
inclination
[
i
]
=
0.0
;
}
/*if(int(i)==int(mission.current_seq)-1){
std::cout <<"[inclina]#"<<i<<"\t"<<inclination[i]<<std::endl;
}*/
}
}
double
EKF2
::
calculate_inclination_current
(
gps_message
gps_msg
)
...
...
@@ -1321,10 +1331,9 @@ double EKF2::calculate_inclination_current(gps_message gps_msg)
}
std
::
cout
.
setf
(
std
::
ios
::
fixed
);
std
::
cout
.
precision
(
7
);
//std::cout << "[Mission] #"<<mission.current_seq<<"\t"<<check_inclination<<std::endl;
return
check_inclination
;
}
void
EKF2
::
prin
t_target_gps
()
void
EKF2
::
se
t_target_gps
()
{
std
::
cout
.
unsetf
(
std
::
ios
::
fixed
);
for
(
size_t
i
=
0
;
i
<
mission
.
count
;
i
++
)
{
...
...
@@ -1334,7 +1343,6 @@ void EKF2::print_target_gps()
target_lat
=
int32_t
(
mission_item
.
lat
*
long
(
pow
(
10
,
7
)));
target_lon
=
int32_t
(
mission_item
.
lon
*
long
(
pow
(
10
,
7
)));
target_alt
=
int32_t
(
mission_item
.
altitude
*
long
(
pow
(
10
,
4
)));
//std::cout<<"start\t#"<<i<< "(lat/lon/alt)\t" << target_lat<< "\t"<<target_lon<<"\t"<<target_alt<<std::endl;
}
}
}
...
...
home/Firmware/src/modules/ekf2/EKF2.hpp
View file @
9dfacf6
...
...
@@ -135,12 +135,14 @@ private:
void
UpdateMagSample
(
ekf2_timestamps_s
&
ekf2_timestamps
);
void
UpdateRangeSample
(
ekf2_timestamps_s
&
ekf2_timestamps
);
void
UpdateMagCalibration
(
const
hrt_abstime
&
timestamp
);
void
prin
t_target_gps
();
void
se
t_target_gps
();
void
print_current_gps
(
gps_message
gps_msg
);
void
calculate_inclination_target
();
double
calculate_inclination_current
(
gps_message
gps_msg
);
void
check_inclination
(
double
check
);
bool
check_warning
();
double
*
inclination
=
nullptr
;
int
warning_count
=
0
;
int32_t
target_lat
,
target_lon
,
target_alt
;
mission_s
mission
;
/*
...
...
Please
register
or
login
to post a comment