ekf2: estimator aid source status (starting with fake position)

This commit is contained in:
Daniel Agar
2022-03-15 10:27:46 -04:00
parent 79a34b5aed
commit 1ae467e9cd
13 changed files with 287 additions and 97 deletions
+8
View File
@@ -614,6 +614,8 @@ void EKF2::Run()
UpdateMagCalibration(now);
PublishSensorBias(now);
PublishAidSourceStatus(now);
} else {
// ekf no update
perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
@@ -632,6 +634,12 @@ void EKF2::Run()
ScheduleDelayed(100_ms);
}
void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
{
// fake position
PublishAidSourceStatus(_ekf.aid_src_fake_pos(), _status_fake_pos_pub_last, _estimator_aid_src_fake_pos_pub);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
{
if (_ekf.attitude_valid()) {