mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 22:32:11 +08:00
fix(ekf2): add translation for vehicle_local_position
This commit is contained in:
Marco Hauswirth
@@ -1,4 +1,3 @@
|
||||
uint64 timestamp # [us] time since system start
|
||||
|
||||
float32 altitude_offset # [m] detected altitude offset to apply
|
||||
|
||||
|
||||
@@ -0,0 +1,88 @@
|
||||
# Fused local position in NED.
|
||||
# The coordinate system origin is the vehicle position at the time when the EKF2-module was started.
|
||||
|
||||
uint32 MESSAGE_VERSION = 1
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
bool xy_valid # true if x and y are valid
|
||||
bool z_valid # true if z is valid
|
||||
bool v_xy_valid # true if vx and vy are valid
|
||||
bool v_z_valid # true if vz is valid
|
||||
|
||||
# Position in local NED frame
|
||||
float32 x # North position in NED earth-fixed frame, (metres)
|
||||
float32 y # East position in NED earth-fixed frame, (metres)
|
||||
float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres)
|
||||
|
||||
# Position reset delta
|
||||
float32[2] delta_xy # Amount of lateral shift of position estimate in latest reset (in x and y) [m]
|
||||
uint8 xy_reset_counter # Index of latest lateral position estimate reset
|
||||
float32 delta_z # Amount of vertical shift of position estimate in latest reset [m]
|
||||
uint8 z_reset_counter # Index of latest vertical position estimate reset
|
||||
|
||||
# Velocity in NED frame
|
||||
float32 vx # North velocity in NED earth-fixed frame, (metres/sec)
|
||||
float32 vy # East velocity in NED earth-fixed frame, (metres/sec)
|
||||
float32 vz # Down velocity in NED earth-fixed frame, (metres/sec)
|
||||
float32 z_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec)
|
||||
|
||||
# Velocity reset delta
|
||||
float32[2] delta_vxy # Amount of lateral shift of velocity estimate in latest reset (in x and y) [m/s]
|
||||
uint8 vxy_reset_counter # Index of latest vertical velocity estimate reset
|
||||
float32 delta_vz # Amount of vertical shift of velocity estimate in latest reset [m/s]
|
||||
uint8 vz_reset_counter # Index of latest vertical velocity estimate reset
|
||||
|
||||
# Acceleration in NED frame
|
||||
float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2)
|
||||
float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2)
|
||||
float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
|
||||
|
||||
float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians)
|
||||
float32 heading_var
|
||||
float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only
|
||||
float32 delta_heading # Heading delta caused by latest heading reset [rad]
|
||||
uint8 heading_reset_counter # Index of latest heading reset
|
||||
bool heading_good_for_control
|
||||
|
||||
float32 tilt_var
|
||||
|
||||
# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
|
||||
bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon)
|
||||
bool z_global # true if z has a valid global reference (ref_alt)
|
||||
uint64 ref_timestamp # Time when reference position was set since system start, (microseconds)
|
||||
float64 ref_lat # Reference point latitude, (degrees)
|
||||
float64 ref_lon # Reference point longitude, (degrees)
|
||||
float32 ref_alt # Reference altitude AMSL, (metres)
|
||||
|
||||
# Distance to surface
|
||||
bool dist_bottom_valid # true if distance to bottom surface is valid
|
||||
float32 dist_bottom # Distance from from bottom surface to ground, (metres)
|
||||
float32 dist_bottom_var # terrain estimate variance (m^2)
|
||||
|
||||
float32 delta_dist_bottom # Amount of vertical shift of dist bottom estimate in latest reset [m]
|
||||
uint8 dist_bottom_reset_counter # Index of latest dist bottom estimate reset
|
||||
|
||||
uint8 dist_bottom_sensor_bitfield # bitfield indicating what type of sensor is used to estimate dist_bottom
|
||||
uint8 DIST_BOTTOM_SENSOR_NONE = 0
|
||||
uint8 DIST_BOTTOM_SENSOR_RANGE = 1 # (1 << 0) a range sensor is used to estimate dist_bottom field
|
||||
uint8 DIST_BOTTOM_SENSOR_FLOW = 2 # (1 << 1) a flow sensor is used to estimate dist_bottom field (mostly fixed-wing use case)
|
||||
|
||||
float32 eph # Standard deviation of horizontal position error, (metres)
|
||||
float32 epv # Standard deviation of vertical position error, (metres)
|
||||
float32 evh # Standard deviation of horizontal velocity error, (metres/sec)
|
||||
float32 evv # Standard deviation of vertical velocity error, (metres/sec)
|
||||
|
||||
bool dead_reckoning # True if this position is estimated through dead-reckoning
|
||||
|
||||
# estimator specified vehicle limits
|
||||
# set to INFINITY when limiting not required
|
||||
float32 vxy_max # maximum horizontal speed (meters/sec)
|
||||
float32 vz_max # maximum vertical speed (meters/sec)
|
||||
float32 hagl_min # minimum height above ground level (meters)
|
||||
float32 hagl_max_z # maximum height above ground level for z-control (meters)
|
||||
float32 hagl_max_xy # maximum height above ground level for xy-control (meters)
|
||||
|
||||
# TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
|
||||
# TOPICS estimator_local_position
|
||||
@@ -20,6 +20,7 @@
|
||||
#include "translation_vehicle_attitude_setpoint_v1.h"
|
||||
#include "translation_vehicle_command_ack_v1.h"
|
||||
#include "translation_vehicle_local_position_v1.h"
|
||||
#include "translation_vehicle_local_position_v2.h"
|
||||
#include "translation_vehicle_status_v1.h"
|
||||
#include "translation_vehicle_status_v2.h"
|
||||
#include "translation_vehicle_status_v3.h"
|
||||
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
// Translate VehicleLocalPosition v0 <--> v1
|
||||
#include <px4_msgs_old/msg/vehicle_local_position_v0.hpp>
|
||||
#include <px4_msgs/msg/vehicle_local_position.hpp>
|
||||
#include <px4_msgs_old/msg/vehicle_local_position_v1.hpp>
|
||||
|
||||
class VehicleLocalPositionV1Translation {
|
||||
public:
|
||||
using MessageOlder = px4_msgs_old::msg::VehicleLocalPositionV0;
|
||||
static_assert(MessageOlder::MESSAGE_VERSION == 0);
|
||||
|
||||
using MessageNewer = px4_msgs::msg::VehicleLocalPosition;
|
||||
using MessageNewer = px4_msgs_old::msg::VehicleLocalPositionV1;
|
||||
static_assert(MessageNewer::MESSAGE_VERSION == 1);
|
||||
|
||||
static constexpr const char* kTopic = "fmu/out/vehicle_local_position";
|
||||
|
||||
@@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2026 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
// Translate VehicleLocalPosition v1 <--> v2
|
||||
#include <px4_msgs_old/msg/vehicle_local_position_v1.hpp>
|
||||
#include <px4_msgs/msg/vehicle_local_position.hpp>
|
||||
|
||||
class VehicleLocalPositionV2Translation {
|
||||
public:
|
||||
using MessageOlder = px4_msgs_old::msg::VehicleLocalPositionV1;
|
||||
static_assert(MessageOlder::MESSAGE_VERSION == 1);
|
||||
|
||||
using MessageNewer = px4_msgs::msg::VehicleLocalPosition;
|
||||
static_assert(MessageNewer::MESSAGE_VERSION == 2);
|
||||
|
||||
static constexpr const char* kTopic = "fmu/out/vehicle_local_position";
|
||||
|
||||
#define COMMON_VEHICLE_LOCAL_POSITION_FIELDS(FUNCTION) \
|
||||
FUNCTION(timestamp) \
|
||||
FUNCTION(timestamp_sample) \
|
||||
FUNCTION(xy_valid) \
|
||||
FUNCTION(z_valid) \
|
||||
FUNCTION(v_xy_valid) \
|
||||
FUNCTION(v_z_valid) \
|
||||
FUNCTION(x) \
|
||||
FUNCTION(y) \
|
||||
FUNCTION(z) \
|
||||
FUNCTION(delta_xy) \
|
||||
FUNCTION(xy_reset_counter) \
|
||||
FUNCTION(delta_z) \
|
||||
FUNCTION(z_reset_counter) \
|
||||
FUNCTION(vx) \
|
||||
FUNCTION(vy) \
|
||||
FUNCTION(vz) \
|
||||
FUNCTION(z_deriv) \
|
||||
FUNCTION(delta_vxy) \
|
||||
FUNCTION(vxy_reset_counter) \
|
||||
FUNCTION(delta_vz) \
|
||||
FUNCTION(vz_reset_counter) \
|
||||
FUNCTION(ax) \
|
||||
FUNCTION(ay) \
|
||||
FUNCTION(az) \
|
||||
FUNCTION(heading) \
|
||||
FUNCTION(heading_var) \
|
||||
FUNCTION(unaided_heading) \
|
||||
FUNCTION(delta_heading) \
|
||||
FUNCTION(heading_reset_counter) \
|
||||
FUNCTION(heading_good_for_control) \
|
||||
FUNCTION(tilt_var) \
|
||||
FUNCTION(xy_global) \
|
||||
FUNCTION(z_global) \
|
||||
FUNCTION(ref_timestamp) \
|
||||
FUNCTION(ref_lat) \
|
||||
FUNCTION(ref_lon) \
|
||||
FUNCTION(ref_alt) \
|
||||
FUNCTION(dist_bottom_valid) \
|
||||
FUNCTION(dist_bottom) \
|
||||
FUNCTION(dist_bottom_var) \
|
||||
FUNCTION(delta_dist_bottom) \
|
||||
FUNCTION(dist_bottom_reset_counter) \
|
||||
FUNCTION(dist_bottom_sensor_bitfield) \
|
||||
FUNCTION(eph) \
|
||||
FUNCTION(epv) \
|
||||
FUNCTION(evh) \
|
||||
FUNCTION(evv) \
|
||||
FUNCTION(dead_reckoning) \
|
||||
FUNCTION(vxy_max) \
|
||||
FUNCTION(vz_max) \
|
||||
FUNCTION(hagl_min) \
|
||||
FUNCTION(hagl_max_z) \
|
||||
FUNCTION(hagl_max_xy)
|
||||
|
||||
#define COPY_FIELD_FROM_OLDER(field) msg_newer.field = msg_older.field;
|
||||
#define COPY_FIELD_TO_OLDER(field) msg_older.field = msg_newer.field;
|
||||
|
||||
static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
|
||||
COMMON_VEHICLE_LOCAL_POSITION_FIELDS(COPY_FIELD_FROM_OLDER)
|
||||
msg_newer.altitude_good_for_local_control = false;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
|
||||
COMMON_VEHICLE_LOCAL_POSITION_FIELDS(COPY_FIELD_TO_OLDER)
|
||||
}
|
||||
|
||||
#undef COPY_FIELD_FROM_OLDER
|
||||
#undef COPY_FIELD_TO_OLDER
|
||||
#undef COMMON_VEHICLE_LOCAL_POSITION_FIELDS
|
||||
};
|
||||
|
||||
REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleLocalPositionV2Translation);
|
||||
@@ -1,7 +1,7 @@
|
||||
# Fused local position in NED.
|
||||
# The coordinate system origin is the vehicle position at the time when the EKF2-module was started.
|
||||
|
||||
uint32 MESSAGE_VERSION = 1
|
||||
uint32 MESSAGE_VERSION = 2
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
|
||||
|
||||
@@ -183,13 +183,6 @@ public:
|
||||
// get the diagonal elements of the covariance matrix
|
||||
matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
|
||||
|
||||
void shiftAltOrigin(float offset)
|
||||
{
|
||||
if (PX4_ISFINITE(_local_origin_alt)) {
|
||||
_local_origin_alt += offset;
|
||||
}
|
||||
}
|
||||
|
||||
void decorrelateAltPos()
|
||||
{
|
||||
P.uncorrelateCovariance<1>(State::pos.idx + 2);
|
||||
|
||||
@@ -2631,8 +2631,6 @@ void EKF2::GpsAltDriftDetector::updateBaroLpf(float baro_alt, uint64_t timestamp
|
||||
void EKF2::GpsAltDriftDetector::update(const sensor_gps_s &gps, float ekf_amsl,
|
||||
uORB::PublicationMulti<gps_altitude_drift_correction_s> &pub)
|
||||
{
|
||||
_altitude_offset = 0.f;
|
||||
|
||||
const bool gps_timeout = (_last_gps_ts != 0) && (gps.timestamp - _last_gps_ts > 500000);
|
||||
|
||||
if (gps_timeout || _last_gps_ts == 0 || _last_baro_ts == 0) {
|
||||
@@ -2719,7 +2717,6 @@ void EKF2::GpsAltDriftDetector::publishCorrection(
|
||||
correction.timestamp = hrt_absolute_time();
|
||||
correction.altitude_offset = offset;
|
||||
pub.publish(correction);
|
||||
_altitude_offset += offset;
|
||||
}
|
||||
|
||||
void EKF2::GpsAltDriftDetector::reset()
|
||||
|
||||
@@ -515,7 +515,6 @@ private:
|
||||
public:
|
||||
static constexpr int kWindowSize = 20; // roughly 20 seconds (at 1 Hz sample rate)
|
||||
static constexpr int kStabilityWindow = 5; // samples to check for re-enable
|
||||
static constexpr float kBaroLpfTimeConst = 3.f;
|
||||
static constexpr float kDriftThreshold = 1.f; // [m]
|
||||
|
||||
void updateBaroLpf(float baro_alt, uint64_t timestamp);
|
||||
@@ -539,7 +538,6 @@ private:
|
||||
int _wcount{0};
|
||||
uint64_t _last_sample_ts{0};
|
||||
bool _hit_pending{false};
|
||||
float _altitude_offset{0.f};
|
||||
};
|
||||
|
||||
GpsAltDriftDetector _gps_alt_drift{};
|
||||
|
||||
Reference in New Issue
Block a user