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

dataman: add DatamanClient with sync functions

Browse Source 
Rework of dataman
This commit is contained in:
Igor Mišić
2023-07-24 13:10:31 +02:00
committed by Beat Küng
parent c67af1479f
commit 208552fdab
27 changed files with 1392 additions and 948 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/CMakeLists.txt
+1
View File
@@ -131,6 +131,7 @@ px4_add_module(
adsb
airspeed
component_general_json # for checksums.h
dataman_client
drivers_accelerometer
drivers_gyroscope
drivers_magnetometer
src/modules/mavlink/mavlink_mission.cpp
+54 -49
View File
@@ -84,27 +84,28 @@ MavlinkMissionManager::init_offboard_mission()
_dataman_init = true;
/* lock MISSION_STATE item */
int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE);
if (dm_lock_ret != 0) {
if (!success_lock) {
PX4_ERR("DM_KEY_MISSION_STATE lock failed");
}
mission_s mission_state;
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
sizeof(mission_s));
/* unlock MISSION_STATE item */
if (dm_lock_ret == 0) {
dm_unlock(DM_KEY_MISSION_STATE);
if (success_lock) {
_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
}
if (ret > 0) {
if (success) {
_dataman_id = (dm_item_t)mission_state.dataman_id;
_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
_current_seq = mission_state.current_seq;
} else if (ret < 0) {
PX4_WARN("offboard mission init failed (%i)", ret);
} else {
PX4_WARN("offboard mission init failed");
}
load_geofence_stats();
@@ -115,33 +116,35 @@ MavlinkMissionManager::init_offboard_mission()
_my_dataman_id = _dataman_id;
}
int
bool
MavlinkMissionManager::load_geofence_stats()
{
mission_stats_entry_s stats;
// initialize fence points count
int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));
if (ret == sizeof(mission_stats_entry_s)) {
if (success) {
_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
_geofence_update_counter = stats.update_counter;
}
return ret;
return success;
}
int
bool
MavlinkMissionManager::load_safepoint_stats()
{
mission_stats_entry_s stats;
// initialize safe points count
int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));
if (ret == sizeof(mission_stats_entry_s)) {
if (success) {
_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
}
return ret;
return success;
}
/**
@@ -160,20 +163,21 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
/* update mission state in dataman */
/* lock MISSION_STATE item */
int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
bool success_lock = _dataman_client.lockSync(DM_KEY_MISSION_STATE);
if (dm_lock_ret != 0) {
if (!success_lock) {
PX4_ERR("DM_KEY_MISSION_STATE lock failed");
}
int res = dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission),
sizeof(mission_s));
/* unlock MISSION_STATE item */
if (dm_lock_ret == 0) {
dm_unlock(DM_KEY_MISSION_STATE);
if (success_lock) {
_dataman_client.unlockSync(DM_KEY_MISSION_STATE);
}
if (res == sizeof(mission_s)) {
if (success) {
/* update active mission state */
_dataman_id = dataman_id;
_count[MAV_MISSION_TYPE_MISSION] = count;
@@ -206,9 +210,10 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data
/* update stats in dataman */
int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));
if (res == sizeof(mission_stats_entry_s)) {
if (success) {
_count[MAV_MISSION_TYPE_FENCE] = count;
} else {
@@ -233,9 +238,10 @@ MavlinkMissionManager::update_safepoint_count(unsigned count)
stats.update_counter = ++_safepoint_update_counter;
/* update stats in dataman */
int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast<uint8_t *>(&stats),
sizeof(mission_stats_entry_s));
if (res == sizeof(mission_stats_entry_s)) {
if (success) {
_count[MAV_MISSION_TYPE_RALLY] = count;
} else {
@@ -298,21 +304,20 @@ void
MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
{
mission_item_s mission_item{};
int16_t bytes_read = 0;
bool read_success = false;
switch (_mission_type) {
case MAV_MISSION_TYPE_MISSION: {
bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s));
read_success = (bytes_read == sizeof(mission_item_s));
read_success = _dataman_client.readSync(_dataman_id, seq, reinterpret_cast<uint8_t *>(&mission_item),
sizeof(mission_item_s));
}
break;
case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
mission_fence_point_s mission_fence_point;
bytes_read = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s));
read_success = (bytes_read == sizeof(mission_fence_point_s));
read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1,
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
mission_item.nav_cmd = mission_fence_point.nav_cmd;
mission_item.frame = mission_fence_point.frame;
@@ -332,8 +337,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
mission_safe_point_s mission_safe_point;
bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s));
read_success = (bytes_read == sizeof(mission_safe_point_s));
read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast<uint8_t *>(&mission_safe_point),
sizeof(mission_safe_point_s));
mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
mission_item.frame = mission_safe_point.frame;
@@ -385,13 +390,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) {
mavlink_log_critical(_mavlink->get_mavlink_log_pub(),
"Mission storage: Unable to read from storage, type: %" PRId8 " bytes read: %" PRId16 "\t",
(uint8_t)_mission_type, bytes_read);
"Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type);
/* EVENT
* @description Mission type: {1}. Number of bytes read: {2}
* @description Mission type: {1}
*/
events::send<uint8_t, int16_t>(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
"Mission: Unable to read from storage", _mission_type, bytes_read);
events::send<uint8_t>(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error,
"Mission: Unable to read from storage", _mission_type);
}
PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id);
@@ -961,9 +965,9 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
// switching back to idle
PX4_DEBUG("locking fence dataman items");
int ret = dm_lock(DM_KEY_FENCE_POINTS);
bool success = _dataman_client.lockSync(DM_KEY_FENCE_POINTS);
if (ret == 0) {
if (success) {
_geofence_locked = true;
} else {
@@ -1009,10 +1013,10 @@ MavlinkMissionManager::switch_to_idle_state()
// when switching to idle, we *always* check if the lock was held and release it.
// This is to ensure we don't end up in a state where we forget to release it.
if (_geofence_locked) {
dm_unlock(DM_KEY_FENCE_POINTS);
_geofence_locked = false;
PX4_DEBUG("unlocking geofence");
if (_dataman_client.unlockSync(DM_KEY_FENCE_POINTS)) {
_geofence_locked = false;
PX4_DEBUG("unlocking geofence");
}
}
_state = MAVLINK_WPM_STATE_IDLE;
@@ -1131,7 +1135,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
} else {
dm_item_t dm_item = _transfer_dataman_id;
write_failed = dm_write(dm_item, wp.seq, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s);
write_failed = !_dataman_client.writeSync(dm_item, wp.seq, reinterpret_cast<uint8_t *>(&mission_item),
sizeof(struct mission_item_s));
if (!write_failed) {
/* waypoint marked as current */
@@ -1167,8 +1172,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
mission_fence_point.frame = mission_item.frame;
if (!check_failed) {
write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point,
sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s);
write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1,
reinterpret_cast<uint8_t *>(&mission_fence_point), sizeof(mission_fence_point_s));
}
}
@@ -1180,8 +1185,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
mission_safe_point.lon = mission_item.lon;
mission_safe_point.alt = mission_item.altitude;
mission_safe_point.frame = mission_item.frame;
write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point,
sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s);
write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1,
reinterpret_cast<uint8_t *>(&mission_safe_point), sizeof(mission_safe_point_s));
}
break;
src/modules/mavlink/mavlink_mission.h
+6 -4
View File
@@ -45,7 +45,7 @@
#pragma once
#include <dataman/dataman.h>
#include <dataman_client/DatamanClient.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/mission_result.h>
@@ -95,6 +95,8 @@ private:
enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state
enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible)
DatamanClient _dataman_client{};
uint64_t _time_last_recv{0};
uint64_t _time_last_sent{0};
@@ -132,7 +134,7 @@ private:
static uint16_t _geofence_update_counter;
static uint16_t _safepoint_update_counter;
bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress)
bool _geofence_locked{false}; ///< if true, we currently hold the dataman lock for the geofence (transaction in progress)
MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz
@@ -168,10 +170,10 @@ private:
int update_safepoint_count(unsigned count);
/** load geofence stats from dataman */
int load_geofence_stats();
bool load_geofence_stats();
/** load safe point stats from dataman */
int load_safepoint_stats();
bool load_safepoint_stats();
/**
* @brief Sends an waypoint ack message