mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
fix(dataman_client): fail fast when dataman is unavailable (#26652)
* fix(dataman_client): fail fast when dataman is unavailable Check client_id before every DatamanClient operation to avoid waiting for a response timeout when dataman was never running. - Adds CLIENT_ID_NOT_SET guard to all sync and async methods - Avoids cross-module linkage between dataman_client lib and dataman module Supersedes #26128 * better init
This commit is contained in:
@@ -148,6 +148,10 @@ bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_respon
|
||||
|
||||
bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
|
||||
{
|
||||
if (_client_id == CLIENT_ID_NOT_SET) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (length > g_per_item_size[item]) {
|
||||
PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast<uint8_t>(item));
|
||||
return false;
|
||||
@@ -184,6 +188,10 @@ bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, ui
|
||||
|
||||
bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout)
|
||||
{
|
||||
if (_client_id == CLIENT_ID_NOT_SET) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (length > g_per_item_size[item]) {
|
||||
PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast<uint8_t>(item));
|
||||
return false;
|
||||
@@ -219,6 +227,10 @@ bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, u
|
||||
|
||||
bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout)
|
||||
{
|
||||
if (_client_id == CLIENT_ID_NOT_SET) {
|
||||
return false;
|
||||
}
|
||||
|
||||
hrt_abstime timestamp = hrt_absolute_time();
|
||||
|
||||
dataman_request_s request;
|
||||
@@ -245,6 +257,10 @@ bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout)
|
||||
|
||||
bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length)
|
||||
{
|
||||
if (_client_id == CLIENT_ID_NOT_SET) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (length > g_per_item_size[item]) {
|
||||
PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast<uint8_t>(item));
|
||||
return false;
|
||||
@@ -283,6 +299,10 @@ bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, u
|
||||
|
||||
bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length)
|
||||
{
|
||||
if (_client_id == CLIENT_ID_NOT_SET) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (length > g_per_item_size[item]) {
|
||||
PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast<uint8_t>(item));
|
||||
return false;
|
||||
@@ -323,6 +343,10 @@ bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer,
|
||||
|
||||
bool DatamanClient::clearAsync(dm_item_t item)
|
||||
{
|
||||
if (_client_id == CLIENT_ID_NOT_SET) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
|
||||
if (_state == State::Idle) {
|
||||
|
||||
@@ -183,11 +183,11 @@ private:
|
||||
|
||||
px4_pollfd_struct_t _fds;
|
||||
|
||||
uint8_t _client_id{0};
|
||||
static constexpr uint8_t CLIENT_ID_NOT_SET{0};
|
||||
|
||||
uint8_t _client_id{CLIENT_ID_NOT_SET};
|
||||
|
||||
perf_counter_t _sync_perf{nullptr};
|
||||
|
||||
static constexpr uint8_t CLIENT_ID_NOT_SET{0};
|
||||
};
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user