diff --git a/msg/GpsAltitudeDriftCorrection.msg b/msg/GpsAltitudeDriftCorrection.msg index 234953d6fb..ea24f2a81f 100644 --- a/msg/GpsAltitudeDriftCorrection.msg +++ b/msg/GpsAltitudeDriftCorrection.msg @@ -1,4 +1,3 @@ uint64 timestamp # [us] time since system start float32 altitude_offset # [m] detected altitude offset to apply - diff --git a/msg/px4_msgs_old/msg/VehicleLocalPositionV1.msg b/msg/px4_msgs_old/msg/VehicleLocalPositionV1.msg new file mode 100644 index 0000000000..3178a00bb3 --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleLocalPositionV1.msg @@ -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 diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index d59b232462..d23c746b95 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -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" diff --git a/msg/translation_node/translations/translation_vehicle_local_position_v1.h b/msg/translation_node/translations/translation_vehicle_local_position_v1.h index 4c6fb67f64..8591ed0ab3 100644 --- a/msg/translation_node/translations/translation_vehicle_local_position_v1.h +++ b/msg/translation_node/translations/translation_vehicle_local_position_v1.h @@ -6,14 +6,14 @@ // Translate VehicleLocalPosition v0 <--> v1 #include -#include +#include 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"; diff --git a/msg/translation_node/translations/translation_vehicle_local_position_v2.h b/msg/translation_node/translations/translation_vehicle_local_position_v2.h new file mode 100644 index 0000000000..2cfcfc8503 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_local_position_v2.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * Copyright (c) 2026 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleLocalPosition v1 <--> v2 +#include +#include + +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); diff --git a/msg/versioned/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg index 829ce32284..5d26451ed6 100644 --- a/msg/versioned/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -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) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7d18c502fa..59c6677def 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -183,13 +183,6 @@ public: // get the diagonal elements of the covariance matrix matrix::Vector 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); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 49054ce48f..f44ad9f52a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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 &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() diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 9807d1bb81..e4963d0881 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -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{};