Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

fix(navigator): check geofence dataman update failures (#27353)

Browse Source 
* navigator: check geofence dataman update failures

* navigator: fix geofence import state handling

- Move the import success log after the dataman state write succeeds
- Store the active fence dataman id in the stats entry
- Add error logs before geofence import failure exits
This commit is contained in:
Eurus
2026-05-19 03:55:41 +08:00
committed by GitHub
parent 727e0cf5be
commit 68ba1e487a
1 changed files with 22 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+22 -8
View File
@@ -612,6 +612,7 @@ Geofence::loadFromFile(const char *filename)
sizeof(vertex));
if (!success) {
PX4_ERR("Failed to write geofence vertex to dataman");
goto error;
}
@@ -622,6 +623,7 @@ Geofence::loadFromFile(const char *filename)
} else {
/* Parse the line as the vertical limits */
if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) {
PX4_ERR("Scanf to parse geofence vertical limits failed.");
goto error;
}
@@ -633,8 +635,6 @@ Geofence::loadFromFile(const char *filename)
/* Check if import was successful */
if (gotVertical && pointCounter > 2) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t");
events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported");
ret_val = PX4_ERROR;
uint32_t crc32{0U};
@@ -645,23 +645,37 @@ Geofence::loadFromFile(const char *filename)
bool success = _dataman_client.readSync(write_fence_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
if (success) {
mission_fence_point.vertex_count = pointCounter;
crc32 = crc32_for_fence_point(mission_fence_point, crc32);
_dataman_client.writeSync(write_fence_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
if (!success) {
PX4_ERR("Failed to read geofence vertex from dataman");
goto error;
}
mission_fence_point.vertex_count = pointCounter;
crc32 = crc32_for_fence_point(mission_fence_point, crc32);
success = _dataman_client.writeSync(write_fence_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_fence_point),
sizeof(mission_fence_point_s));
if (!success) {
PX4_ERR("Failed to update geofence vertex count in dataman");
goto error;
}
}
mission_stats_entry_s stats;
mission_stats_entry_s stats{};
stats.num_items = pointCounter;
stats.opaque_id = crc32;
stats.dataman_id = write_fence_dataman_id;
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));
if (success) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t");
events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported");
ret_val = PX4_OK;
} else {
PX4_ERR("Failed to write geofence dataman state");
}
} else {