fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar
2022-12-20 10:23:56 -05:00
committed by GitHub
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/streams/ODOMETRY.hpp
+5 -27
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 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
@@ -49,42 +49,20 @@ public:
unsigned get_size() override
{
if (_mavlink->odometry_loopback_enabled()) {
return _vodom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
} else {
return _odom_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
return _vehicle_odometry_sub.advertised() ? MAVLINK_MSG_ID_ODOMETRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _odom_sub{ORB_ID(vehicle_odometry)};
uORB::Subscription _vodom_sub{ORB_ID(vehicle_visual_odometry)};
uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)};
bool send() override
{
vehicle_odometry_s odom;
// check if it is to send visual odometry loopback or not
bool odom_updated = false;
mavlink_odometry_t msg{};
if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom);
// source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else {
odom_updated = _odom_sub.update(&odom);
// source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
}
if (odom_updated) {
if (_vehicle_odometry_sub.update(&odom)) {
mavlink_odometry_t msg{};
msg.time_usec = odom.timestamp_sample;
// set the frame_id according to the local frame of the data