fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 17:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: reset position by fusion (#23279)

Browse Source 
* reset position by fusion

* handle local_pos_valid for fixed wing in gnss denied

* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

* global origin, also reset vertical pos without gps as ref

* fix wo gnss, that bitcraze ci passes

* revert some changes as requested

* remove duplicate reset messages

* undo unrelated whitespace changes, I'll fix it everywhere in a followup

* [SQUASH] ekf2: add vehicle_command_ack

* resetGlobalPosToExternalObservation consolidate logic

* remove gnss check from local_pos validation check

* reset when  0<accuracy<1, otherwise fuse

* replace gps param with flag

* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected

* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Marco Hauswirth
2024-07-07 22:43:55 +02:00
committed by GitHub
parent ac1effa32a
commit e04c53241a
10 changed files with 182 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+32 -6
View File
@@ -497,6 +497,12 @@ void EKF2::Run()
vehicle_command_s vehicle_command;
if (_vehicle_command_sub.update(&vehicle_command)) {
vehicle_command_ack_s command_ack{};
command_ack.command = vehicle_command.command;
command_ack.target_system = vehicle_command.source_system;
command_ack.target_component = vehicle_command.source_component;
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
double latitude = vehicle_command.param5;
double longitude = vehicle_command.param6;
@@ -509,15 +515,23 @@ void EKF2::Run()
PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
_instance, latitude, longitude, static_cast<double>(altitude));
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
_instance, latitude, longitude, static_cast<double>(altitude));
}
}
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
}
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) &&
PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) {
PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)
) {
const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,
kMaxDelaySecondsExternalPosMeasurement);
@@ -530,9 +544,21 @@ void EKF2::Run()
}
// TODO add check for lat and long validity
_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
accuracy, timestamp_observation);
if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
accuracy, timestamp_observation)
) {
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
}
} else {
command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; // TODO: expand
}
command_ack.timestamp = hrt_absolute_time();
_vehicle_command_ack_pub.publish(command_ack);
}
}
}