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-14 21:04:46 +0900
Browse Files
Options
Browse Files
Download
Plain Diff
Commit
9c485c5923ed7fa756f19e62fa08f13a27f177c2
9c485c59
2 parents
cf4c370d
6cfb5a1c
Merge branch '#1-feature-collectGPS'
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
43 additions
and
16 deletions
home/Firmware/src/lib/ecl/EKF/gps_checks.cpp
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/lib/ecl/EKF/gps_checks.cpp
View file @
9c485c5
...
...
@@ -40,7 +40,7 @@
*/
#include "ekf.h"
#include <iostream>
#include <ecl.h>
#include <geo_lookup/geo_mag_declination.h>
#include <mathlib/mathlib.h>
...
...
@@ -55,12 +55,10 @@
#define MASK_GPS_VDRIFT (1<<6)
#define MASK_GPS_HSPD (1<<7)
#define MASK_GPS_VSPD (1<<8)
bool
Ekf
::
collect_gps
(
const
gps_message
&
gps
)
{
// Run GPS checks always
_gps_checks_passed
=
gps_is_good
(
gps
);
if
(
_filter_initialised
&&
!
_NED_origin_initialised
&&
_gps_checks_passed
)
{
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
const
double
lat
=
gps
.
lat
*
1.0e-7
;
...
...
@@ -142,6 +140,7 @@ bool Ekf::collect_gps(const gps_message &gps)
}
// start collecting GPS if there is a 3D fix and the NED origin has been set
// print GPS data every 1 seocnds
return
_NED_origin_initialised
&&
(
gps
.
fix_type
>=
3
);
}
...
...
home/Firmware/src/modules/ekf2/EKF2.cpp
View file @
9c485c5
...
...
@@ -32,7 +32,7 @@
****************************************************************************/
#include "EKF2.hpp"
#include <iostream>
using
namespace
time_literals
;
using
math
::
constrain
;
using
matrix
::
Eulerf
;
...
...
@@ -44,7 +44,7 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
#if !defined(CONSTRAINED_FLASH)
static
px4
::
atomic
<
EKF2Selector
*>
_ekf2_selector
{
nullptr
};
#endif // !CONSTRAINED_FLASH
static
int
hold
=
0
;
EKF2
::
EKF2
(
bool
multi_mode
,
const
px4
::
wq_config_t
&
config
,
bool
replay_mode
)
:
ModuleParams
(
nullptr
),
ScheduledWorkItem
(
MODULE_NAME
,
config
),
...
...
@@ -169,6 +169,8 @@ EKF2::~EKF2()
{
perf_free
(
_ecl_ekf_update_perf
);
perf_free
(
_ecl_ekf_update_full_perf
);
perf_free
(
_imu_missed_perf
);
perf_free
(
_mag_missed_perf
);
}
bool
EKF2
::
multi_init
(
int
imu
,
int
mag
)
...
...
@@ -193,17 +195,18 @@ bool EKF2::multi_init(int imu, int mag)
_estimator_visual_odometry_aligned_pub
.
advertised
();
_yaw_est_pub
.
advertise
();
_vehicle_imu_sub
.
ChangeInstance
(
imu
);
_magnetometer_sub
.
ChangeInstance
(
mag
);
bool
changed_instance
=
_vehicle_imu_sub
.
ChangeInstance
(
imu
)
&&
_magnetometer_sub
.
ChangeInstance
(
mag
);
const
int
status_instance
=
_estimator_states_pub
.
get_instance
();
if
((
status_instance
>=
0
)
if
((
status_instance
>=
0
)
&&
changed_instance
&&
(
_attitude_pub
.
get_instance
()
==
status_instance
)
&&
(
_local_position_pub
.
get_instance
()
==
status_instance
)
&&
(
_global_position_pub
.
get_instance
()
==
status_instance
))
{
_instance
=
status_instance
;
ScheduleNow
();
return
true
;
}
...
...
@@ -219,6 +222,12 @@ int EKF2::print_status()
_ekf
.
local_position_is_valid
(),
_ekf
.
global_position_is_valid
());
perf_print_counter
(
_ecl_ekf_update_perf
);
perf_print_counter
(
_ecl_ekf_update_full_perf
);
perf_print_counter
(
_imu_missed_perf
);
if
(
_device_id_mag
!=
0
)
{
perf_print_counter
(
_mag_missed_perf
);
}
return
0
;
}
...
...
@@ -298,8 +307,9 @@ void EKF2::Run()
imu_updated
=
_vehicle_imu_sub
.
update
(
&
imu
);
if
(
imu_updated
&&
(
_vehicle_imu_sub
.
get_last_generation
()
!=
last_generation
+
1
))
{
PX4_ERR
(
"%d - vehicle_imu lost, generation %d -> %d"
,
_instance
,
last_generation
,
_vehicle_imu_sub
.
get_last_generation
());
perf_count
(
_imu_missed_perf
);
PX4_DEBUG
(
"%d - vehicle_imu lost, generation %d -> %d"
,
_instance
,
last_generation
,
_vehicle_imu_sub
.
get_last_generation
());
}
imu_sample_new
.
time_us
=
imu
.
timestamp_sample
;
...
...
@@ -1498,7 +1508,18 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
+
vehicle_gps_position
.
vdop
*
vehicle_gps_position
.
vdop
),
};
_ekf
.
setGpsData
(
gps_msg
);
if
(
hold
!=
int
(
gps_msg
.
time_usec
/
1000000
))
{
hold
=
int
(
gps_msg
.
time_usec
/
1000000
);
if
(
hold
%
2
==
0
)
{
std
::
cout
<<
"----------------------current--------------------------"
<<
std
::
endl
;
std
::
cout
<<
"time : "
<<
gps_msg
.
time_usec
/
1000000
<<
std
::
endl
;
std
::
cout
<<
"lat : "
<<
gps_msg
.
lat
<<
std
::
endl
;
std
::
cout
<<
"lon : "
<<
gps_msg
.
lon
<<
std
::
endl
;
std
::
cout
<<
"alt : "
<<
gps_msg
.
alt
<<
std
::
endl
;
}
}
_gps_time_usec
=
gps_msg
.
time_usec
;
_gps_alttitude_ellipsoid
=
vehicle_gps_position
.
alt_ellipsoid
;
}
...
...
@@ -1513,8 +1534,9 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
if
(
_magnetometer_sub
.
update
(
&
magnetometer
))
{
if
(
_magnetometer_sub
.
get_last_generation
()
!=
last_generation
+
1
)
{
PX4_ERR
(
"%d - vehicle_magnetometer lost, generation %d -> %d"
,
_instance
,
last_generation
,
_magnetometer_sub
.
get_last_generation
());
perf_count
(
_mag_missed_perf
);
PX4_DEBUG
(
"%d - vehicle_magnetometer lost, generation %d -> %d"
,
_instance
,
last_generation
,
_magnetometer_sub
.
get_last_generation
());
}
bool
reset
=
false
;
...
...
@@ -1738,7 +1760,8 @@ int EKF2::task_spawn(int argc, char *argv[])
while
((
multi_instances_allocated
<
multi_instances
)
&&
(
vehicle_status_sub
.
get
().
arming_state
!=
vehicle_status_s
::
ARMING_STATE_ARMED
)
&&
(
hrt_elapsed_time
(
&
time_started
)
<
30
_s
))
{
&&
((
hrt_elapsed_time
(
&
time_started
)
<
30
_s
)
||
(
vehicle_status_sub
.
get
().
hil_state
==
vehicle_status_s
::
HIL_STATE_ON
)))
{
vehicle_status_sub
.
update
();
...
...
@@ -1763,16 +1786,21 @@ int EKF2::task_spawn(int argc, char *argv[])
if
((
actual_instance
>=
0
)
&&
(
_objects
[
actual_instance
].
load
()
==
nullptr
))
{
_objects
[
actual_instance
].
store
(
ekf2_inst
);
ekf2_inst
->
ScheduleNow
();
success
=
true
;
multi_instances_allocated
++
;
ekf2_instance_created
[
imu
][
mag
]
=
true
;
if
(
actual_instance
==
0
)
{
// force selector to run immediately if first instance started
_ekf2_selector
.
load
()
->
ScheduleNow
();
}
PX4_INFO
(
"starting instance %d, IMU:%d (%d), MAG:%d (%d)"
,
actual_instance
,
imu
,
vehicle_imu_sub
.
get
().
accel_device_id
,
mag
,
vehicle_mag_sub
.
get
().
device_id
);
_ekf2_selector
.
load
()
->
ScheduleNow
();
// sleep briefly before starting more instances
px4_usleep
(
10000
);
}
else
{
PX4_ERR
(
"instance numbering problem instance: %d"
,
actual_instance
);
...
...
Please
register
or
login
to post a comment