2021-1-capstone-design1 / JJS_Project2

Go to a project
Toggle navigation Toggle navigation pinning
이충섭
61f3686b 1 parent 9c485c59

test : change location

Inline Side-by-side
Showing 3 changed files with 1 additions and 2013 deletions
1 -/****************************************************************************
2 - *
3 - * Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
4 - *
5 - * Redistribution and use in source and binary forms, with or without
6 - * modification, are permitted provided that the following conditions
7 - * are met:
8 - *
9 - * 1. Redistributions of source code must retain the above copyright
10 - * notice, this list of conditions and the following disclaimer.
11 - * 2. Redistributions in binary form must reproduce the above copyright
12 - * notice, this list of conditions and the following disclaimer in
13 - * the documentation and/or other materials provided with the
14 - * distribution.
15 - * 3. Neither the name PX4 nor the names of its contributors may be
16 - * used to endorse or promote products derived from this software
17 - * without specific prior written permission.
18 - *
19 - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20 - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21 - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22 - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23 - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24 - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25 - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26 - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 - * POSSIBILITY OF SUCH DAMAGE.
31 - *
32 - ****************************************************************************/
33 -
34 -#include "EKF2.hpp"
35 -#include <iostream>
36 -using namespace time_literals;
37 -using math::constrain;
38 -using matrix::Eulerf;
39 -using matrix::Quatf;
40 -using matrix::Vector3f;
41 -
42 -pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
43 -static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
44 -#if !defined(CONSTRAINED_FLASH)
45 -static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
46 -#endif // !CONSTRAINED_FLASH
47 -static int hold=0;
48 -EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
49 - ModuleParams(nullptr),
50 - ScheduledWorkItem(MODULE_NAME, config),
51 - _replay_mode(replay_mode && !multi_mode),
52 - _multi_mode(multi_mode),
53 - _instance(multi_mode ? -1 : 0),
54 - _attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
55 - _local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
56 - _global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)),
57 - _odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
58 - _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
59 - _params(_ekf.getParamHandle()),
60 - _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
61 - _param_ekf2_mag_delay(_params->mag_delay_ms),
62 - _param_ekf2_baro_delay(_params->baro_delay_ms),
63 - _param_ekf2_gps_delay(_params->gps_delay_ms),
64 - _param_ekf2_of_delay(_params->flow_delay_ms),
65 - _param_ekf2_rng_delay(_params->range_delay_ms),
66 - _param_ekf2_asp_delay(_params->airspeed_delay_ms),
67 - _param_ekf2_ev_delay(_params->ev_delay_ms),
68 - _param_ekf2_avel_delay(_params->auxvel_delay_ms),
69 - _param_ekf2_gyr_noise(_params->gyro_noise),
70 - _param_ekf2_acc_noise(_params->accel_noise),
71 - _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
72 - _param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
73 - _param_ekf2_mag_e_noise(_params->mage_p_noise),
74 - _param_ekf2_mag_b_noise(_params->magb_p_noise),
75 - _param_ekf2_wind_noise(_params->wind_vel_p_noise),
76 - _param_ekf2_terr_noise(_params->terrain_p_noise),
77 - _param_ekf2_terr_grad(_params->terrain_gradient),
78 - _param_ekf2_gps_v_noise(_params->gps_vel_noise),
79 - _param_ekf2_gps_p_noise(_params->gps_pos_noise),
80 - _param_ekf2_noaid_noise(_params->pos_noaid_noise),
81 - _param_ekf2_baro_noise(_params->baro_noise),
82 - _param_ekf2_baro_gate(_params->baro_innov_gate),
83 - _param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
84 - _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
85 - _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
86 - _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
87 - _param_ekf2_tas_gate(_params->tas_innov_gate),
88 - _param_ekf2_head_noise(_params->mag_heading_noise),
89 - _param_ekf2_mag_noise(_params->mag_noise),
90 - _param_ekf2_eas_noise(_params->eas_noise),
91 - _param_ekf2_beta_gate(_params->beta_innov_gate),
92 - _param_ekf2_beta_noise(_params->beta_noise),
93 - _param_ekf2_mag_decl(_params->mag_declination_deg),
94 - _param_ekf2_hdg_gate(_params->heading_innov_gate),
95 - _param_ekf2_mag_gate(_params->mag_innov_gate),
96 - _param_ekf2_decl_type(_params->mag_declination_source),
97 - _param_ekf2_mag_type(_params->mag_fusion_type),
98 - _param_ekf2_mag_acclim(_params->mag_acc_gate),
99 - _param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
100 - _param_ekf2_gps_check(_params->gps_check_mask),
101 - _param_ekf2_req_eph(_params->req_hacc),
102 - _param_ekf2_req_epv(_params->req_vacc),
103 - _param_ekf2_req_sacc(_params->req_sacc),
104 - _param_ekf2_req_nsats(_params->req_nsats),
105 - _param_ekf2_req_pdop(_params->req_pdop),
106 - _param_ekf2_req_hdrift(_params->req_hdrift),
107 - _param_ekf2_req_vdrift(_params->req_vdrift),
108 - _param_ekf2_aid_mask(_params->fusion_mode),
109 - _param_ekf2_hgt_mode(_params->vdist_sensor_type),
110 - _param_ekf2_terr_mask(_params->terrain_fusion_mode),
111 - _param_ekf2_noaid_tout(_params->valid_timeout_max),
112 - _param_ekf2_rng_noise(_params->range_noise),
113 - _param_ekf2_rng_sfe(_params->range_noise_scaler),
114 - _param_ekf2_rng_gate(_params->range_innov_gate),
115 - _param_ekf2_min_rng(_params->rng_gnd_clearance),
116 - _param_ekf2_rng_pitch(_params->rng_sens_pitch),
117 - _param_ekf2_rng_aid(_params->range_aid),
118 - _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
119 - _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
120 - _param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
121 - _param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
122 - _param_ekf2_evv_gate(_params->ev_vel_innov_gate),
123 - _param_ekf2_evp_gate(_params->ev_pos_innov_gate),
124 - _param_ekf2_of_n_min(_params->flow_noise),
125 - _param_ekf2_of_n_max(_params->flow_noise_qual_min),
126 - _param_ekf2_of_qmin(_params->flow_qual_min),
127 - _param_ekf2_of_gate(_params->flow_innov_gate),
128 - _param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
129 - _param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
130 - _param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
131 - _param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
132 - _param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
133 - _param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
134 - _param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
135 - _param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
136 - _param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
137 - _param_ekf2_of_pos_x(_params->flow_pos_body(0)),
138 - _param_ekf2_of_pos_y(_params->flow_pos_body(1)),
139 - _param_ekf2_of_pos_z(_params->flow_pos_body(2)),
140 - _param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
141 - _param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
142 - _param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
143 - _param_ekf2_tau_vel(_params->vel_Tau),
144 - _param_ekf2_tau_pos(_params->pos_Tau),
145 - _param_ekf2_gbias_init(_params->switch_on_gyro_bias),
146 - _param_ekf2_abias_init(_params->switch_on_accel_bias),
147 - _param_ekf2_angerr_init(_params->initial_tilt_err),
148 - _param_ekf2_abl_lim(_params->acc_bias_lim),
149 - _param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
150 - _param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
151 - _param_ekf2_abl_tau(_params->acc_bias_learn_tc),
152 - _param_ekf2_drag_noise(_params->drag_noise),
153 - _param_ekf2_bcoef_x(_params->bcoef_x),
154 - _param_ekf2_bcoef_y(_params->bcoef_y),
155 - _param_ekf2_aspd_max(_params->max_correction_airspeed),
156 - _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
157 - _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
158 - _param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
159 - _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
160 - _param_ekf2_pcoef_z(_params->static_pressure_coef_z),
161 - _param_ekf2_move_test(_params->is_moving_scaler),
162 - _param_ekf2_mag_check(_params->check_mag_strength),
163 - _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
164 - _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
165 -{
166 -}
167 -
168 -EKF2::~EKF2()
169 -{
170 - perf_free(_ecl_ekf_update_perf);
171 - perf_free(_ecl_ekf_update_full_perf);
172 - perf_free(_imu_missed_perf);
173 - perf_free(_mag_missed_perf);
174 -}
175 -
176 -bool EKF2::multi_init(int imu, int mag)
177 -{
178 - // advertise immediately to ensure consistent uORB instance numbering
179 - _attitude_pub.advertise();
180 - _local_position_pub.advertise();
181 - _global_position_pub.advertise();
182 - _odometry_pub.advertise();
183 - _wind_pub.advertise();
184 -
185 - _ekf2_timestamps_pub.advertise();
186 - _ekf_gps_drift_pub.advertise();
187 - _estimator_innovation_test_ratios_pub.advertise();
188 - _estimator_innovation_variances_pub.advertise();
189 - _estimator_innovations_pub.advertise();
190 - _estimator_optical_flow_vel_pub.advertise();
191 - _estimator_sensor_bias_pub.advertise();
192 - _estimator_states_pub.advertise();
193 - _estimator_status_pub.advertise();
194 - _estimator_status_flags_pub.advertise();
195 - _estimator_visual_odometry_aligned_pub.advertised();
196 - _yaw_est_pub.advertise();
197 -
198 - bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
199 -
200 - const int status_instance = _estimator_states_pub.get_instance();
201 -
202 - if ((status_instance >= 0) && changed_instance
203 - && (_attitude_pub.get_instance() == status_instance)
204 - && (_local_position_pub.get_instance() == status_instance)
205 - && (_global_position_pub.get_instance() == status_instance)) {
206 -
207 - _instance = status_instance;
208 -
209 - ScheduleNow();
210 - return true;
211 - }
212 -
213 - PX4_ERR("publication instance problem: %d att: %d lpos: %d gpos: %d", status_instance,
214 - _attitude_pub.get_instance(), _local_position_pub.get_instance(), _global_position_pub.get_instance());
215 -
216 - return false;
217 -}
218 -
219 -int EKF2::print_status()
220 -{
221 - PX4_INFO_RAW("ekf2:%d attitude: %d, local position: %d, global position: %d\n", _instance, _ekf.attitude_valid(),
222 - _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
223 - perf_print_counter(_ecl_ekf_update_perf);
224 - perf_print_counter(_ecl_ekf_update_full_perf);
225 - perf_print_counter(_imu_missed_perf);
226 -
227 - if (_device_id_mag != 0) {
228 - perf_print_counter(_mag_missed_perf);
229 - }
230 -
231 - return 0;
232 -}
233 -
234 -void EKF2::Run()
235 -{
236 - if (should_exit()) {
237 - _sensor_combined_sub.unregisterCallback();
238 - _vehicle_imu_sub.unregisterCallback();
239 -
240 - return;
241 - }
242 -
243 - // check for parameter updates
244 - if (_parameter_update_sub.updated() || !_callback_registered) {
245 - // clear update
246 - parameter_update_s pupdate;
247 - _parameter_update_sub.copy(&pupdate);
248 -
249 - // update parameters from storage
250 - updateParams();
251 -
252 - _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
253 -
254 - // The airspeed scale factor correcton is only available via parameter as used by the airspeed module
255 - param_t param_aspd_scale = param_find("ASPD_SCALE");
256 -
257 - if (param_aspd_scale != PARAM_INVALID) {
258 - param_get(param_aspd_scale, &_airspeed_scale_factor);
259 - }
260 - }
261 -
262 - if (!_callback_registered) {
263 - if (_multi_mode) {
264 - _callback_registered = _vehicle_imu_sub.registerCallback();
265 -
266 - } else {
267 - _callback_registered = _sensor_combined_sub.registerCallback();
268 - }
269 -
270 - if (!_callback_registered) {
271 - PX4_WARN("%d - failed to register callback, retrying", _instance);
272 - ScheduleDelayed(1_s);
273 - return;
274 - }
275 - }
276 -
277 - if (_vehicle_command_sub.updated()) {
278 - vehicle_command_s vehicle_command;
279 -
280 - if (_vehicle_command_sub.update(&vehicle_command)) {
281 - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
282 - if (!_ekf.control_status_flags().in_air) {
283 -
284 - uint64_t origin_time {};
285 - double latitude = vehicle_command.param5;
286 - double longitude = vehicle_command.param6;
287 - float altitude = vehicle_command.param7;
288 -
289 - _ekf.setEkfGlobalOrigin(latitude, longitude, altitude);
290 -
291 - // Validate the ekf origin status.
292 - _ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
293 - PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
294 - }
295 - }
296 - }
297 - }
298 -
299 - bool imu_updated = false;
300 - imuSample imu_sample_new {};
301 -
302 - hrt_abstime imu_dt = 0; // for tracking time slip later
303 -
304 - if (_multi_mode) {
305 - const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
306 - vehicle_imu_s imu;
307 - imu_updated = _vehicle_imu_sub.update(&imu);
308 -
309 - if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
310 - perf_count(_imu_missed_perf);
311 - PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
312 - _vehicle_imu_sub.get_last_generation());
313 - }
314 -
315 - imu_sample_new.time_us = imu.timestamp_sample;
316 - imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
317 - imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
318 - imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f;
319 - imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
320 -
321 - if (imu.delta_velocity_clipping > 0) {
322 - imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
323 - imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
324 - imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
325 - }
326 -
327 - imu_dt = imu.delta_angle_dt;
328 -
329 - if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
330 - _device_id_accel = imu.accel_device_id;
331 - _device_id_gyro = imu.gyro_device_id;
332 - _imu_calibration_count = imu.calibration_count;
333 -
334 - } else if ((imu.calibration_count > _imu_calibration_count)
335 - || (imu.accel_device_id != _device_id_accel)
336 - || (imu.gyro_device_id != _device_id_gyro)) {
337 -
338 - PX4_INFO("%d - resetting IMU bias", _instance);
339 - _device_id_accel = imu.accel_device_id;
340 - _device_id_gyro = imu.gyro_device_id;
341 -
342 - _ekf.resetImuBias();
343 - _imu_calibration_count = imu.calibration_count;
344 - }
345 -
346 - } else {
347 - sensor_combined_s sensor_combined;
348 - imu_updated = _sensor_combined_sub.update(&sensor_combined);
349 -
350 - imu_sample_new.time_us = sensor_combined.timestamp;
351 - imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
352 - imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
353 - imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
354 - imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
355 -
356 - if (sensor_combined.accelerometer_clipping > 0) {
357 - imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
358 - imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
359 - imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
360 - }
361 -
362 - imu_dt = sensor_combined.gyro_integral_dt;
363 -
364 - if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) {
365 - sensor_selection_s sensor_selection;
366 -
367 - if (_sensor_selection_sub.copy(&sensor_selection)) {
368 - if (_device_id_accel != sensor_selection.accel_device_id) {
369 - _ekf.resetAccelBias();
370 - _device_id_accel = sensor_selection.accel_device_id;
371 - }
372 -
373 - if (_device_id_gyro != sensor_selection.gyro_device_id) {
374 - _ekf.resetGyroBias();
375 - _device_id_gyro = sensor_selection.gyro_device_id;
376 - }
377 - }
378 - }
379 - }
380 -
381 - if (imu_updated) {
382 - const hrt_abstime now = imu_sample_new.time_us;
383 -
384 - // push imu data into estimator
385 - _ekf.setIMUData(imu_sample_new);
386 - PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
387 -
388 - // integrate time to monitor time slippage
389 - if (_start_time_us > 0) {
390 - _integrated_time_us += imu_dt;
391 - _last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us;
392 -
393 - } else {
394 - _start_time_us = imu_sample_new.time_us;
395 - _last_time_slip_us = 0;
396 - }
397 -
398 - // update all other topics if they have new data
399 - if (_status_sub.updated()) {
400 - vehicle_status_s vehicle_status;
401 -
402 - if (_status_sub.copy(&vehicle_status)) {
403 - const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
404 -
405 - // only fuse synthetic sideslip measurements if conditions are met
406 - _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
407 -
408 - // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
409 - _ekf.set_is_fixed_wing(is_fixed_wing);
410 -
411 - _preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
412 - vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
413 -
414 - _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
415 -
416 - // update standby (arming state) flag
417 - const bool standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
418 -
419 - if (_standby != standby) {
420 - _standby = standby;
421 -
422 - // reset preflight checks if transitioning in or out of standby arming state
423 - _preflt_checker.reset();
424 - }
425 - }
426 - }
427 -
428 - if (_vehicle_land_detected_sub.updated()) {
429 - vehicle_land_detected_s vehicle_land_detected;
430 -
431 - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
432 - _ekf.set_in_air_status(!vehicle_land_detected.landed);
433 -
434 - if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
435 - if (!_had_valid_terrain) {
436 - // update ground effect flag based on land detector state if we've never had valid terrain data
437 - _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
438 - }
439 -
440 - } else {
441 - _ekf.set_gnd_effect_flag(false);
442 - }
443 - }
444 - }
445 -
446 - // ekf2_timestamps (using 0.1 ms relative timestamps)
447 - ekf2_timestamps_s ekf2_timestamps {
448 - .timestamp = now,
449 - .airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
450 - .distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
451 - .optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
452 - .vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
453 - .vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
454 - .visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
455 - };
456 -
457 - UpdateAirspeedSample(ekf2_timestamps);
458 - UpdateAuxVelSample(ekf2_timestamps);
459 - UpdateBaroSample(ekf2_timestamps);
460 - UpdateGpsSample(ekf2_timestamps);
461 - UpdateMagSample(ekf2_timestamps);
462 - UpdateRangeSample(ekf2_timestamps);
463 -
464 - vehicle_odometry_s ev_odom;
465 - const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
466 -
467 - optical_flow_s optical_flow;
468 - const bool new_optical_flow = UpdateFlowSample(ekf2_timestamps, optical_flow);
469 -
470 -
471 - // run the EKF update and output
472 - const hrt_abstime ekf_update_start = hrt_absolute_time();
473 -
474 - if (_ekf.update()) {
475 - perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
476 -
477 - PublishLocalPosition(now);
478 - PublishOdometry(now, imu_sample_new);
479 - PublishGlobalPosition(now);
480 - PublishSensorBias(now);
481 - PublishWindEstimate(now);
482 -
483 - // publish status/logging messages
484 - PublishEkfDriftMetrics(now);
485 - PublishEventFlags(now);
486 - PublishStates(now);
487 - PublishStatus(now);
488 - PublishStatusFlags(now);
489 - PublishInnovations(now, imu_sample_new);
490 - PublishInnovationTestRatios(now);
491 - PublishInnovationVariances(now);
492 - PublishYawEstimatorStatus(now);
493 -
494 - UpdateMagCalibration(now);
495 -
496 - } else {
497 - // ekf no update
498 - perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
499 - }
500 -
501 - // publish external visual odometry after fixed frame alignment if new odometry is received
502 - if (new_ev_odom) {
503 - PublishOdometryAligned(now, ev_odom);
504 - }
505 -
506 - if (new_optical_flow) {
507 - PublishOpticalFlowVel(now, optical_flow);
508 - }
509 -
510 - // publish ekf2_timestamps
511 - _ekf2_timestamps_pub.publish(ekf2_timestamps);
512 - }
513 -}
514 -
515 -void EKF2::PublishAttitude(const hrt_abstime &timestamp)
516 -{
517 - if (_ekf.attitude_valid()) {
518 - // generate vehicle attitude quaternion data
519 - vehicle_attitude_s att;
520 - att.timestamp_sample = timestamp;
521 - const Quatf q{_ekf.calculate_quaternion()};
522 - q.copyTo(att.q);
523 -
524 - _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
525 - att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
526 - _attitude_pub.publish(att);
527 -
528 - } else if (_replay_mode) {
529 - // in replay mode we have to tell the replay module not to wait for an update
530 - // we do this by publishing an attitude with zero timestamp
531 - vehicle_attitude_s att{};
532 - _attitude_pub.publish(att);
533 - }
534 -}
535 -
536 -void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
537 -{
538 - // publish GPS drift data only when updated to minimise overhead
539 - float gps_drift[3];
540 - bool blocked;
541 -
542 - if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
543 - ekf_gps_drift_s drift_data;
544 - drift_data.hpos_drift_rate = gps_drift[0];
545 - drift_data.vpos_drift_rate = gps_drift[1];
546 - drift_data.hspd = gps_drift[2];
547 - drift_data.blocked = blocked;
548 - drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
549 -
550 - _ekf_gps_drift_pub.publish(drift_data);
551 - }
552 -}
553 -
554 -void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
555 -{
556 - // information events
557 - uint32_t information_events = _ekf.information_event_status().value;
558 - bool information_event_updated = false;
559 -
560 - if (information_events != 0) {
561 - information_event_updated = true;
562 - _filter_information_event_changes++;
563 - }
564 -
565 - // warning events
566 - uint32_t warning_events = _ekf.warning_event_status().value;
567 - bool warning_event_updated = false;
568 -
569 - if (warning_events != 0) {
570 - warning_event_updated = true;
571 - _filter_warning_event_changes++;
572 - }
573 -
574 - if (information_event_updated || warning_event_updated) {
575 - estimator_event_flags_s event_flags{};
576 - event_flags.timestamp_sample = timestamp;
577 -
578 - event_flags.information_event_changes = _filter_information_event_changes;
579 - event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed;
580 - event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps;
581 - event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow;
582 - event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision;
583 - event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero;
584 - event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known;
585 - event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps;
586 - event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision;
587 - event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion;
588 - event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion;
589 - event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion;
590 - event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion;
591 - event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps;
592 -
593 - event_flags.warning_event_changes = _filter_warning_event_changes;
594 - event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor;
595 - event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout;
596 - event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped;
597 - event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate;
598 - event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout;
599 - event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use;
600 - event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset;
601 - event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course;
602 - event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use;
603 - event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped;
604 - event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
605 -
606 - event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
607 - _estimator_event_flags_pub.publish(event_flags);
608 - }
609 -
610 - _ekf.clear_information_events();
611 - _ekf.clear_warning_events();
612 -}
613 -
614 -void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
615 -{
616 - if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
617 - // only publish if position has changed by at least 1 mm (map_projection_reproject is relatively expensive)
618 - const Vector3f position{_ekf.getPosition()};
619 -
620 - if ((_last_local_position_for_gpos - position).longerThan(0.001f)) {
621 - // generate and publish global position data
622 - vehicle_global_position_s global_pos;
623 - global_pos.timestamp_sample = timestamp;
624 -
625 - // Position of local NED origin in GPS / WGS84 frame
626 - map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon);
627 -
628 - float delta_xy[2];
629 - _ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
630 -
631 - global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
632 - global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
633 -
634 - // global altitude has opposite sign of local down position
635 - float delta_z;
636 - uint8_t z_reset_counter;
637 - _ekf.get_posD_reset(&delta_z, &z_reset_counter);
638 - global_pos.delta_alt = -delta_z;
639 -
640 - _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
641 -
642 - if (_ekf.isTerrainEstimateValid()) {
643 - // Terrain altitude in m, WGS84
644 - global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
645 - global_pos.terrain_alt_valid = true;
646 -
647 - } else {
648 - global_pos.terrain_alt = NAN;
649 - global_pos.terrain_alt_valid = false;
650 - }
651 -
652 - global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
653 - global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
654 - _global_position_pub.publish(global_pos);
655 -
656 - _last_local_position_for_gpos = position;
657 - }
658 - }
659 -}
660 -
661 -void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu)
662 -{
663 - // publish estimator innovation data
664 - estimator_innovations_s innovations{};
665 - innovations.timestamp_sample = timestamp;
666 - _ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
667 - _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
668 - _ekf.getBaroHgtInnov(innovations.baro_vpos);
669 - _ekf.getRngHgtInnov(innovations.rng_vpos);
670 - _ekf.getAuxVelInnov(innovations.aux_hvel);
671 - _ekf.getFlowInnov(innovations.flow);
672 - _ekf.getHeadingInnov(innovations.heading);
673 - _ekf.getMagInnov(innovations.mag_field);
674 - _ekf.getDragInnov(innovations.drag);
675 - _ekf.getAirspeedInnov(innovations.airspeed);
676 - _ekf.getBetaInnov(innovations.beta);
677 - _ekf.getHaglInnov(innovations.hagl);
678 - // Not yet supported
679 - innovations.aux_vvel = NAN;
680 -
681 - innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
682 - _estimator_innovations_pub.publish(innovations);
683 -
684 - // calculate noise filtered velocity innovations which are used for pre-flight checking
685 - if (_standby) {
686 - // TODO: move to run before publications
687 - _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
688 - _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
689 - _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
690 - _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
691 -
692 - _preflt_checker.update(imu.delta_ang_dt, innovations);
693 - }
694 -}
695 -
696 -void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
697 -{
698 - // publish estimator innovation test ratio data
699 - estimator_innovations_s test_ratios{};
700 - test_ratios.timestamp_sample = timestamp;
701 - _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
702 - test_ratios.gps_vpos);
703 - _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
704 - _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
705 - _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
706 - _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
707 - _ekf.getFlowInnovRatio(test_ratios.flow[0]);
708 - _ekf.getHeadingInnovRatio(test_ratios.heading);
709 - _ekf.getMagInnovRatio(test_ratios.mag_field[0]);
710 - _ekf.getDragInnovRatio(&test_ratios.drag[0]);
711 - _ekf.getAirspeedInnovRatio(test_ratios.airspeed);
712 - _ekf.getBetaInnovRatio(test_ratios.beta);
713 - _ekf.getHaglInnovRatio(test_ratios.hagl);
714 - // Not yet supported
715 - test_ratios.aux_vvel = NAN;
716 -
717 - test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
718 - _estimator_innovation_test_ratios_pub.publish(test_ratios);
719 -}
720 -
721 -void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
722 -{
723 - // publish estimator innovation variance data
724 - estimator_innovations_s variances{};
725 - variances.timestamp_sample = timestamp;
726 - _ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
727 - _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
728 - _ekf.getBaroHgtInnovVar(variances.baro_vpos);
729 - _ekf.getRngHgtInnovVar(variances.rng_vpos);
730 - _ekf.getAuxVelInnovVar(variances.aux_hvel);
731 - _ekf.getFlowInnovVar(variances.flow);
732 - _ekf.getHeadingInnovVar(variances.heading);
733 - _ekf.getMagInnovVar(variances.mag_field);
734 - _ekf.getDragInnovVar(variances.drag);
735 - _ekf.getAirspeedInnovVar(variances.airspeed);
736 - _ekf.getBetaInnovVar(variances.beta);
737 - _ekf.getHaglInnovVar(variances.hagl);
738 - // Not yet supported
739 - variances.aux_vvel = NAN;
740 -
741 - variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
742 - _estimator_innovation_variances_pub.publish(variances);
743 -}
744 -
745 -void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
746 -{
747 - vehicle_local_position_s lpos;
748 - // generate vehicle local position data
749 - lpos.timestamp_sample = timestamp;
750 -
751 - // Position of body origin in local NED frame
752 - const Vector3f position{_ekf.getPosition()};
753 - lpos.x = position(0);
754 - lpos.y = position(1);
755 - lpos.z = position(2);
756 -
757 - // Velocity of body origin in local NED frame (m/s)
758 - const Vector3f velocity{_ekf.getVelocity()};
759 - lpos.vx = velocity(0);
760 - lpos.vy = velocity(1);
761 - lpos.vz = velocity(2);
762 -
763 - // vertical position time derivative (m/s)
764 - lpos.z_deriv = _ekf.getVerticalPositionDerivative();
765 -
766 - // Acceleration of body origin in local frame
767 - const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
768 - lpos.ax = vel_deriv(0);
769 - lpos.ay = vel_deriv(1);
770 - lpos.az = vel_deriv(2);
771 -
772 - // TODO: better status reporting
773 - lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
774 - lpos.z_valid = !_preflt_checker.hasVertFailed();
775 - lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
776 - lpos.v_z_valid = !_preflt_checker.hasVertFailed();
777 -
778 - // Position of local NED origin in GPS / WGS84 frame
779 - if (_ekf.global_origin_valid()) {
780 - lpos.ref_timestamp = _ekf.global_origin().timestamp;
781 - lpos.ref_lat = math::degrees(_ekf.global_origin().lat_rad); // Reference point latitude in degrees
782 - lpos.ref_lon = math::degrees(_ekf.global_origin().lon_rad); // Reference point longitude in degrees
783 - lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude(); // Reference point in MSL altitude meters
784 - lpos.xy_global = true;
785 - lpos.z_global = true;
786 -
787 - } else {
788 - lpos.ref_timestamp = 0;
789 - lpos.ref_lat = static_cast<double>(NAN);
790 - lpos.ref_lon = static_cast<double>(NAN);
791 - lpos.ref_alt = NAN;
792 - lpos.xy_global = false;
793 - lpos.z_global = false;
794 - }
795 -
796 - Quatf delta_q_reset;
797 - _ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
798 -
799 - lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
800 - lpos.delta_heading = Eulerf(delta_q_reset).psi();
801 -
802 - // Distance to bottom surface (ground) in meters
803 - // constrain the distance to ground to _rng_gnd_clearance
804 - lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
805 - lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
806 - lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
807 -
808 - if (!_had_valid_terrain) {
809 - _had_valid_terrain = lpos.dist_bottom_valid;
810 - }
811 -
812 - // only consider ground effect if compensation is configured and the vehicle is armed (props spinning)
813 - if ((_param_ekf2_gnd_eff_dz.get() > 0.0f) && _armed && lpos.dist_bottom_valid) {
814 - // set ground effect flag if vehicle is closer than a specified distance to the ground
815 - _ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get());
816 -
817 - // if we have no valid terrain estimate and never had one then use ground effect flag from land detector
818 - // _had_valid_terrain is used to make sure that we don't fall back to using this option
819 - // if we temporarily lose terrain data due to the distance sensor getting out of range
820 - }
821 -
822 - _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
823 - _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
824 -
825 - // get state reset information of position and velocity
826 - _ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
827 - _ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
828 - _ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
829 - _ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
830 -
831 - // get control limit information
832 - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
833 -
834 - // convert NaN to INFINITY
835 - if (!PX4_ISFINITE(lpos.vxy_max)) {
836 - lpos.vxy_max = INFINITY;
837 - }
838 -
839 - if (!PX4_ISFINITE(lpos.vz_max)) {
840 - lpos.vz_max = INFINITY;
841 - }
842 -
843 - if (!PX4_ISFINITE(lpos.hagl_min)) {
844 - lpos.hagl_min = INFINITY;
845 - }
846 -
847 - if (!PX4_ISFINITE(lpos.hagl_max)) {
848 - lpos.hagl_max = INFINITY;
849 - }
850 -
851 - // publish vehicle local position data
852 - lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
853 - _local_position_pub.publish(lpos);
854 -}
855 -
856 -void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
857 -{
858 - // generate vehicle odometry data
859 - vehicle_odometry_s odom;
860 - odom.timestamp_sample = imu.time_us;
861 -
862 - odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
863 -
864 - // Vehicle odometry position
865 - const Vector3f position{_ekf.getPosition()};
866 - odom.x = position(0);
867 - odom.y = position(1);
868 - odom.z = position(2);
869 -
870 - // Vehicle odometry linear velocity
871 - odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
872 - const Vector3f velocity{_ekf.getVelocity()};
873 - odom.vx = velocity(0);
874 - odom.vy = velocity(1);
875 - odom.vz = velocity(2);
876 -
877 - // Vehicle odometry quaternion
878 - _ekf.getQuaternion().copyTo(odom.q);
879 -
880 - // Vehicle odometry angular rates
881 - const Vector3f gyro_bias{_ekf.getGyroBias()};
882 - const Vector3f rates{imu.delta_ang / imu.delta_ang_dt};
883 - odom.rollspeed = rates(0) - gyro_bias(0);
884 - odom.pitchspeed = rates(1) - gyro_bias(1);
885 - odom.yawspeed = rates(2) - gyro_bias(2);
886 -
887 - // get the covariance matrix size
888 - static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
889 - static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
890 -
891 - // Get covariances to vehicle odometry
892 - float covariances[24];
893 - _ekf.covariances_diagonal().copyTo(covariances);
894 -
895 - // initially set pose covariances to 0
896 - for (size_t i = 0; i < POS_URT_SIZE; i++) {
897 - odom.pose_covariance[i] = 0.0;
898 - }
899 -
900 - // set the position variances
901 - odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
902 - odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
903 - odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
904 -
905 - // TODO: implement propagation from quaternion covariance to Euler angle covariance
906 - // by employing the covariance law
907 -
908 - // initially set velocity covariances to 0
909 - for (size_t i = 0; i < VEL_URT_SIZE; i++) {
910 - odom.velocity_covariance[i] = 0.0;
911 - }
912 -
913 - // set the linear velocity variances
914 - odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
915 - odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
916 - odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
917 -
918 - // publish vehicle odometry data
919 - odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
920 - _odometry_pub.publish(odom);
921 -}
922 -
923 -void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom)
924 -{
925 - const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame
926 - const Dcmf ev_rot_mat(quat_ev2ekf);
927 -
928 - vehicle_odometry_s aligned_ev_odom{ev_odom};
929 -
930 - // Rotate external position and velocity into EKF navigation frame
931 - const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z);
932 - aligned_ev_odom.x = aligned_pos(0);
933 - aligned_ev_odom.y = aligned_pos(1);
934 - aligned_ev_odom.z = aligned_pos(2);
935 -
936 - switch (ev_odom.velocity_frame) {
937 - case vehicle_odometry_s::BODY_FRAME_FRD: {
938 - const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
939 - aligned_ev_odom.vx = aligned_vel(0);
940 - aligned_ev_odom.vy = aligned_vel(1);
941 - aligned_ev_odom.vz = aligned_vel(2);
942 - break;
943 - }
944 -
945 - case vehicle_odometry_s::LOCAL_FRAME_FRD: {
946 - const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
947 - aligned_ev_odom.vx = aligned_vel(0);
948 - aligned_ev_odom.vy = aligned_vel(1);
949 - aligned_ev_odom.vz = aligned_vel(2);
950 - break;
951 - }
952 - }
953 -
954 - aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
955 -
956 - // Compute orientation in EKF navigation frame
957 - Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ;
958 - ev_quat_aligned.normalize();
959 -
960 - ev_quat_aligned.copyTo(aligned_ev_odom.q);
961 - quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
962 -
963 - _estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
964 -}
965 -
966 -void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
967 -{
968 - // estimator_sensor_bias
969 - estimator_sensor_bias_s bias{};
970 - bias.timestamp_sample = timestamp;
971 -
972 - const Vector3f gyro_bias{_ekf.getGyroBias()};
973 - const Vector3f accel_bias{_ekf.getAccelBias()};
974 - const Vector3f mag_bias{_mag_cal_last_bias};
975 -
976 - // only publish on change
977 - if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
978 - || (accel_bias - _last_accel_bias_published).longerThan(0.001f)
979 - || (mag_bias - _last_mag_bias_published).longerThan(0.001f)) {
980 -
981 - // take device ids from sensor_selection_s if not using specific vehicle_imu_s
982 - if (_device_id_gyro != 0) {
983 - bias.gyro_device_id = _device_id_gyro;
984 - gyro_bias.copyTo(bias.gyro_bias);
985 - bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
986 - _ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
987 - bias.gyro_bias_valid = true;
988 -
989 - _last_gyro_bias_published = gyro_bias;
990 - }
991 -
992 - if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
993 - bias.accel_device_id = _device_id_accel;
994 - accel_bias.copyTo(bias.accel_bias);
995 - bias.accel_bias_limit = _params->acc_bias_lim;
996 - _ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
997 - bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
998 -
999 - _last_accel_bias_published = accel_bias;
1000 - }
1001 -
1002 - if (_device_id_mag != 0) {
1003 - bias.mag_device_id = _device_id_mag;
1004 - mag_bias.copyTo(bias.mag_bias);
1005 - bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
1006 - _mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
1007 - bias.mag_bias_valid = _mag_cal_available;
1008 -
1009 - _last_mag_bias_published = mag_bias;
1010 - }
1011 -
1012 - bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1013 - _estimator_sensor_bias_pub.publish(bias);
1014 - }
1015 -}
1016 -
1017 -void EKF2::PublishStates(const hrt_abstime &timestamp)
1018 -{
1019 - // publish estimator states
1020 - estimator_states_s states;
1021 - states.timestamp_sample = timestamp;
1022 - states.n_states = 24;
1023 - _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
1024 - _ekf.covariances_diagonal().copyTo(states.covariances);
1025 - states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1026 - _estimator_states_pub.publish(states);
1027 -}
1028 -
1029 -void EKF2::PublishStatus(const hrt_abstime &timestamp)
1030 -{
1031 - estimator_status_s status{};
1032 - status.timestamp_sample = timestamp;
1033 -
1034 - _ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
1035 -
1036 - _ekf.get_gps_check_status(&status.gps_check_fail_flags);
1037 -
1038 - // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
1039 - // the GPS Fix bit, which is always checked)
1040 - status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
1041 -
1042 - status.control_mode_flags = _ekf.control_status().value;
1043 - status.filter_fault_flags = _ekf.fault_status().value;
1044 -
1045 - uint16_t innov_check_flags_temp = 0;
1046 - _ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio,
1047 - status.vel_test_ratio, status.pos_test_ratio,
1048 - status.hgt_test_ratio, status.tas_test_ratio,
1049 - status.hagl_test_ratio, status.beta_test_ratio);
1050 -
1051 - // Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition
1052 - // TODO: legacy use only, those flags are also in estimator_status_flags
1053 - status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1);
1054 -
1055 - _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
1056 - _ekf.get_ekf_soln_status(&status.solution_status_flags);
1057 - _ekf.getImuVibrationMetrics().copyTo(status.vibe);
1058 -
1059 - // reset counters
1060 - status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
1061 - status.reset_count_vel_d = _ekf.state_reset_status().velD_counter;
1062 - status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter;
1063 - status.reset_count_pod_d = _ekf.state_reset_status().posD_counter;
1064 - status.reset_count_quat = _ekf.state_reset_status().quat_counter;
1065 -
1066 - status.time_slip = _last_time_slip_us * 1e-6f;
1067 -
1068 - status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
1069 - status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
1070 - status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
1071 - status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
1072 - status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
1073 -
1074 - status.accel_device_id = _device_id_accel;
1075 - status.baro_device_id = _device_id_baro;
1076 - status.gyro_device_id = _device_id_gyro;
1077 - status.mag_device_id = _device_id_mag;
1078 -
1079 - status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1080 - _estimator_status_pub.publish(status);
1081 -}
1082 -
1083 -void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
1084 -{
1085 - // publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
1086 - bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s);
1087 -
1088 - // filter control status
1089 - if (_ekf.control_status().value != _filter_control_status) {
1090 - update = true;
1091 - _filter_control_status = _ekf.control_status().value;
1092 - _filter_control_status_changes++;
1093 - }
1094 -
1095 - // filter fault status
1096 - if (_ekf.fault_status().value != _filter_fault_status) {
1097 - update = true;
1098 - _filter_fault_status = _ekf.fault_status().value;
1099 - _filter_fault_status_changes++;
1100 - }
1101 -
1102 - // innovation check fail status
1103 - if (_ekf.innov_check_fail_status().value != _innov_check_fail_status) {
1104 - update = true;
1105 - _innov_check_fail_status = _ekf.innov_check_fail_status().value;
1106 - _innov_check_fail_status_changes++;
1107 - }
1108 -
1109 - if (update) {
1110 - estimator_status_flags_s status_flags{};
1111 - status_flags.timestamp_sample = timestamp;
1112 -
1113 - status_flags.control_status_changes = _filter_control_status_changes;
1114 - status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align;
1115 - status_flags.cs_yaw_align = _ekf.control_status_flags().yaw_align;
1116 - status_flags.cs_gps = _ekf.control_status_flags().gps;
1117 - status_flags.cs_opt_flow = _ekf.control_status_flags().opt_flow;
1118 - status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg;
1119 - status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D;
1120 - status_flags.cs_mag_dec = _ekf.control_status_flags().mag_dec;
1121 - status_flags.cs_in_air = _ekf.control_status_flags().in_air;
1122 - status_flags.cs_wind = _ekf.control_status_flags().wind;
1123 - status_flags.cs_baro_hgt = _ekf.control_status_flags().baro_hgt;
1124 - status_flags.cs_rng_hgt = _ekf.control_status_flags().rng_hgt;
1125 - status_flags.cs_gps_hgt = _ekf.control_status_flags().gps_hgt;
1126 - status_flags.cs_ev_pos = _ekf.control_status_flags().ev_pos;
1127 - status_flags.cs_ev_yaw = _ekf.control_status_flags().ev_yaw;
1128 - status_flags.cs_ev_hgt = _ekf.control_status_flags().ev_hgt;
1129 - status_flags.cs_fuse_beta = _ekf.control_status_flags().fuse_beta;
1130 - status_flags.cs_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
1131 - status_flags.cs_fixed_wing = _ekf.control_status_flags().fixed_wing;
1132 - status_flags.cs_mag_fault = _ekf.control_status_flags().mag_fault;
1133 - status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd;
1134 - status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect;
1135 - status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck;
1136 - status_flags.cs_gps_yaw = _ekf.control_status_flags().gps_yaw;
1137 - status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
1138 - status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel;
1139 - status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z;
1140 - status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest;
1141 -
1142 - status_flags.fault_status_changes = _filter_fault_status_changes;
1143 - status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;
1144 - status_flags.fs_bad_mag_y = _ekf.fault_status_flags().bad_mag_y;
1145 - status_flags.fs_bad_mag_z = _ekf.fault_status_flags().bad_mag_z;
1146 - status_flags.fs_bad_hdg = _ekf.fault_status_flags().bad_hdg;
1147 - status_flags.fs_bad_mag_decl = _ekf.fault_status_flags().bad_mag_decl;
1148 - status_flags.fs_bad_airspeed = _ekf.fault_status_flags().bad_airspeed;
1149 - status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip;
1150 - status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X;
1151 - status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y;
1152 - status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N;
1153 - status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E;
1154 - status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D;
1155 - status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N;
1156 - status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E;
1157 - status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D;
1158 - status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias;
1159 - status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical;
1160 - status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping;
1161 -
1162 - status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes;
1163 - status_flags.reject_hor_vel = _ekf.innov_check_fail_status_flags().reject_hor_vel;
1164 - status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel;
1165 - status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos;
1166 - status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos;
1167 - status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x;
1168 - status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y;
1169 - status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z;
1170 - status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw;
1171 - status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed;
1172 - status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip;
1173 - status_flags.reject_hagl = _ekf.innov_check_fail_status_flags().reject_hagl;
1174 - status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X;
1175 - status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
1176 -
1177 - status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1178 - _estimator_status_flags_pub.publish(status_flags);
1179 -
1180 - _last_status_flag_update = status_flags.timestamp;
1181 - }
1182 -}
1183 -
1184 -void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
1185 -{
1186 - static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
1187 - "yaw_estimator_status_s::yaw wrong size");
1188 -
1189 - yaw_estimator_status_s yaw_est_test_data;
1190 -
1191 - if (_ekf.getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance,
1192 - yaw_est_test_data.yaw,
1193 - yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve,
1194 - yaw_est_test_data.weight)) {
1195 -
1196 - yaw_est_test_data.timestamp_sample = timestamp;
1197 - yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1198 -
1199 - _yaw_est_pub.publish(yaw_est_test_data);
1200 - }
1201 -}
1202 -
1203 -void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
1204 -{
1205 - if (_ekf.get_wind_status()) {
1206 - // Publish wind estimate only if ekf declares them valid
1207 - wind_s wind{};
1208 - wind.timestamp_sample = timestamp;
1209 -
1210 - const Vector2f wind_vel = _ekf.getWindVelocity();
1211 - const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
1212 - _ekf.getAirspeedInnov(wind.tas_innov);
1213 - _ekf.getAirspeedInnovVar(wind.tas_innov_var);
1214 - _ekf.getBetaInnov(wind.beta_innov);
1215 - _ekf.getBetaInnovVar(wind.beta_innov_var);
1216 -
1217 - wind.windspeed_north = wind_vel(0);
1218 - wind.windspeed_east = wind_vel(1);
1219 - wind.variance_north = wind_vel_var(0);
1220 - wind.variance_east = wind_vel_var(1);
1221 - wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1222 -
1223 - _wind_pub.publish(wind);
1224 - }
1225 -}
1226 -
1227 -void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &flow_sample)
1228 -{
1229 - estimator_optical_flow_vel_s flow_vel{};
1230 - flow_vel.timestamp_sample = flow_sample.timestamp;
1231 -
1232 - _ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
1233 - _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
1234 - _ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
1235 - _ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
1236 - _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
1237 - flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
1238 -
1239 - _estimator_optical_flow_vel_pub.publish(flow_vel);
1240 -}
1241 -
1242 -float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
1243 -{
1244 - float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
1245 -
1246 - if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {
1247 -
1248 - _wgs84_hgt_offset = height_diff;
1249 - _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
1250 -
1251 - } else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {
1252 -
1253 - // apply a 10 second first order low pass filter to baro offset
1254 - float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
1255 - _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
1256 - float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
1257 - _wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
1258 - }
1259 -
1260 - return amsl_hgt + _wgs84_hgt_offset;
1261 -}
1262 -
1263 -void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
1264 -{
1265 - // EKF airspeed sample
1266 - airspeed_s airspeed;
1267 -
1268 - if (_airspeed_sub.update(&airspeed)) {
1269 - // The airspeed measurement received via the airspeed.msg topic has not been corrected
1270 - // for scale favtor errors and requires the ASPD_SCALE correction to be applied.
1271 - // This could be avoided if true_airspeed_m_s from the airspeed-validated.msg topic
1272 - // was used instead, however this would introduce a potential circular dependency
1273 - // via the wind estimator that uses EKF velocity estimates.
1274 - const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
1275 -
1276 - // only set airspeed data if condition for airspeed fusion are met
1277 - if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
1278 -
1279 - airspeedSample airspeed_sample {
1280 - .time_us = airspeed.timestamp,
1281 - .true_airspeed = true_airspeed_m_s,
1282 - .eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
1283 - };
1284 - _ekf.setAirspeedData(airspeed_sample);
1285 - }
1286 -
1287 - ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
1288 - (int64_t)ekf2_timestamps.timestamp / 100);
1289 - }
1290 -}
1291 -
1292 -void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
1293 -{
1294 - // EKF auxillary velocity sample
1295 - // - use the landing target pose estimate as another source of velocity data
1296 - const unsigned last_generation = _landing_target_pose_sub.get_last_generation();
1297 - landing_target_pose_s landing_target_pose;
1298 -
1299 - if (_landing_target_pose_sub.update(&landing_target_pose)) {
1300 -
1301 - if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) {
1302 - PX4_ERR("%d - landing_target_pose lost, generation %d -> %d", _instance, last_generation,
1303 - _landing_target_pose_sub.get_last_generation());
1304 - }
1305 -
1306 - // we can only use the landing target if it has a fixed position and a valid velocity estimate
1307 - if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
1308 - // velocity of vehicle relative to target has opposite sign to target relative to vehicle
1309 - auxVelSample auxvel_sample{
1310 - .time_us = landing_target_pose.timestamp,
1311 - .vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
1312 - .velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
1313 - };
1314 - _ekf.setAuxVelData(auxvel_sample);
1315 - }
1316 - }
1317 -}
1318 -
1319 -void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
1320 -{
1321 - // EKF baro sample
1322 - vehicle_air_data_s airdata;
1323 -
1324 - if (_airdata_sub.update(&airdata)) {
1325 - _ekf.set_air_density(airdata.rho);
1326 -
1327 - _ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
1328 -
1329 - _device_id_baro = airdata.baro_device_id;
1330 -
1331 - ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
1332 - (int64_t)ekf2_timestamps.timestamp / 100);
1333 - }
1334 -}
1335 -
1336 -bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
1337 -{
1338 - bool new_ev_odom = false;
1339 - const unsigned last_generation = _ev_odom_sub.get_last_generation();
1340 -
1341 - // EKF external vision sample
1342 - if (_ev_odom_sub.update(&ev_odom)) {
1343 -
1344 - if (_ev_odom_sub.get_last_generation() != last_generation + 1) {
1345 - PX4_ERR("%d - vehicle_visual_odometry lost, generation %d -> %d", _instance, last_generation,
1346 - _ev_odom_sub.get_last_generation());
1347 - }
1348 -
1349 - if (_param_ekf2_aid_mask.get() & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
1350 -
1351 - extVisionSample ev_data{};
1352 -
1353 - // if error estimates are unavailable, use parameter defined defaults
1354 -
1355 - // check for valid velocity data
1356 - if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
1357 - ev_data.vel(0) = ev_odom.vx;
1358 - ev_data.vel(1) = ev_odom.vy;
1359 - ev_data.vel(2) = ev_odom.vz;
1360 -
1361 - if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
1362 - ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
1363 -
1364 - } else {
1365 - ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
1366 - }
1367 -
1368 - // velocity measurement error from ev_data or parameters
1369 - float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
1370 -
1371 - if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
1372 - && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
1373 - && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
1374 - ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
1375 - ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
1376 - ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
1377 - ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
1378 - ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
1379 - ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
1380 -
1381 - } else {
1382 - ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
1383 - }
1384 - }
1385 -
1386 - // check for valid position data
1387 - if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
1388 - ev_data.pos(0) = ev_odom.x;
1389 - ev_data.pos(1) = ev_odom.y;
1390 - ev_data.pos(2) = ev_odom.z;
1391 -
1392 - float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
1393 -
1394 - // position measurement error from ev_data or parameters
1395 - if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
1396 - && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
1397 - && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
1398 - ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
1399 - ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
1400 - ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
1401 -
1402 - } else {
1403 - ev_data.posVar.setAll(param_evp_noise_var);
1404 - }
1405 - }
1406 -
1407 - // check for valid orientation data
1408 - if (PX4_ISFINITE(ev_odom.q[0])) {
1409 - ev_data.quat = Quatf(ev_odom.q);
1410 -
1411 - // orientation measurement error from ev_data or parameters
1412 - float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
1413 -
1414 - if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
1415 - ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
1416 -
1417 - } else {
1418 - ev_data.angVar = param_eva_noise_var;
1419 - }
1420 - }
1421 -
1422 - // use timestamp from external computer, clocks are synchronized when using MAVROS
1423 - ev_data.time_us = ev_odom.timestamp_sample;
1424 - _ekf.setExtVisionData(ev_data);
1425 -
1426 - new_ev_odom = true;
1427 - }
1428 -
1429 - ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
1430 - (int64_t)ekf2_timestamps.timestamp / 100);
1431 - }
1432 -
1433 - return new_ev_odom;
1434 -}
1435 -
1436 -bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow)
1437 -{
1438 - bool new_optical_flow = false;
1439 - const unsigned last_generation = _optical_flow_sub.get_last_generation();
1440 -
1441 - if (_optical_flow_sub.update(&optical_flow)) {
1442 -
1443 - if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
1444 - PX4_ERR("%d - optical_flow lost, generation %d -> %d", _instance, last_generation,
1445 - _optical_flow_sub.get_last_generation());
1446 - }
1447 -
1448 - if (_param_ekf2_aid_mask.get() & MASK_USE_OF) {
1449 -
1450 - flowSample flow {
1451 - .time_us = optical_flow.timestamp,
1452 - // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
1453 - // is produced by a RH rotation of the image about the sensor axis.
1454 - .flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral},
1455 - .gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
1456 - .dt = 1e-6f * (float)optical_flow.integration_timespan,
1457 - .quality = optical_flow.quality,
1458 - };
1459 -
1460 - if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
1461 - PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
1462 - flow.dt < 1) {
1463 -
1464 - // Save sensor limits reported by the optical flow sensor
1465 - _ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
1466 - optical_flow.max_ground_distance);
1467 -
1468 - _ekf.setOpticalFlowData(flow);
1469 -
1470 - new_optical_flow = true;
1471 - }
1472 - }
1473 -
1474 - ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
1475 - (int64_t)ekf2_timestamps.timestamp / 100);
1476 - }
1477 -
1478 - return new_optical_flow;
1479 -}
1480 -
1481 -void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
1482 -{
1483 - // EKF GPS message
1484 - if (_param_ekf2_aid_mask.get() & MASK_USE_GPS) {
1485 - vehicle_gps_position_s vehicle_gps_position;
1486 -
1487 - if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
1488 - gps_message gps_msg{
1489 - .time_usec = vehicle_gps_position.timestamp,
1490 - .lat = vehicle_gps_position.lat,
1491 - .lon = vehicle_gps_position.lon,
1492 - .alt = vehicle_gps_position.alt,
1493 - .yaw = vehicle_gps_position.heading,
1494 - .yaw_offset = vehicle_gps_position.heading_offset,
1495 - .fix_type = vehicle_gps_position.fix_type,
1496 - .eph = vehicle_gps_position.eph,
1497 - .epv = vehicle_gps_position.epv,
1498 - .sacc = vehicle_gps_position.s_variance_m_s,
1499 - .vel_m_s = vehicle_gps_position.vel_m_s,
1500 - .vel_ned = Vector3f{
1501 - vehicle_gps_position.vel_n_m_s,
1502 - vehicle_gps_position.vel_e_m_s,
1503 - vehicle_gps_position.vel_d_m_s
1504 - },
1505 - .vel_ned_valid = vehicle_gps_position.vel_ned_valid,
1506 - .nsats = vehicle_gps_position.satellites_used,
1507 - .pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
1508 - + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
1509 - };
1510 - _ekf.setGpsData(gps_msg);
1511 - if(hold != int(gps_msg.time_usec/1000000))
1512 - {
1513 - hold=int(gps_msg.time_usec/1000000);
1514 - if(hold%2==0)
1515 - {
1516 - std::cout <<"----------------------current--------------------------"<<std::endl;
1517 - std :: cout << "time : "<<gps_msg.time_usec/1000000<<std::endl;
1518 - std :: cout << "lat : " <<gps_msg.lat <<std::endl;
1519 - std :: cout << "lon : " <<gps_msg.lon <<std::endl;
1520 - std :: cout << "alt : " <<gps_msg.alt <<std::endl;
1521 - }
1522 - }
1523 - _gps_time_usec = gps_msg.time_usec;
1524 - _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
1525 - }
1526 - }
1527 -}
1528 -
1529 -void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
1530 -{
1531 - const unsigned last_generation = _magnetometer_sub.get_last_generation();
1532 - vehicle_magnetometer_s magnetometer;
1533 -
1534 - if (_magnetometer_sub.update(&magnetometer)) {
1535 -
1536 - if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
1537 - perf_count(_mag_missed_perf);
1538 - PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
1539 - _magnetometer_sub.get_last_generation());
1540 - }
1541 -
1542 - bool reset = false;
1543 -
1544 - // check if magnetometer has changed
1545 - if (magnetometer.device_id != _device_id_mag) {
1546 - if (_device_id_mag != 0) {
1547 - PX4_WARN("%d - mag sensor ID changed %d -> %d", _instance, _device_id_mag, magnetometer.device_id);
1548 - }
1549 -
1550 - reset = true;
1551 -
1552 - } else if (magnetometer.calibration_count > _mag_calibration_count) {
1553 - // existing calibration has changed, reset saved mag bias
1554 - PX4_DEBUG("%d - mag %d calibration updated, resetting bias", _instance, _device_id_mag);
1555 - reset = true;
1556 - }
1557 -
1558 - if (reset) {
1559 - _ekf.resetMagBias();
1560 - _device_id_mag = magnetometer.device_id;
1561 - _mag_calibration_count = magnetometer.calibration_count;
1562 -
1563 - // reset magnetometer bias learning
1564 - _mag_cal_total_time_us = 0;
1565 - _mag_cal_last_us = 0;
1566 - _mag_cal_available = false;
1567 - }
1568 -
1569 - _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
1570 -
1571 - ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
1572 - (int64_t)ekf2_timestamps.timestamp / 100);
1573 - }
1574 -}
1575 -
1576 -void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
1577 -{
1578 - if (!_distance_sensor_selected) {
1579 - // get subscription index of first downward-facing range sensor
1580 - uORB::SubscriptionMultiArray<distance_sensor_s> distance_sensor_subs{ORB_ID::distance_sensor};
1581 -
1582 - for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
1583 - distance_sensor_s distance_sensor;
1584 -
1585 - if (distance_sensor_subs[i].copy(&distance_sensor)) {
1586 - // only use the first instace which has the correct orientation
1587 - if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
1588 - && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
1589 -
1590 - if (_distance_sensor_sub.ChangeInstance(i)) {
1591 - PX4_INFO("%d - selected distance_sensor:%d", _instance, i);
1592 - _distance_sensor_selected = true;
1593 - }
1594 - }
1595 - }
1596 - }
1597 - }
1598 -
1599 - // EKF range sample
1600 - const unsigned last_generation = _distance_sensor_sub.get_last_generation();
1601 - distance_sensor_s distance_sensor;
1602 -
1603 - if (_distance_sensor_sub.update(&distance_sensor)) {
1604 -
1605 - if (_distance_sensor_sub.get_last_generation() != last_generation + 1) {
1606 - PX4_ERR("%d - distance_sensor lost, generation %d -> %d", _instance, last_generation,
1607 - _distance_sensor_sub.get_last_generation());
1608 - }
1609 -
1610 - ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
1611 - (int64_t)ekf2_timestamps.timestamp / 100);
1612 -
1613 - if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
1614 - rangeSample range_sample {
1615 - .time_us = distance_sensor.timestamp,
1616 - .rng = distance_sensor.current_distance,
1617 - .quality = distance_sensor.signal_quality,
1618 - };
1619 - _ekf.setRangeData(range_sample);
1620 -
1621 - // Save sensor limits reported by the rangefinder
1622 - _ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
1623 -
1624 - _last_range_sensor_update = distance_sensor.timestamp;
1625 - return;
1626 - }
1627 - }
1628 -
1629 - if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
1630 - _distance_sensor_selected = false;
1631 - }
1632 -}
1633 -
1634 -void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
1635 -{
1636 - // Check if conditions are OK for learning of magnetometer bias values
1637 - // the EKF is operating in the correct mode and there are no filter faults
1638 - if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) {
1639 -
1640 - if (_mag_cal_last_us != 0) {
1641 - _mag_cal_total_time_us += timestamp - _mag_cal_last_us;
1642 -
1643 - // Start checking mag bias estimates when we have accumulated sufficient calibration time
1644 - if (_mag_cal_total_time_us > 30_s) {
1645 - _mag_cal_last_bias = _ekf.getMagBias();
1646 - _mag_cal_last_bias_variance = _ekf.getMagBiasVariance();
1647 - _mag_cal_available = true;
1648 - }
1649 - }
1650 -
1651 - _mag_cal_last_us = timestamp;
1652 -
1653 - } else {
1654 - // conditions are NOT OK for learning magnetometer bias, reset timestamp
1655 - // but keep the accumulated calibration time
1656 - _mag_cal_last_us = 0;
1657 -
1658 - if (_ekf.fault_status().value != 0) {
1659 - // if a filter fault has occurred, assume previous learning was invalid and do not
1660 - // count it towards total learning time.
1661 - _mag_cal_total_time_us = 0;
1662 - }
1663 - }
1664 -
1665 - if (!_armed) {
1666 - // update stored declination value
1667 - if (!_mag_decl_saved) {
1668 - float declination_deg;
1669 -
1670 - if (_ekf.get_mag_decl_deg(&declination_deg)) {
1671 - _param_ekf2_mag_decl.set(declination_deg);
1672 - _mag_decl_saved = true;
1673 -
1674 - if (!_multi_mode) {
1675 - _param_ekf2_mag_decl.commit_no_notification();
1676 - }
1677 - }
1678 - }
1679 - }
1680 -}
1681 -
1682 -int EKF2::custom_command(int argc, char *argv[])
1683 -{
1684 - return print_usage("unknown command");
1685 -}
1686 -
1687 -int EKF2::task_spawn(int argc, char *argv[])
1688 -{
1689 - bool success = false;
1690 - bool replay_mode = false;
1691 -
1692 - if (argc > 1 && !strcmp(argv[1], "-r")) {
1693 - PX4_INFO("replay mode enabled");
1694 - replay_mode = true;
1695 - }
1696 -
1697 -#if !defined(CONSTRAINED_FLASH)
1698 - bool multi_mode = false;
1699 - int32_t imu_instances = 0;
1700 - int32_t mag_instances = 0;
1701 -
1702 - int32_t sens_imu_mode = 1;
1703 - param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
1704 -
1705 - if (sens_imu_mode == 0) {
1706 - // ekf selector requires SENS_IMU_MODE = 0
1707 - multi_mode = true;
1708 -
1709 - // IMUs (1 - 4 supported)
1710 - param_get(param_find("EKF2_MULTI_IMU"), &imu_instances);
1711 -
1712 - if (imu_instances < 1 || imu_instances > 4) {
1713 - const int32_t imu_instances_limited = math::constrain(imu_instances, 1, 4);
1714 - PX4_WARN("EKF2_MULTI_IMU limited %d -> %d", imu_instances, imu_instances_limited);
1715 - param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
1716 - imu_instances = imu_instances_limited;
1717 - }
1718 -
1719 - int32_t sens_mag_mode = 1;
1720 - param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode);
1721 -
1722 - if (sens_mag_mode == 0) {
1723 - param_get(param_find("EKF2_MULTI_MAG"), &mag_instances);
1724 -
1725 - // Mags (1 - 4 supported)
1726 - if (mag_instances < 1 || mag_instances > 4) {
1727 - const int32_t mag_instances_limited = math::constrain(mag_instances, 1, 4);
1728 - PX4_WARN("EKF2_MULTI_MAG limited %d -> %d", mag_instances, mag_instances_limited);
1729 - param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited);
1730 - mag_instances = mag_instances_limited;
1731 - }
1732 -
1733 - } else {
1734 - mag_instances = 1;
1735 - }
1736 - }
1737 -
1738 - if (multi_mode) {
1739 - // Start EKF2Selector if it's not already running
1740 - if (_ekf2_selector.load() == nullptr) {
1741 - EKF2Selector *inst = new EKF2Selector();
1742 -
1743 - if (inst) {
1744 - _ekf2_selector.store(inst);
1745 -
1746 - } else {
1747 - PX4_ERR("Failed to create EKF2 selector");
1748 - return PX4_ERROR;
1749 - }
1750 - }
1751 -
1752 - const hrt_abstime time_started = hrt_absolute_time();
1753 - const int multi_instances = math::min(imu_instances * mag_instances, (int)EKF2_MAX_INSTANCES);
1754 - int multi_instances_allocated = 0;
1755 -
1756 - // allocate EKF2 instances until all found or arming
1757 - uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
1758 -
1759 - bool ekf2_instance_created[4][4] {}; // IMUs * mags
1760 -
1761 - while ((multi_instances_allocated < multi_instances)
1762 - && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
1763 - && ((hrt_elapsed_time(&time_started) < 30_s)
1764 - || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
1765 -
1766 - vehicle_status_sub.update();
1767 -
1768 - for (uint8_t mag = 0; mag < mag_instances; mag++) {
1769 - uORB::SubscriptionData<vehicle_magnetometer_s> vehicle_mag_sub{ORB_ID(vehicle_magnetometer), mag};
1770 -
1771 - for (uint8_t imu = 0; imu < imu_instances; imu++) {
1772 -
1773 - uORB::SubscriptionData<vehicle_imu_s> vehicle_imu_sub{ORB_ID(vehicle_imu), imu};
1774 - vehicle_mag_sub.update();
1775 -
1776 - // Mag & IMU data must be valid, first mag can be ignored initially
1777 - if ((vehicle_mag_sub.get().device_id != 0 || mag == 0)
1778 - && (vehicle_imu_sub.get().accel_device_id != 0)
1779 - && (vehicle_imu_sub.get().gyro_device_id != 0)) {
1780 -
1781 - if (!ekf2_instance_created[imu][mag]) {
1782 - EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
1783 -
1784 - if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
1785 - int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
1786 -
1787 - if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
1788 - _objects[actual_instance].store(ekf2_inst);
1789 - success = true;
1790 - multi_instances_allocated++;
1791 - ekf2_instance_created[imu][mag] = true;
1792 -
1793 - if (actual_instance == 0) {
1794 - // force selector to run immediately if first instance started
1795 - _ekf2_selector.load()->ScheduleNow();
1796 - }
1797 -
1798 - PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance,
1799 - imu, vehicle_imu_sub.get().accel_device_id,
1800 - mag, vehicle_mag_sub.get().device_id);
1801 -
1802 - // sleep briefly before starting more instances
1803 - px4_usleep(10000);
1804 -
1805 - } else {
1806 - PX4_ERR("instance numbering problem instance: %d", actual_instance);
1807 - delete ekf2_inst;
1808 - break;
1809 - }
1810 -
1811 - } else {
1812 - PX4_ERR("alloc and init failed imu: %d mag:%d", imu, mag);
1813 - px4_usleep(1000000);
1814 - break;
1815 - }
1816 - }
1817 -
1818 - } else {
1819 - px4_usleep(50000); // give the sensors extra time to start
1820 - continue;
1821 - }
1822 - }
1823 - }
1824 -
1825 - if (multi_instances_allocated < multi_instances) {
1826 - px4_usleep(100000);
1827 - }
1828 - }
1829 -
1830 - }
1831 -
1832 -#endif // !CONSTRAINED_FLASH
1833 -
1834 - else {
1835 - // otherwise launch regular
1836 - EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
1837 -
1838 - if (ekf2_inst) {
1839 - _objects[0].store(ekf2_inst);
1840 - ekf2_inst->ScheduleNow();
1841 - success = true;
1842 - }
1843 - }
1844 -
1845 - return success ? PX4_OK : PX4_ERROR;
1846 -}
1847 -
1848 -int EKF2::print_usage(const char *reason)
1849 -{
1850 - if (reason) {
1851 - PX4_WARN("%s\n", reason);
1852 - }
1853 -
1854 - PRINT_MODULE_DESCRIPTION(
1855 - R"DESCR_STR(
1856 -### Description
1857 -Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing.
1858 -
1859 -The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html) page.
1860 -
1861 -ekf2 can be started in replay mode (`-r`): in this mode it does not access the system time, but only uses the
1862 -timestamps from the sensor topics.
1863 -
1864 -)DESCR_STR");
1865 -
1866 - PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
1867 - PRINT_MODULE_USAGE_COMMAND("start");
1868 - PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
1869 - PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
1870 -
1871 - return 0;
1872 -}
1873 -
1874 -extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
1875 -{
1876 - if (argc <= 1 || strcmp(argv[1], "-h") == 0) {
1877 - return EKF2::print_usage();
1878 - }
1879 -
1880 - if (strcmp(argv[1], "start") == 0) {
1881 - int ret = 0;
1882 - EKF2::lock_module();
1883 -
1884 - ret = EKF2::task_spawn(argc - 1, argv + 1);
1885 -
1886 - if (ret < 0) {
1887 - PX4_ERR("start failed (%i)", ret);
1888 - }
1889 -
1890 - EKF2::unlock_module();
1891 - return ret;
1892 -
1893 - } else if (strcmp(argv[1], "status") == 0) {
1894 - if (EKF2::trylock_module()) {
1895 -#if !defined(CONSTRAINED_FLASH)
1896 - if (_ekf2_selector.load()) {
1897 - _ekf2_selector.load()->PrintStatus();
1898 - }
1899 -#endif // !CONSTRAINED_FLASH
1900 -
1901 - for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
1902 - if (_objects[i].load()) {
1903 - PX4_INFO_RAW("\n");
1904 - _objects[i].load()->print_status();
1905 - }
1906 - }
1907 -
1908 - EKF2::unlock_module();
1909 -
1910 - } else {
1911 - PX4_WARN("module locked, try again later");
1912 - }
1913 -
1914 - return 0;
1915 -
1916 - } else if (strcmp(argv[1], "stop") == 0) {
1917 - EKF2::lock_module();
1918 -
1919 - if (argc > 2) {
1920 - int instance = atoi(argv[2]);
1921 -
1922 - if (instance >= 0 && instance < EKF2_MAX_INSTANCES) {
1923 - PX4_INFO("stopping instance %d", instance);
1924 - EKF2 *inst = _objects[instance].load();
1925 -
1926 - if (inst) {
1927 - inst->request_stop();
1928 - px4_usleep(20000); // 20 ms
1929 - delete inst;
1930 - _objects[instance].store(nullptr);
1931 - }
1932 - } else {
1933 - PX4_ERR("invalid instance %d", instance);
1934 - }
1935 -
1936 - } else {
1937 - // otherwise stop everything
1938 - bool was_running = false;
1939 -
1940 -#if !defined(CONSTRAINED_FLASH)
1941 - if (_ekf2_selector.load()) {
1942 - PX4_INFO("stopping ekf2 selector");
1943 - _ekf2_selector.load()->Stop();
1944 - delete _ekf2_selector.load();
1945 - _ekf2_selector.store(nullptr);
1946 - was_running = true;
1947 - }
1948 -#endif // !CONSTRAINED_FLASH
1949 -
1950 - for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
1951 - EKF2 *inst = _objects[i].load();
1952 -
1953 - if (inst) {
1954 - PX4_INFO("stopping ekf2 instance %d", i);
1955 - was_running = true;
1956 - inst->request_stop();
1957 - px4_usleep(20000); // 20 ms
1958 - delete inst;
1959 - _objects[i].store(nullptr);
1960 - }
1961 - }
1962 -
1963 - if (!was_running) {
1964 - PX4_WARN("not running");
1965 - }
1966 - }
1967 -
1968 - EKF2::unlock_module();
1969 - return PX4_OK;
1970 - }
1971 -
1972 - EKF2::lock_module(); // Lock here, as the method could access _object.
1973 - int ret = EKF2::custom_command(argc - 1, argv + 1);
1974 - EKF2::unlock_module();
1975 -
1976 - return ret;
1977 -}
...@@ -47,7 +47,7 @@ ...@@ -47,7 +47,7 @@
47 47
48 #include "mission.h" 48 #include "mission.h"
49 #include "navigator.h" 49 #include "navigator.h"
50 -#include "iostream" 50 +
51 #include <string.h> 51 #include <string.h>
52 #include <drivers/drv_hrt.h> 52 #include <drivers/drv_hrt.h>
53 #include <dataman/dataman.h> 53 #include <dataman/dataman.h>
...@@ -1031,30 +1031,6 @@ Mission::set_mission_items() ...@@ -1031,30 +1031,6 @@ Mission::set_mission_items()
1031 generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw); 1031 generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw);
1032 } 1032 }
1033 1033
1034 - /* don't advance mission after FW to MC command */
1035 - if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
1036 - && _work_item_type == WORK_ITEM_TYPE_DEFAULT
1037 - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
1038 - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
1039 - && !_navigator->get_land_detected()->landed
1040 - && pos_sp_triplet->current.valid) {
1041 -
1042 - new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
1043 - }
1044 -
1045 - /* after FW to MC transition finish moving to the waypoint */
1046 - if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE &&
1047 - new_work_item_type == WORK_ITEM_TYPE_DEFAULT
1048 - && pos_sp_triplet->current.valid) {
1049 -
1050 - new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
1051 -
1052 - _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
1053 - copy_position_if_valid(&_mission_item, &pos_sp_triplet->current);
1054 - _mission_item.autocontinue = true;
1055 - _mission_item.time_inside = 0.0f;
1056 - }
1057 -
1058 // ignore certain commands in mission fast forward 1034 // ignore certain commands in mission fast forward
1059 if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) && 1035 if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) &&
1060 (_mission_item.nav_cmd == NAV_CMD_DELAY)) { 1036 (_mission_item.nav_cmd == NAV_CMD_DELAY)) {
...@@ -1862,9 +1838,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit ...@@ -1862,9 +1838,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
1862 1838
1863 void Mission::publish_navigator_mission_item() 1839 void Mission::publish_navigator_mission_item()
1864 { 1840 {
1865 -
1866 - for(int i=0;i<8;i++)
1867 -{
1868 navigator_mission_item_s navigator_mission_item{}; 1841 navigator_mission_item_s navigator_mission_item{};
1869 1842
1870 navigator_mission_item.instance_count = _navigator->mission_instance_count(); 1843 navigator_mission_item.instance_count = _navigator->mission_instance_count();
...@@ -1891,11 +1864,4 @@ void Mission::publish_navigator_mission_item() ...@@ -1891,11 +1864,4 @@ void Mission::publish_navigator_mission_item()
1891 navigator_mission_item.timestamp = hrt_absolute_time(); 1864 navigator_mission_item.timestamp = hrt_absolute_time();
1892 1865
1893 _navigator_mission_item_pub.publish(navigator_mission_item); 1866 _navigator_mission_item_pub.publish(navigator_mission_item);
1894 -
1895 -
1896 - std::cout<<navigator_mission_item.latitude<<std::endl;
1897 - navigator_mission_item.sequence_current++;
1898 - std::cout<<"************************"<<std::endl;
1899 -}
1900 -
1901 } 1867 }
......
...@@ -281,7 +281,6 @@ private: ...@@ -281,7 +281,6 @@ private:
281 WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ 281 WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */
282 WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ 282 WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
283 WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ 283 WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */
284 - WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
285 WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, 284 WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
286 WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, 285 WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
287 WORK_ITEM_TYPE_PRECISION_LAND 286 WORK_ITEM_TYPE_PRECISION_LAND
......