From 3c623af903063e2e98f7aebb92815f9e19716169 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Aug 2019 19:15:22 -0400 Subject: [PATCH] ekf2: move to WQ with uORB callback scheduling --- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/modules/ekf2/ekf2_main.cpp | 223 +++++++++--------- 2 files changed, 109 insertions(+), 116 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index f2de048913..a35705f726 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -62,7 +62,7 @@ static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8}; static constexpr wq_config_t I2C3{"wq:I2C3", 1250, -9}; static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10}; -static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 2000, -11}; // PX4 att/pos controllers, highest priority after sensors +static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12}; static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index f36f14ce2c..9f8282a899 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,11 +48,12 @@ #include #include #include -#include +#include #include #include #include #include +#include #include #include #include @@ -91,30 +92,24 @@ using namespace time_literals; extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); -class Ekf2 final : public ModuleBase, public ModuleParams +class Ekf2 final : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - Ekf2(); + Ekf2(bool replay_mode = false); ~Ekf2() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static Ekf2 *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; - - void set_replay_mode(bool replay) { _replay_mode = replay; } + void Run() override; - static void task_main_trampoline(int argc, char *argv[]); + bool init(); int print_status() override; @@ -123,8 +118,10 @@ private: template void update_mag_bias(Param &mag_bias_param, int axis_index); + template bool update_mag_decl(Param &mag_decl_param); + bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now); bool publish_wind_estimate(const hrt_abstime ×tamp); @@ -167,15 +164,16 @@ private: */ float filter_altitude_ellipsoid(float amsl_hgt); - bool _replay_mode = false; ///< true when we use replay data from a log + const bool _replay_mode; ///< true when we use replay data from a log // time slip monitoring uint64_t _integrated_time_us = 0; ///< integral of gyro delta time from start (uSec) uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) - perf_counter_t _perf_update_data; - perf_counter_t _perf_ekf_update; + perf_counter_t _cycle_perf; + perf_counter_t _interval_perf; + perf_counter_t _ekf_update_perf; // Initialise time stamps used to send sensor data to the EKF and for logging uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected @@ -255,6 +253,8 @@ private: uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 + bool _imu_bias_reset_request{false}; + // republished aligned external visual odometry bool new_ev_data_received = false; vehicle_odometry_s _ev_odom{}; @@ -270,7 +270,7 @@ private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; - int _sensors_sub{ -1}; + uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; // because we can have several distance sensor instances with different orientations uORB::Subscription _range_finder_subs[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; @@ -278,7 +278,10 @@ private: // because we can have multiple GPS instances uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}; - int _gps_orb_instance{ -1}; + + sensor_selection_s _sensor_selection{}; + vehicle_land_detected_s _vehicle_land_detected{}; + vehicle_status_s _vehicle_status{}; uORB::Publication _estimator_innovations_pub{ORB_ID(ekf2_innovations)}; uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; @@ -540,10 +543,13 @@ private: }; -Ekf2::Ekf2(): +Ekf2::Ekf2(bool replay_mode): ModuleParams(nullptr), - _perf_update_data(perf_alloc_once(PC_ELAPSED, "EKF2 data acquisition")), - _perf_ekf_update(perf_alloc_once(PC_ELAPSED, "EKF2 update")), + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _replay_mode(replay_mode), + _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")), + _interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval")), + _ekf_update_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": update")), _params(_ekf.getParamHandle()), _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), _param_ekf2_mag_delay(_params->mag_delay_ms), @@ -640,8 +646,6 @@ Ekf2::Ekf2(): _param_ekf2_bcoef_y(_params->bcoef_y), _param_ekf2_move_test(_params->is_moving_scaler) { - _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); - // initialise parameter cache updateParams(); @@ -650,10 +654,20 @@ Ekf2::Ekf2(): Ekf2::~Ekf2() { - perf_free(_perf_update_data); - perf_free(_perf_ekf_update); + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_ekf_update_perf); +} + +bool +Ekf2::init() +{ + if (!_sensors_sub.registerCallback()) { + PX4_ERR("sensor combined callback registration failed!"); + return false; + } - orb_unsubscribe(_sensors_sub); + return true; } int Ekf2::print_status() @@ -663,8 +677,9 @@ int Ekf2::print_status() PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us); - perf_print_counter(_perf_update_data); - perf_print_counter(_perf_ekf_update); + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_ekf_update_perf); return 0; } @@ -703,41 +718,20 @@ bool Ekf2::update_mag_decl(Param &mag_decl_param) return false; } -void Ekf2::run() +void Ekf2::Run() { - bool imu_bias_reset_request = false; - - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _sensors_sub; - fds[0].events = POLLIN; - - // initialize data structures outside of loop - // because they will else not always be - // properly populated - sensor_combined_s sensors = {}; - vehicle_land_detected_s vehicle_land_detected = {}; - vehicle_status_s vehicle_status = {}; - sensor_selection_s sensor_selection = {}; - - while (!should_exit()) { - int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); - - if (!(fds[0].revents & POLLIN)) { - // no new data - continue; - } + if (should_exit()) { + _sensors_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } - if (ret < 0) { - // Poll error, sleep and try again - px4_usleep(10000); - continue; + perf_begin(_cycle_perf); + perf_count(_interval_perf); - } else if (ret == 0) { - // Poll timeout or no new data, do nothing - continue; - } + sensor_combined_s sensors; - perf_begin(_perf_update_data); + if (_sensors_sub.update(&sensors)) { // check for parameter updates if (_parameter_update_sub.updated()) { @@ -749,10 +743,8 @@ void Ekf2::run() updateParams(); } - orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); - // ekf2_timestamps (using 0.1 ms relative timestamps) - ekf2_timestamps_s ekf2_timestamps = {}; + ekf2_timestamps_s ekf2_timestamps{}; ekf2_timestamps.timestamp = sensors.timestamp; ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; @@ -764,9 +756,9 @@ void Ekf2::run() ekf2_timestamps.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; // update all other topics if they have new data - if (_status_sub.update(&vehicle_status)) { + if (_status_sub.update(&_vehicle_status)) { - bool is_fixed_wing = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + const bool is_fixed_wing = (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); // only fuse synthetic sideslip measurements if conditions are met _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1)); @@ -776,27 +768,27 @@ void Ekf2::run() } // Always update sensor selction first time through if time stamp is non zero - if (_sensor_selection_sub.updated() || (sensor_selection.timestamp == 0)) { - const sensor_selection_s sensor_selection_prev = sensor_selection; + if (_sensor_selection_sub.updated() || (_sensor_selection.timestamp == 0)) { + const sensor_selection_s sensor_selection_prev = _sensor_selection; - if (_sensor_selection_sub.copy(&sensor_selection)) { - if ((sensor_selection_prev.timestamp > 0) && (sensor_selection.timestamp > sensor_selection_prev.timestamp)) { - if (sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { + if (_sensor_selection_sub.copy(&_sensor_selection)) { + if ((sensor_selection_prev.timestamp > 0) && (_sensor_selection.timestamp > sensor_selection_prev.timestamp)) { + if (_sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) { PX4_WARN("accel id changed, resetting IMU bias"); - imu_bias_reset_request = true; + _imu_bias_reset_request = true; } - if (sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { + if (_sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) { PX4_WARN("gyro id changed, resetting IMU bias"); - imu_bias_reset_request = true; + _imu_bias_reset_request = true; } } } } // attempt reset until successful - if (imu_bias_reset_request) { - imu_bias_reset_request = !_ekf.reset_imu_bias(); + if (_imu_bias_reset_request) { + _imu_bias_reset_request = !_ekf.reset_imu_bias(); } const hrt_abstime now = sensors.timestamp; @@ -823,7 +815,9 @@ void Ekf2::run() // Do not reset parmameters when armed to prevent potential time slips casued by parameter set // and notification events // Check if there has been a persistant change in magnetometer ID - if (sensor_selection.mag_device_id != 0 && sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get()) { + if (_sensor_selection.mag_device_id != 0 + && (_sensor_selection.mag_device_id != (uint32_t)_param_ekf2_magbias_id.get())) { + if (_invalid_mag_id_count < 200) { _invalid_mag_id_count++; } @@ -834,7 +828,7 @@ void Ekf2::run() } } - if ((vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) { + if ((_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) { // the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID // this means we need to reset the learned bias values to zero _param_ekf2_magbias_x.set(0.f); @@ -843,7 +837,7 @@ void Ekf2::run() _param_ekf2_magbias_y.commit_no_notification(); _param_ekf2_magbias_z.set(0.f); _param_ekf2_magbias_z.commit_no_notification(); - _param_ekf2_magbias_id.set(sensor_selection.mag_device_id); + _param_ekf2_magbias_id.set(_sensor_selection.mag_device_id); _param_ekf2_magbias_id.commit(); _invalid_mag_id_count = 0; @@ -1227,8 +1221,8 @@ void Ekf2::run() bool vehicle_land_detected_updated = _vehicle_land_detected_sub.updated(); if (vehicle_land_detected_updated) { - if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { - _ekf.set_in_air_status(!vehicle_land_detected.landed); + if (_vehicle_land_detected_sub.copy(&_vehicle_land_detected)) { + _ekf.set_in_air_status(!_vehicle_land_detected.landed); } } @@ -1247,12 +1241,10 @@ void Ekf2::run() } } - perf_end(_perf_update_data); - // run the EKF update and output - perf_begin(_perf_ekf_update); + perf_begin(_ekf_update_perf); const bool updated = _ekf.update(); - perf_end(_perf_ekf_update); + perf_end(_ekf_update_perf); // integrate time to monitor time slippage if (_start_time_us == 0) { @@ -1370,7 +1362,7 @@ void Ekf2::run() } // only consider ground effect if compensation is configured and the vehicle is armed (props spinning) - if (_param_ekf2_gnd_eff_dz.get() > 0.0f && vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (_param_ekf2_gnd_eff_dz.get() > 0.0f && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // set ground effect flag if vehicle is closer than a specified distance to the ground if (lpos.dist_bottom_valid) { _ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get()); @@ -1381,7 +1373,7 @@ void Ekf2::run() } else if (vehicle_land_detected_updated && !_had_valid_terrain) { // update ground effect flag based on land detector state - _ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect); + _ekf.set_gnd_effect_flag(_vehicle_land_detected.in_ground_effect); } } else { @@ -1591,8 +1583,8 @@ void Ekf2::run() /* Check and save learned magnetometer bias estimates */ // Check if conditions are OK for learning of magnetometer bias values - if (!vehicle_land_detected.landed && // not on ground - (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && // vehicle is armed + if (!_vehicle_land_detected.landed && // not on ground + (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) && // vehicle is armed !status.filter_fault_flags && // there are no filter faults control_status.flags.mag_3D) { // the EKF is operating in the correct mode @@ -1642,9 +1634,9 @@ void Ekf2::run() } // Check and save the last valid calibration when we are disarmed - if ((vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + if ((_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status.filter_fault_flags == 0) - && (sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) { + && (_sensor_selection.mag_device_id == (uint32_t)_param_ekf2_magbias_id.get())) { update_mag_bias(_param_ekf2_magbias_x, 0); update_mag_bias(_param_ekf2_magbias_y, 1); @@ -1658,7 +1650,7 @@ void Ekf2::run() publish_wind_estimate(now); - if (!_mag_decl_saved && (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { + if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { _mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl); } @@ -1688,7 +1680,7 @@ void Ekf2::run() _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]); // calculate noise filtered velocity innovations which are used for pre-flight checking - if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { // calculate coefficients for LPF applied to innovation sequences float alpha = constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f); float beta = 1.0f - alpha; @@ -1707,7 +1699,7 @@ void Ekf2::run() float yaw_test_limit; - if (doing_ne_aiding && vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (doing_ne_aiding && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { // use a smaller tolerance when doing NE inertial frame aiding as a rotary wing // vehicle which cannot use GPS course to realign heading in flight yaw_test_limit = _nav_yaw_innov_test_lim; @@ -1756,6 +1748,8 @@ void Ekf2::run() // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); } + + perf_end(_cycle_perf); } int Ekf2::getRangeSubIndex() @@ -2422,19 +2416,6 @@ float Ekf2::filter_altitude_ellipsoid(float amsl_hgt) return amsl_hgt + _wgs84_hgt_offset; } -Ekf2 *Ekf2::instantiate(int argc, char *argv[]) -{ - Ekf2 *instance = new Ekf2(); - - if (instance) { - if (argc >= 2 && !strcmp(argv[1], "-r")) { - instance->set_replay_mode(true); - } - } - - return instance; -} - int Ekf2::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); @@ -2468,19 +2449,31 @@ timestamps from the sensor topics. int Ekf2::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("ekf2", - SCHED_DEFAULT, - SCHED_PRIORITY_ESTIMATOR, - 6600, - (px4_main_t)&run_trampoline, - (char *const *)argv); - - if (_task_id < 0) { - _task_id = -1; - return -errno; + bool replay_mode = false; + if (argc > 1 && !strcmp(argv[1], "-r")) { + PX4_INFO("replay mode enabled"); + replay_mode = true; } - return 0; + Ekf2 *instance = new Ekf2(replay_mode); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } int ekf2_main(int argc, char *argv[])