dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Add validity flag to ekf origin reporting 

Flag is false until the origin has been initialised
This commit is contained in:
Paul Riseborough 2016-12-28 10:38:05 +11:00 committed by Lorenz Meier
parent b154ce9c33
commit 934890c2df
3 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/ekf.h
View File

@ -114,7 +114,8 @@ public:
bool collect_imu(imuSample &imu); bool collect_imu(imuSample &imu);
// get the ekf WGS-84 origin position and height and the system time it was last set // get the ekf WGS-84 origin position and height and the system time it was last set
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt); // return true if the origin is valid
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning); void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);

4
EKF/ekf_helper.cpp
View File

@ -694,11 +694,13 @@ void Ekf::get_covariances(float *covariances)
} }
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) // return true if the origin is valid
bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
{ {
memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t)); memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
return _NED_origin_initialised;
} }
// return an array containing the output predictor angular, velocity and position tracking // return an array containing the output predictor angular, velocity and position tracking

5
EKF/estimator_interface.h
View File

@ -125,8 +125,9 @@ public:
*/ */
virtual void get_imu_vibe_metrics(float vibe[3]) = 0; virtual void get_imu_vibe_metrics(float vibe[3]) = 0;
// get the ekf WGS-84 origin positoin and height and the system time it was last set // get the ekf WGS-84 origin position and height and the system time it was last set
virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; // return true if the origin is valid
virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
virtual void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0; virtual void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0;