fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 13:53:34 +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/mavlink_main.h
-2
View File
@@ -502,7 +502,6 @@ public:
bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); }
bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); }
bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); }
bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); }
@@ -675,7 +674,6 @@ private:
(ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
(ParamInt<px4::params::MAV_RADIO_TOUT>) _param_mav_radio_timeout,
(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
(ParamBool<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled
src/modules/mavlink/mavlink_params.c
-11
View File
@@ -142,17 +142,6 @@ PARAM_DEFINE_INT32(MAV_HASH_CHK_EN, 1);
*/
PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
/**
* Activate ODOMETRY loopback.
*
* If set, it gets the data from 'vehicle_visual_odometry' instead of 'vehicle_odometry'
* serving as a loopback of the received ODOMETRY messages on the Mavlink receiver.
*
* @boolean
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
/**
* Timeout in seconds for the RADIO_STATUS reports coming in
*
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