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-13 17:46:07 +0900
Browse Files
Options
Browse Files
Download
Email Patches
Plain Diff
Commit
1fd483af2cc893c2b4ef0f9cdd022318b9bae16b
1fd483af
1 parent
7065b9da
feat : caculate inclination among all of waypoints and between starting point and current position
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
81 additions
and
10 deletions
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/modules/ekf2/EKF2.hpp
home/Firmware/src/modules/ekf2/EKF2.cpp
View file @
1fd483a
...
...
@@ -40,7 +40,7 @@ using math::constrain;
using
matrix
::
Eulerf
;
using
matrix
::
Quatf
;
using
matrix
::
Vector3f
;
int
static
hold
=
0
;
pthread_mutex_t
ekf2_module_mutex
=
PTHREAD_MUTEX_INITIALIZER
;
static
px4
::
atomic
<
EKF2
*>
_objects
[
EKF2_MAX_INSTANCES
]
{};
#if !defined(CONSTRAINED_FLASH)
...
...
@@ -1509,19 +1509,83 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
+
vehicle_gps_position
.
vdop
*
vehicle_gps_position
.
vdop
),
};
_ekf
.
setGpsData
(
gps_msg
);
mission_s
mission
;
dm_read
(
DM_KEY_MISSION_STATE
,
0
,
&
mission
,
sizeof
(
mission_s
));
for
(
size_t
i
=
0
;
i
<
mission
.
count
;
i
++
)
{
struct
mission_item_s
mission_item
{};
dm_read
((
dm_item_t
)
mission
.
dataman_id
,
i
,
&
mission_item
,
sizeof
(
mission_item_s
));
std
::
cout
<<
"target
\t
"
<<
i
<<
"(lat/lon/alt)
\t
"
<<
mission_item
.
lat
<<
"
\t
"
<<
mission_item
.
lon
<<
"
\t
"
<<
mission_item
.
altitude
<<
std
::
endl
;
if
(
hold
!=
int
(
gps_msg
.
time_usec
/
1000000
))
{
dm_read
(
DM_KEY_MISSION_STATE
,
0
,
&
mission
,
sizeof
(
mission_s
));
hold
=
int
(
gps_msg
.
time_usec
/
1000000
);
if
(
hold
%
2
==
0
)
{
calculate_inclination_target
();
print_target_gps
();
//print_current_gps(gps_msg);
calculate_inclination_current
(
gps_msg
);
}
}
_gps_time_usec
=
gps_msg
.
time_usec
;
_gps_alttitude_ellipsoid
=
vehicle_gps_position
.
alt_ellipsoid
;
}
}
}
void
EKF2
::
print_current_gps
(
gps_message
gps_msg
)
{
std
::
cout
<<
"curre
\t
#"
<<
mission
.
current_seq
<<
"(lat/lon/alt)
\t
"
<<
gps_msg
.
lat
<<
"
\t
"
<<
gps_msg
.
lon
<<
"
\t
"
<<
gps_msg
.
alt
<<
std
::
endl
;
}
void
EKF2
::
calculate_inclination_target
()
{
double
x
,
y
;
inclination
=
new
double
[
int
(
mission
.
count
)];
for
(
size_t
i
=
0
;
i
<
mission
.
count
-
1
;
i
++
)
{
struct
mission_item_s
mission_item
{};
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
);
std
::
cout
.
precision
(
7
);
if
(
x
!=
0.0
){
inclination
[
i
]
=
y
/
x
;
}
else
{
inclination
[
i
]
=
0.0
;
}
/*if(int(i)==int(mission.current_seq)-1){
std::cout <<"[inclina]#"<<i<<"\t"<<inclination[i]<<std::endl;
}*/
}
}
void
EKF2
::
calculate_inclination_current
(
gps_message
gps_msg
)
{
double
x
,
y
;
double
check_inclination
=
0
;
x
=
target_lat
-
gps_msg
.
lat
;
y
=
target_lon
-
gps_msg
.
lon
;
if
(
x
!=
0.0
){
check_inclination
=
y
/
x
;
}
std
::
cout
.
setf
(
std
::
ios
::
fixed
);
std
::
cout
.
precision
(
7
);
std
::
cout
<<
"[Mission] #"
<<
mission
.
current_seq
<<
"
\t
"
<<
check_inclination
<<
std
::
endl
;
}
void
EKF2
::
print_target_gps
()
{
std
::
cout
.
unsetf
(
std
::
ios
::
fixed
);
for
(
size_t
i
=
0
;
i
<
mission
.
count
;
i
++
)
{
struct
mission_item_s
mission_item
{};
if
(
int
(
mission
.
current_seq
)
==
int
(
i
)){
dm_read
((
dm_item_t
)
mission
.
dataman_id
,
i
,
&
mission_item
,
sizeof
(
mission_item_s
));
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;
}
}
}
void
EKF2
::
UpdateMagSample
(
ekf2_timestamps_s
&
ekf2_timestamps
)
{
const
unsigned
last_generation
=
_magnetometer_sub
.
get_last_generation
();
...
...
home/Firmware/src/modules/ekf2/EKF2.hpp
View file @
1fd483a
...
...
@@ -43,7 +43,7 @@
#include "EKF2Selector.hpp"
#include <float.h>
#include <dataman/dataman.h>
#include <containers/LockGuard.hpp>
#include <drivers/drv_hrt.h>
#include <lib/ecl/EKF/ekf.h>
...
...
@@ -155,14 +155,19 @@ private:
void
UpdateRangeSample
(
ekf2_timestamps_s
&
ekf2_timestamps
);
void
UpdateMagCalibration
(
const
hrt_abstime
&
timestamp
);
void
print_target_gps
();
void
print_current_gps
(
gps_message
gps_msg
);
void
calculate_inclination_target
();
void
calculate_inclination_current
(
gps_message
gps_msg
);
double
*
inclination
=
nullptr
;
int32_t
target_lat
,
target_lon
,
target_alt
;
mission_s
mission
;
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float
filter_altitude_ellipsoid
(
float
amsl_hgt
);
static
constexpr
float
sq
(
float
x
)
{
return
x
*
x
;
};
const
bool
_replay_mode
{
false
};
///< true when we use replay data from a log
const
bool
_multi_mode
;
int
_instance
{
0
};
...
...
@@ -176,6 +181,8 @@ private:
perf_counter_t
_ecl_ekf_update_perf
{
perf_alloc
(
PC_ELAPSED
,
MODULE_NAME
": ECL update"
)};
perf_counter_t
_ecl_ekf_update_full_perf
{
perf_alloc
(
PC_ELAPSED
,
MODULE_NAME
": ECL full update"
)};
perf_counter_t
_imu_missed_perf
{
perf_alloc
(
PC_COUNT
,
MODULE_NAME
": IMU message missed"
)};
perf_counter_t
_mag_missed_perf
{
perf_alloc
(
PC_COUNT
,
MODULE_NAME
": mag message missed"
)};
// Used to check, save and use learned magnetometer biases
hrt_abstime
_mag_cal_last_us
{
0
};
///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
...
...
Please
register
or
login
to post a comment