mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 02:43:35 +08:00
ekf2: implement multi-instance AGP fusion
- Support up to 4 independent AGP sources simultaneously - Dynamic parameter lookup without boilerplate switch statements - Per-source state tracking and fusion logic - Update simulator to use new AuxGlobalPosition message
This commit is contained in:
Marco Hauswirth
@@ -42,147 +42,159 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
|
||||
#if defined(MODULE_NAME)
|
||||
|
||||
if (_aux_global_position_sub.updated()) {
|
||||
for (int instance = 0; instance < MAX_AGP_IDS; instance++) {
|
||||
if (_aux_global_position_subs[instance].updated()) {
|
||||
|
||||
vehicle_global_position_s aux_global_position{};
|
||||
_aux_global_position_sub.copy(&aux_global_position);
|
||||
aux_global_position_s aux_global_position{};
|
||||
_aux_global_position_subs[instance].copy(&aux_global_position);
|
||||
|
||||
const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(_param_ekf2_agp_delay.get() * 1000);
|
||||
const uint8_t sensor_id = aux_global_position.id;
|
||||
const int slot = mapSensorIdToSlot(sensor_id);
|
||||
|
||||
AuxGlobalPositionSample sample{};
|
||||
sample.time_us = time_us;
|
||||
sample.latitude = aux_global_position.lat;
|
||||
sample.longitude = aux_global_position.lon;
|
||||
sample.altitude_amsl = aux_global_position.alt;
|
||||
sample.eph = aux_global_position.eph;
|
||||
sample.epv = aux_global_position.epv;
|
||||
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
|
||||
if (slot >= MAX_AGP_IDS) {
|
||||
// All parameter slots are full, cannot handle this sensor. Set ID of unused slot to 0
|
||||
continue;
|
||||
}
|
||||
|
||||
_aux_global_position_buffer.push(sample);
|
||||
const int64_t time_us = aux_global_position.timestamp_sample - static_cast<int64_t>(getDelayParam(slot) * 1000);
|
||||
|
||||
_time_last_buffer_push = imu_delayed.time_us;
|
||||
AuxGlobalPositionSample sample{};
|
||||
sample.time_us = time_us;
|
||||
sample.id = sensor_id;
|
||||
sample.latitude = aux_global_position.lat;
|
||||
sample.longitude = aux_global_position.lon;
|
||||
sample.altitude_amsl = aux_global_position.alt;
|
||||
sample.eph = aux_global_position.eph;
|
||||
sample.epv = aux_global_position.epv;
|
||||
sample.lat_lon_reset_counter = aux_global_position.lat_lon_reset_counter;
|
||||
|
||||
_sources[slot].buffer.push(sample);
|
||||
_sources[slot].time_last_buffer_push = imu_delayed.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // MODULE_NAME
|
||||
|
||||
AuxGlobalPositionSample sample;
|
||||
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
|
||||
SourceData &source = _sources[slot];
|
||||
AuxGlobalPositionSample sample;
|
||||
|
||||
if (_aux_global_position_buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
|
||||
if (source.buffer.pop_first_older_than(imu_delayed.time_us, &sample)) {
|
||||
|
||||
if (!(_param_ekf2_agp_ctrl.get() & static_cast<int32_t>(Ctrl::kHPos))) {
|
||||
return;
|
||||
}
|
||||
if (!(getCtrlParam(slot) & static_cast<int32_t>(Ctrl::kHPos))) {
|
||||
continue;
|
||||
}
|
||||
|
||||
estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position;
|
||||
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
|
||||
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
|
||||
estimator_aid_source2d_s &aid_src = source.aid_src;
|
||||
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
|
||||
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
|
||||
|
||||
// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
|
||||
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
|
||||
const float pos_var = sq(pos_noise);
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
float pos_noise = math::max(sample.eph, getNoiseParam(slot), 0.01f);
|
||||
const float pos_var = sq(pos_noise);
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
|
||||
ekf.updateAidSourceStatus(aid_src,
|
||||
sample.time_us, // sample timestamp
|
||||
matrix::Vector2d(sample.latitude, sample.longitude), // observation
|
||||
pos_obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
|
||||
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
|
||||
ekf.updateAidSourceStatus(aid_src,
|
||||
sample.time_us, // sample timestamp
|
||||
matrix::Vector2d(sample.latitude, sample.longitude), // observation
|
||||
pos_obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
|
||||
math::max(getGateParam(slot), 1.f)); // innovation gate
|
||||
|
||||
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
|
||||
&& ekf.control_status_flags().yaw_align;
|
||||
const bool continuing_conditions = starting_conditions
|
||||
&& ekf.global_origin_valid();
|
||||
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
|
||||
&& ekf.control_status_flags().yaw_align;
|
||||
const bool continuing_conditions = starting_conditions
|
||||
&& ekf.global_origin_valid();
|
||||
|
||||
switch (_state) {
|
||||
case State::kStopped:
|
||||
switch (source.state) {
|
||||
case State::kStopped:
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case State::kStarting:
|
||||
if (starting_conditions) {
|
||||
_state = State::kStarting;
|
||||
/* FALLTHROUGH */
|
||||
case State::kStarting:
|
||||
if (starting_conditions) {
|
||||
source.state = State::kStarting;
|
||||
|
||||
if (ekf.global_origin_valid()) {
|
||||
const bool fused = ekf.fuseHorizontalPosition(aid_src);
|
||||
bool reset = false;
|
||||
if (ekf.global_origin_valid()) {
|
||||
const bool fused = ekf.fuseHorizontalPosition(aid_src);
|
||||
bool reset = false;
|
||||
|
||||
if (!fused && isResetAllowed(ekf)) {
|
||||
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
reset = true;
|
||||
if (!fused && isResetAllowed(ekf, slot)) {
|
||||
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
reset = true;
|
||||
}
|
||||
|
||||
if (fused || reset) {
|
||||
ekf.enableControlStatusAuxGpos();
|
||||
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
source.state = State::kActive;
|
||||
}
|
||||
|
||||
} else {
|
||||
// Try to initialize using measurement
|
||||
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
|
||||
sq(sample.epv))) {
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
ekf.enableControlStatusAuxGpos();
|
||||
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
source.state = State::kActive;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (fused || reset) {
|
||||
ekf.enableControlStatusAuxGpos();
|
||||
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
_state = State::kActive;
|
||||
break;
|
||||
|
||||
case State::kActive:
|
||||
if (continuing_conditions) {
|
||||
ekf.fuseHorizontalPosition(aid_src);
|
||||
|
||||
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
|
||||
|| (source.reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
|
||||
if (isResetAllowed(ekf, slot)) {
|
||||
|
||||
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
|
||||
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
source.reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
|
||||
} else {
|
||||
ekf.disableControlStatusAuxGpos();
|
||||
source.state = State::kStopped;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// Try to initialize using measurement
|
||||
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
|
||||
sq(sample.epv))) {
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
ekf.enableControlStatusAuxGpos();
|
||||
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
_state = State::kActive;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case State::kActive:
|
||||
if (continuing_conditions) {
|
||||
ekf.fuseHorizontalPosition(aid_src);
|
||||
|
||||
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.reset_timeout_max)
|
||||
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
|
||||
if (isResetAllowed(ekf)) {
|
||||
|
||||
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
|
||||
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
|
||||
} else {
|
||||
ekf.disableControlStatusAuxGpos();
|
||||
_state = State::kStopped;
|
||||
}
|
||||
ekf.disableControlStatusAuxGpos();
|
||||
source.state = State::kStopped;
|
||||
}
|
||||
|
||||
} else {
|
||||
ekf.disableControlStatusAuxGpos();
|
||||
_state = State::kStopped;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
#if defined(MODULE_NAME)
|
||||
aid_src.timestamp = hrt_absolute_time();
|
||||
_estimator_aid_src_aux_global_position_pub.publish(aid_src);
|
||||
aid_src.timestamp = hrt_absolute_time();
|
||||
source.aid_src_pub.publish(aid_src);
|
||||
|
||||
_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
|
||||
source.test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
|
||||
#endif // MODULE_NAME
|
||||
|
||||
} else if ((_state != State::kStopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
|
||||
ekf.disableControlStatusAuxGpos();
|
||||
_state = State::kStopped;
|
||||
ECL_WARN("Aux global position data stopped");
|
||||
} else if ((source.state != State::kStopped) && isTimedOut(source.time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
|
||||
ekf.disableControlStatusAuxGpos();
|
||||
source.state = State::kStopped;
|
||||
ECL_WARN("Aux global position data stopped for slot %d (sensor ID %d)", slot, getIdParam(slot));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf) const
|
||||
bool AuxGlobalPosition::isResetAllowed(const Ekf &ekf, int slot) const
|
||||
{
|
||||
return ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kAuto)
|
||||
return ((static_cast<Mode>(getModeParam(slot)) == Mode::kAuto)
|
||||
&& !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos))
|
||||
|| ((static_cast<Mode>(_param_ekf2_agp_mode.get()) == Mode::kDeadReckoning)
|
||||
|| ((static_cast<Mode>(getModeParam(slot)) == Mode::kDeadReckoning)
|
||||
&& !ekf.isOtherSourceOfHorizontalAidingThan(ekf.control_status_flags().aux_gpos));
|
||||
}
|
||||
|
||||
|
||||
@@ -49,10 +49,12 @@
|
||||
|
||||
#if defined(MODULE_NAME)
|
||||
# include <px4_platform_common/module_params.h>
|
||||
# include <px4_platform_common/log.h>
|
||||
# include <lib/parameters/param.h>
|
||||
# include <uORB/PublicationMulti.hpp>
|
||||
# include <uORB/Subscription.hpp>
|
||||
# include <uORB/topics/estimator_aid_source2d.h>
|
||||
# include <uORB/topics/vehicle_global_position.h>
|
||||
# include <uORB/topics/aux_global_position.h>
|
||||
#endif // MODULE_NAME
|
||||
|
||||
class Ekf;
|
||||
@@ -60,9 +62,28 @@ class Ekf;
|
||||
class AuxGlobalPosition : public ModuleParams
|
||||
{
|
||||
public:
|
||||
static constexpr uint8_t MAX_AGP_IDS = 4;
|
||||
|
||||
AuxGlobalPosition() : ModuleParams(nullptr)
|
||||
{
|
||||
_estimator_aid_src_aux_global_position_pub.advertise();
|
||||
for (int i = 0; i < MAX_AGP_IDS; i++) {
|
||||
_sources[i].aid_src_pub.advertise();
|
||||
|
||||
}
|
||||
|
||||
// check existing ID to initialize parameters
|
||||
for (int i = 0; i < MAX_AGP_IDS; i++) {
|
||||
if (getIdParam(i) != 0) {
|
||||
getCtrlParam(i);
|
||||
getModeParam(i);
|
||||
getDelayParam(i);
|
||||
getNoiseParam(i);
|
||||
getGateParam(i);
|
||||
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
~AuxGlobalPosition() = default;
|
||||
@@ -74,7 +95,14 @@ public:
|
||||
updateParams();
|
||||
}
|
||||
|
||||
float test_ratio_filtered() const { return _test_ratio_filtered; }
|
||||
float test_ratio_filtered(uint8_t id = 0) const
|
||||
{
|
||||
if (id < MAX_AGP_IDS) {
|
||||
return _sources[id].test_ratio_filtered;
|
||||
}
|
||||
|
||||
return INFINITY;
|
||||
}
|
||||
|
||||
private:
|
||||
bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const
|
||||
@@ -82,10 +110,11 @@ private:
|
||||
return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < time_delayed_us);
|
||||
}
|
||||
|
||||
bool isResetAllowed(const Ekf &ekf) const;
|
||||
bool isResetAllowed(const Ekf &ekf, int slot) const;
|
||||
|
||||
struct AuxGlobalPositionSample {
|
||||
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
|
||||
uint8_t id{}; ///< source identifier
|
||||
double latitude{};
|
||||
double longitude{};
|
||||
float altitude_amsl{};
|
||||
@@ -94,10 +123,6 @@ private:
|
||||
uint8_t lat_lon_reset_counter{};
|
||||
};
|
||||
|
||||
estimator_aid_source2d_s _aid_src_aux_global_position{};
|
||||
RingBuffer<AuxGlobalPositionSample> _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate
|
||||
uint64_t _time_last_buffer_push{0};
|
||||
|
||||
enum class Ctrl : uint8_t {
|
||||
kHPos = (1 << 0),
|
||||
kVPos = (1 << 1)
|
||||
@@ -114,26 +139,120 @@ private:
|
||||
kActive,
|
||||
};
|
||||
|
||||
State _state{State::kStopped};
|
||||
|
||||
float _test_ratio_filtered{INFINITY};
|
||||
|
||||
#if defined(MODULE_NAME)
|
||||
struct reset_counters_s {
|
||||
uint8_t lat_lon{};
|
||||
};
|
||||
reset_counters_s _reset_counters{};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_global_position_pub{ORB_ID(estimator_aid_src_aux_global_position)};
|
||||
uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)};
|
||||
struct SourceData {
|
||||
estimator_aid_source2d_s aid_src{};
|
||||
RingBuffer<AuxGlobalPositionSample> buffer{20};
|
||||
uint64_t time_last_buffer_push{0};
|
||||
State state{State::kStopped};
|
||||
float test_ratio_filtered{0.f};
|
||||
reset_counters_s reset_counters{};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> aid_src_pub{ORB_ID(estimator_aid_src_aux_global_position)};
|
||||
};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::EKF2_AGP_CTRL>) _param_ekf2_agp_ctrl,
|
||||
(ParamInt<px4::params::EKF2_AGP_MODE>) _param_ekf2_agp_mode,
|
||||
(ParamFloat<px4::params::EKF2_AGP_DELAY>) _param_ekf2_agp_delay,
|
||||
(ParamFloat<px4::params::EKF2_AGP_NOISE>) _param_ekf2_agp_noise,
|
||||
(ParamFloat<px4::params::EKF2_AGP_GATE>) _param_ekf2_agp_gate
|
||||
)
|
||||
SourceData _sources[MAX_AGP_IDS];
|
||||
|
||||
uORB::Subscription _aux_global_position_subs[MAX_AGP_IDS] {
|
||||
{ORB_ID(aux_global_position), 0},
|
||||
{ORB_ID(aux_global_position), 1},
|
||||
{ORB_ID(aux_global_position), 2},
|
||||
{ORB_ID(aux_global_position), 3}
|
||||
};
|
||||
|
||||
int32_t getAgpParamInt32(const char *param_suffix, int instance) const
|
||||
{
|
||||
char param_name[20] {};
|
||||
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
|
||||
|
||||
int32_t value = 0;
|
||||
|
||||
if (param_get(param_find(param_name), &value) != 0) {
|
||||
PX4_ERR("failed to get %s", param_name);
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
float getAgpParamFloat(const char *param_suffix, int instance) const
|
||||
{
|
||||
char param_name[20] {};
|
||||
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
|
||||
|
||||
float value = NAN;
|
||||
|
||||
if (param_get(param_find(param_name), &value) != 0) {
|
||||
PX4_ERR("failed to get %s", param_name);
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
bool setAgpParamInt32(const char *param_suffix, int instance, int32_t value)
|
||||
{
|
||||
char param_name[20] {};
|
||||
snprintf(param_name, sizeof(param_name), "EKF2_AGP%d_%s", instance, param_suffix);
|
||||
|
||||
return param_set_no_notification(param_find(param_name), &value) == PX4_OK;
|
||||
}
|
||||
|
||||
int32_t getCtrlParam(int instance) const
|
||||
{
|
||||
return getAgpParamInt32("CTRL", instance);
|
||||
}
|
||||
|
||||
int32_t getModeParam(int instance) const
|
||||
{
|
||||
return getAgpParamInt32("MODE", instance);
|
||||
}
|
||||
|
||||
float getDelayParam(int instance) const
|
||||
{
|
||||
return getAgpParamFloat("DELAY", instance);
|
||||
}
|
||||
|
||||
float getNoiseParam(int instance) const
|
||||
{
|
||||
return getAgpParamFloat("NOISE", instance);
|
||||
}
|
||||
|
||||
float getGateParam(int instance) const
|
||||
{
|
||||
return getAgpParamFloat("GATE", instance);
|
||||
}
|
||||
|
||||
int32_t getIdParam(int instance) const
|
||||
{
|
||||
return getAgpParamInt32("ID", instance);
|
||||
}
|
||||
|
||||
void setIdParam(int instance, int32_t sensor_id)
|
||||
{
|
||||
setAgpParamInt32("ID", instance, sensor_id);
|
||||
}
|
||||
|
||||
int mapSensorIdToSlot(int32_t sensor_id)
|
||||
{
|
||||
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
|
||||
if (getIdParam(slot) == sensor_id) {
|
||||
return slot;
|
||||
}
|
||||
}
|
||||
|
||||
// Not found, try to assign to first available slot
|
||||
for (int slot = 0; slot < MAX_AGP_IDS; slot++) {
|
||||
if (getIdParam(slot) == 0) {
|
||||
setIdParam(slot, sensor_id);
|
||||
return slot;
|
||||
}
|
||||
}
|
||||
|
||||
// All slots are full
|
||||
return MAX_AGP_IDS;
|
||||
}
|
||||
|
||||
#endif // MODULE_NAME
|
||||
};
|
||||
|
||||
@@ -53,6 +53,7 @@
|
||||
#include <uORB/topics/vehicle_optical_flow.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/aux_global_position.h>
|
||||
|
||||
#include "ReplayEkf2.hpp"
|
||||
|
||||
|
||||
@@ -129,17 +129,16 @@ void SensorAgpSim::Run()
|
||||
CONSTANTS_RADIUS_OF_EARTH);
|
||||
const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f));
|
||||
|
||||
vehicle_global_position_s sample{};
|
||||
aux_global_position_s sample{};
|
||||
|
||||
sample.id = 33;
|
||||
sample.timestamp_sample = gpos.timestamp_sample;
|
||||
sample.lat = latitude;
|
||||
sample.lon = longitude;
|
||||
sample.alt = altitude;
|
||||
sample.lat_lon_valid = true;
|
||||
sample.alt_ellipsoid = altitude;
|
||||
sample.alt_valid = true;
|
||||
sample.eph = 20.f;
|
||||
sample.epv = 5.f;
|
||||
sample.lat_lon_reset_counter = 0;
|
||||
|
||||
sample.timestamp = hrt_absolute_time();
|
||||
_aux_global_position_pub.publish(sample);
|
||||
|
||||
@@ -44,6 +44,7 @@
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/aux_global_position.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@@ -83,7 +84,7 @@ private:
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
|
||||
|
||||
uORB::PublicationMulti<vehicle_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
|
||||
uORB::PublicationMulti<aux_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
|
||||
|
||||
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user