fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 19:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Addressing dm_lock / unlock review comment

Browse Source
This commit is contained in:
Matej Frančeškin
2021-11-12 15:54:28 +01:00
parent ee51e716d6
commit c53a49b1ee
1 changed files with 96 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+96 -66
View File
@@ -1942,113 +1942,143 @@ void Mission::calculate_mission_checksums()
csum.rally_checksum = 0;
csum.all_checksum = 0;
bool fail = false;
dm_lock(DM_KEY_MISSION_STATE);
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
PX4_ERR("dataman read failure");
return;
fail = true;
}
dm_unlock(DM_KEY_MISSION_STATE);
dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
if (fail) {
return;
}
for (int i = 0; i < mission_state.count; i++) {
dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
dm_lock(dm_current);
for (int i = 0; i < mission_state.count && !fail; i++) {
struct mission_item_s mission_item = {};
const ssize_t len = sizeof(mission_item);
if (dm_read(dm_current, i, &mission_item, len) != len) {
PX4_ERR("dataman read failure");
return;
}
fail = true;
uint8_t frame = mission_item.frame;
csum.mission_checksum = crc32part(&frame, sizeof(frame), csum.mission_checksum);
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_item.nav_cmd), sizeof(mission_item.nav_cmd),
csum.mission_checksum);
uint8_t autocontinue = mission_item.autocontinue;
csum.mission_checksum = crc32part(&autocontinue, sizeof(autocontinue), csum.mission_checksum);
for (int j = 0; j < 4; j++) {
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_item.params[j]), sizeof(mission_item.params[j]),
} else {
uint8_t frame = mission_item.frame;
csum.mission_checksum = crc32part(&frame, sizeof(frame), csum.mission_checksum);
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_item.nav_cmd), sizeof(mission_item.nav_cmd),
csum.mission_checksum);
}
uint8_t autocontinue = mission_item.autocontinue;
csum.mission_checksum = crc32part(&autocontinue, sizeof(autocontinue), csum.mission_checksum);
float lat = static_cast<float>(mission_item.lat);
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.mission_checksum);
float lon = static_cast<float>(mission_item.lon);
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.mission_checksum);
float alt = mission_item.params[6];
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&alt), sizeof(alt), csum.mission_checksum);
for (int j = 0; j < 4; j++) {
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_item.params[j]), sizeof(mission_item.params[j]),
csum.mission_checksum);
}
float lat = static_cast<float>(mission_item.lat);
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.mission_checksum);
float lon = static_cast<float>(mission_item.lon);
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.mission_checksum);
float alt = mission_item.params[6];
csum.mission_checksum = crc32part(reinterpret_cast<uint8_t *>(&alt), sizeof(alt), csum.mission_checksum);
}
}
dm_unlock(dm_current);
if (fail) {
return;
}
csum.all_checksum = csum.mission_checksum;
dm_lock(DM_KEY_FENCE_POINTS);
if (dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
PX4_ERR("dataman read failure");
return;
fail = true;
}
for (size_t i = 1; i <= stats.num_items; i++) {
for (size_t i = 1; i <= stats.num_items && !fail; i++) {
mission_fence_point_s mission_fence_point;
const ssize_t len = sizeof(mission_fence_point);
if (dm_read(DM_KEY_FENCE_POINTS, i, &mission_fence_point, len) != len) {
PX4_ERR("dataman read failure");
return;
}
fail = true;
uint8_t frame = mission_fence_point.frame;
csum.fence_checksum = crc32part(&frame, sizeof(frame), csum.fence_checksum);
csum.all_checksum = crc32part(&frame, sizeof(frame), csum.all_checksum);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.nav_cmd),
sizeof(mission_fence_point.nav_cmd), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.nav_cmd),
sizeof(mission_fence_point.nav_cmd), csum.all_checksum);
float lat = static_cast<float>(mission_fence_point.lat);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.all_checksum);
float lon = static_cast<float>(mission_fence_point.lon);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.all_checksum);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.alt), sizeof(mission_fence_point.alt),
csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.alt), sizeof(mission_fence_point.alt),
csum.all_checksum);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.circle_radius),
sizeof(mission_fence_point.circle_radius), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.circle_radius),
sizeof(mission_fence_point.circle_radius), csum.all_checksum);
} else {
uint8_t frame = mission_fence_point.frame;
csum.fence_checksum = crc32part(&frame, sizeof(frame), csum.fence_checksum);
csum.all_checksum = crc32part(&frame, sizeof(frame), csum.all_checksum);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.nav_cmd),
sizeof(mission_fence_point.nav_cmd), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.nav_cmd),
sizeof(mission_fence_point.nav_cmd), csum.all_checksum);
float lat = static_cast<float>(mission_fence_point.lat);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.all_checksum);
float lon = static_cast<float>(mission_fence_point.lon);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.all_checksum);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.alt), sizeof(mission_fence_point.alt),
csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.alt), sizeof(mission_fence_point.alt),
csum.all_checksum);
csum.fence_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.circle_radius),
sizeof(mission_fence_point.circle_radius), csum.fence_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_fence_point.circle_radius),
sizeof(mission_fence_point.circle_radius), csum.all_checksum);
}
}
if (dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
PX4_ERR("dataman read failure");
dm_unlock(DM_KEY_FENCE_POINTS);
if (fail) {
return;
}
for (size_t i = 1; i <= stats.num_items; i++) {
dm_lock(DM_KEY_SAFE_POINTS);
if (dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)) != sizeof(mission_stats_entry_s)) {
PX4_ERR("dataman read failure");
fail = true;
}
for (size_t i = 1; i <= stats.num_items && !fail; i++) {
mission_safe_point_s mission_safe_point;
const ssize_t len = sizeof(mission_safe_point);
if (dm_read(DM_KEY_SAFE_POINTS, i, &mission_safe_point, len) != len) {
PX4_ERR("dataman read failure");
return;
}
fail = true;
uint8_t frame = mission_safe_point.frame;
csum.rally_checksum = crc32part(&frame, sizeof(frame), csum.rally_checksum);
csum.all_checksum = crc32part(&frame, sizeof(frame), csum.all_checksum);
float lat = static_cast<float>(mission_safe_point.lat);
csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.rally_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.all_checksum);
float lon = static_cast<float>(mission_safe_point.lon);
csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.rally_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.all_checksum);
csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_safe_point.alt), sizeof(mission_safe_point.alt),
csum.rally_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_safe_point.alt), sizeof(mission_safe_point.alt),
csum.all_checksum);
} else {
uint8_t frame = mission_safe_point.frame;
csum.rally_checksum = crc32part(&frame, sizeof(frame), csum.rally_checksum);
csum.all_checksum = crc32part(&frame, sizeof(frame), csum.all_checksum);
float lat = static_cast<float>(mission_safe_point.lat);
csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.rally_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lat), sizeof(lat), csum.all_checksum);
float lon = static_cast<float>(mission_safe_point.lon);
csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.rally_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&lon), sizeof(lon), csum.all_checksum);
csum.rally_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_safe_point.alt), sizeof(mission_safe_point.alt),
csum.rally_checksum);
csum.all_checksum = crc32part(reinterpret_cast<uint8_t *>(&mission_safe_point.alt), sizeof(mission_safe_point.alt),
csum.all_checksum);
}
}
mission_checksum_pub.publish(csum);
}
dm_unlock(DM_KEY_SAFE_POINTS);
if (!fail) {
mission_checksum_pub.publish(csum);
}
}