diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index b3494ee2ad..e80e24389d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -915,6 +915,79 @@ void Ekf2::task_main() } } + // publish estimator status + { + struct estimator_status_s status = {}; + status.timestamp = hrt_absolute_time(); + _ekf.get_state_delayed(status.states); + _ekf.get_covariances(status.covariances); + _ekf.get_gps_check_status(&status.gps_check_fail_flags); + _ekf.get_control_mode(&status.control_mode_flags); + _ekf.get_filter_fault_status(&status.filter_fault_flags); + _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, + &status.vel_test_ratio, &status.pos_test_ratio, + &status.hgt_test_ratio, &status.tas_test_ratio, + &status.hagl_test_ratio); + + status.pos_horiz_accuracy = lpos.eph; + status.pos_vert_accuracy = lpos.epv; + _ekf.get_ekf_soln_status(&status.solution_status_flags); + _ekf.get_imu_vibe_metrics(status.vibe); + + if (_estimator_status_pub == nullptr) { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); + + } else { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); + } + + // Publish wind estimate + struct wind_estimate_s wind_estimate = {}; + wind_estimate.timestamp = hrt_absolute_time(); + wind_estimate.windspeed_north = status.states[22]; + wind_estimate.windspeed_east = status.states[23]; + wind_estimate.covariance_north = status.covariances[22]; + wind_estimate.covariance_east = status.covariances[23]; + + if (_wind_pub == nullptr) { + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); + + } else { + orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); + } + } + + // publish estimator innovation data + { + struct ekf2_innovations_s innovations = {}; + innovations.timestamp = hrt_absolute_time(); + _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); + _ekf.get_mag_innov(&innovations.mag_innov[0]); + _ekf.get_heading_innov(&innovations.heading_innov); + _ekf.get_airspeed_innov(&innovations.airspeed_innov); + _ekf.get_beta_innov(&innovations.beta_innov); + _ekf.get_flow_innov(&innovations.flow_innov[0]); + _ekf.get_hagl_innov(&innovations.hagl_innov); + + _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); + _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]); + _ekf.get_heading_innov_var(&innovations.heading_innov_var); + _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var); + _ekf.get_beta_innov_var(&innovations.beta_innov_var); + _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]); + _ekf.get_hagl_innov_var(&innovations.hagl_innov_var); + + _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]); + + if (_estimator_innovations_pub == nullptr) { + _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); + + } else { + orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); + } + + } + } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp @@ -929,76 +1002,6 @@ void Ekf2::task_main() } } - // publish estimator status - struct estimator_status_s status = {}; - status.timestamp = _replay_mode ? now : hrt_absolute_time(); - _ekf.get_state_delayed(status.states); - _ekf.get_covariances(status.covariances); - _ekf.get_gps_check_status(&status.gps_check_fail_flags); - _ekf.get_control_mode(&status.control_mode_flags); - _ekf.get_filter_fault_status(&status.filter_fault_flags); - _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, - &status.vel_test_ratio, &status.pos_test_ratio, - &status.hgt_test_ratio, &status.tas_test_ratio, - &status.hagl_test_ratio); - bool dead_reckoning; - _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy, &dead_reckoning); - _ekf.get_ekf_soln_status(&status.solution_status_flags); - _ekf.get_imu_vibe_metrics(status.vibe); - - if (_estimator_status_pub == nullptr) { - _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); - - } else { - orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); - } - - // Publish wind estimate - struct wind_estimate_s wind_estimate = {}; - wind_estimate.timestamp = _replay_mode ? now : hrt_absolute_time(); - wind_estimate.windspeed_north = status.states[22]; - wind_estimate.windspeed_east = status.states[23]; - wind_estimate.covariance_north = status.covariances[22]; - wind_estimate.covariance_east = status.covariances[23]; - - if (_wind_pub == nullptr) { - _wind_pub = orb_advertise(ORB_ID(wind_estimate), &wind_estimate); - - } else { - orb_publish(ORB_ID(wind_estimate), _wind_pub, &wind_estimate); - } - - // publish estimator innovation data - { - struct ekf2_innovations_s innovations = {}; - innovations.timestamp = _replay_mode ? now : hrt_absolute_time(); - _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); - _ekf.get_mag_innov(&innovations.mag_innov[0]); - _ekf.get_heading_innov(&innovations.heading_innov); - _ekf.get_airspeed_innov(&innovations.airspeed_innov); - _ekf.get_beta_innov(&innovations.beta_innov); - _ekf.get_flow_innov(&innovations.flow_innov[0]); - _ekf.get_hagl_innov(&innovations.hagl_innov); - - _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); - _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]); - _ekf.get_heading_innov_var(&innovations.heading_innov_var); - _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var); - _ekf.get_beta_innov_var(&innovations.beta_innov_var); - _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]); - _ekf.get_hagl_innov_var(&innovations.hagl_innov_var); - - _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]); - - if (_estimator_innovations_pub == nullptr) { - _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); - - } else { - orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); - } - - } - // save the declination to the EKF2_MAG_DECL parameter when a land event is detected if ((_params->mag_declination_source & (1 << 1)) && !_prev_landed && vehicle_land_detected.landed) { float decl_deg;