mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 18:08:53 +08:00
ekf2: move to WQ with uORB callback scheduling
This commit is contained in:
Daniel Agar
@@ -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};
|
||||
|
||||
+108
-115
@@ -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 <px4_module.h>
|
||||
#include <px4_module_params.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_tasks.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <px4_time.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
@@ -91,30 +92,24 @@ using namespace time_literals;
|
||||
|
||||
extern "C" __EXPORT int ekf2_main(int argc, char *argv[]);
|
||||
|
||||
class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
|
||||
class Ekf2 final : public ModuleBase<Ekf2>, 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 Run() override;
|
||||
|
||||
void set_replay_mode(bool replay) { _replay_mode = replay; }
|
||||
|
||||
static void task_main_trampoline(int argc, char *argv[]);
|
||||
bool init();
|
||||
|
||||
int print_status() override;
|
||||
|
||||
@@ -123,8 +118,10 @@ private:
|
||||
|
||||
template<typename Param>
|
||||
void update_mag_bias(Param &mag_bias_param, int axis_index);
|
||||
|
||||
template<typename Param>
|
||||
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<ekf2_innovations_s> _estimator_innovations_pub{ORB_ID(ekf2_innovations)};
|
||||
uORB::Publication<ekf2_timestamps_s> _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);
|
||||
}
|
||||
|
||||
orb_unsubscribe(_sensors_sub);
|
||||
bool
|
||||
Ekf2::init()
|
||||
{
|
||||
if (!_sensors_sub.registerCallback()) {
|
||||
PX4_ERR("sensor combined callback registration failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
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;
|
||||
if (should_exit()) {
|
||||
_sensors_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
px4_pollfd_struct_t fds[1] = {};
|
||||
fds[0].fd = _sensors_sub;
|
||||
fds[0].events = POLLIN;
|
||||
perf_begin(_cycle_perf);
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// 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 = {};
|
||||
sensor_combined_s sensors;
|
||||
|
||||
while (!should_exit()) {
|
||||
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
|
||||
|
||||
if (!(fds[0].revents & POLLIN)) {
|
||||
// no new data
|
||||
continue;
|
||||
}
|
||||
|
||||
if (ret < 0) {
|
||||
// Poll error, sleep and try again
|
||||
px4_usleep(10000);
|
||||
continue;
|
||||
|
||||
} else if (ret == 0) {
|
||||
// Poll timeout or no new data, do nothing
|
||||
continue;
|
||||
}
|
||||
|
||||
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[])
|
||||
|
||||
Reference in New Issue
Block a user