mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 01:33:34 +08:00
parameters: support for an optional remote parameter database (#22836)
The voxl2 has a split architecture. PX4 runs on a posix platform and a Qurt platform. The two communicate uorb topics back and forth with the muorb module. But each has it's own parameters database and they need to stay in sync with each other. This PR adds support to keep the 2 parameter databases in sync. The main parameters database running on Linux has file system support while the Qurt one does not. The Linux side is considered the primary and the Qurt side is considered the remote.
This commit is contained in:
Eric Katzfey
@@ -0,0 +1,310 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "parameters_remote.h"
|
||||
|
||||
#include "uORB/uORBManager.hpp"
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
|
||||
// uORB topics needed to keep parameter server and client in sync
|
||||
#include <uORB/topics/parameter_reset_request.h>
|
||||
#include <uORB/topics/parameter_set_used_request.h>
|
||||
#include <uORB/topics/parameter_set_value_request.h>
|
||||
#include <uORB/topics/parameter_set_value_response.h>
|
||||
|
||||
// Debug flag
|
||||
static bool debug = false;
|
||||
|
||||
static struct param_remote_counters param_remote_counters;
|
||||
|
||||
#define TIMEOUT_WAIT 1000
|
||||
#define TIMEOUT_COUNT 50
|
||||
|
||||
static orb_advert_t parameter_set_used_h = nullptr;
|
||||
static orb_advert_t param_set_value_req_h = nullptr;
|
||||
|
||||
static int param_set_rsp_fd = PX4_ERROR;
|
||||
|
||||
static px4_task_t sync_thread_tid;
|
||||
static const char *sync_thread_name = "param_remote_sync";
|
||||
|
||||
static int remote_sync_thread(int argc, char *argv[])
|
||||
{
|
||||
// This thread gets started by the remote side during PX4 initialization.
|
||||
// We cannot send out the subscribe request immediately because the other
|
||||
// side will not be ready to receive it on the muorb yet and it will get dropped.
|
||||
// So, sleep a little bit to give other side a chance to finish initialization
|
||||
// of the muorb. But don't wait too long otherwise a set request from the server
|
||||
// side could be missed.
|
||||
usleep(200000);
|
||||
|
||||
orb_advert_t _set_value_rsp_h = nullptr;
|
||||
|
||||
int _reset_req_fd = orb_subscribe(ORB_ID(parameter_reset_request));
|
||||
int _set_value_req_fd = orb_subscribe(ORB_ID(parameter_remote_set_value_request));
|
||||
|
||||
struct parameter_reset_request_s _reset_request;
|
||||
struct parameter_set_value_request_s _set_value_request;
|
||||
struct parameter_set_value_response_s _set_value_response;
|
||||
|
||||
px4_pollfd_struct_t fds[2] = { { .fd = _reset_req_fd, .events = POLLIN },
|
||||
{ .fd = _set_value_req_fd, .events = POLLIN }
|
||||
};
|
||||
|
||||
PX4_INFO("Starting parameter remote sync thread");
|
||||
|
||||
while (true) {
|
||||
px4_poll(fds, 2, 1000);
|
||||
|
||||
if (fds[0].revents & POLLIN) {
|
||||
bool updated = true;
|
||||
|
||||
while (updated) {
|
||||
orb_copy(ORB_ID(parameter_reset_request), _reset_req_fd, &_reset_request);
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("Got parameter_reset_request for %s", param_name(_reset_request.parameter_index));
|
||||
}
|
||||
|
||||
param_remote_counters.reset_received++;
|
||||
|
||||
if (_reset_request.reset_all) {
|
||||
param_reset_all();
|
||||
|
||||
} else {
|
||||
param_reset_no_notification(_reset_request.parameter_index);
|
||||
|
||||
}
|
||||
|
||||
(void) orb_check(_reset_req_fd, &updated);
|
||||
}
|
||||
}
|
||||
|
||||
if (fds[1].revents & POLLIN) {
|
||||
bool updated = true;
|
||||
|
||||
while (updated) {
|
||||
orb_copy(ORB_ID(parameter_primary_set_value_request), _set_value_req_fd, &_set_value_request);
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("Got parameter_remote_set_value_request for %s", param_name(_set_value_request.parameter_index));
|
||||
}
|
||||
|
||||
param_remote_counters.set_value_request_received++;
|
||||
|
||||
switch (param_type(_set_value_request.parameter_index)) {
|
||||
case PARAM_TYPE_INT32:
|
||||
param_set_no_remote_update(_set_value_request.parameter_index,
|
||||
(const void *) &_set_value_request.int_value,
|
||||
false);
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_FLOAT:
|
||||
param_set_no_remote_update(_set_value_request.parameter_index,
|
||||
(const void *) &_set_value_request.float_value,
|
||||
false);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("Parameter must be either int or float");
|
||||
break;
|
||||
}
|
||||
|
||||
_set_value_response.timestamp = hrt_absolute_time();
|
||||
_set_value_response.request_timestamp = _set_value_request.timestamp;
|
||||
_set_value_response.parameter_index = _set_value_request.parameter_index;
|
||||
|
||||
if (_set_value_rsp_h == nullptr) {
|
||||
_set_value_rsp_h = orb_advertise(ORB_ID(parameter_remote_set_value_response), &_set_value_response);
|
||||
|
||||
} else {
|
||||
if (debug) {
|
||||
PX4_INFO("Sending set value response for %s", param_name(_set_value_request.parameter_index));
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(parameter_remote_set_value_response), _set_value_rsp_h, &_set_value_response);
|
||||
}
|
||||
|
||||
param_remote_counters.set_value_response_sent++;
|
||||
|
||||
(void) orb_check(_set_value_req_fd, &updated);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void param_remote_init()
|
||||
{
|
||||
|
||||
sync_thread_tid = px4_task_spawn_cmd(sync_thread_name,
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_PARAMS,
|
||||
(1024 * 4),
|
||||
remote_sync_thread,
|
||||
NULL);
|
||||
|
||||
}
|
||||
|
||||
void param_remote_set_used(param_t param)
|
||||
{
|
||||
// Notify the parameter server that this parameter has been marked as used
|
||||
if (debug) {
|
||||
PX4_INFO("Requesting server to mark %s as used", param_name(param));
|
||||
}
|
||||
|
||||
struct parameter_set_used_request_s req;
|
||||
|
||||
req.timestamp = hrt_absolute_time();
|
||||
|
||||
req.parameter_index = param;
|
||||
|
||||
if (parameter_set_used_h == nullptr) {
|
||||
parameter_set_used_h = orb_advertise(ORB_ID(parameter_set_used_request), &req);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(parameter_set_used_request), parameter_set_used_h, &req);
|
||||
}
|
||||
|
||||
param_remote_counters.set_used_sent++;
|
||||
}
|
||||
|
||||
void param_remote_set_value(param_t param, const void *val)
|
||||
{
|
||||
bool send_request = true;
|
||||
struct parameter_set_value_request_s req;
|
||||
req.timestamp = hrt_absolute_time();
|
||||
req.parameter_index = param;
|
||||
|
||||
switch (param_type(param)) {
|
||||
case PARAM_TYPE_INT32:
|
||||
req.int_value = *(int32_t *)val;
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("*** Setting %s to %" PRIi32 " ***", param_name(req.parameter_index), req.int_value);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_FLOAT:
|
||||
req.float_value = *(float *)val;
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("*** Setting %s to %f ***", param_name(req.parameter_index), (double) req.float_value);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("Parameter must be either int or float");
|
||||
send_request = false;
|
||||
break;
|
||||
}
|
||||
|
||||
if (param_set_rsp_fd == PX4_ERROR) {
|
||||
if (debug) {
|
||||
PX4_INFO("Subscribing to parameter_primary_set_value_response");
|
||||
}
|
||||
|
||||
param_set_rsp_fd = orb_subscribe(ORB_ID(parameter_primary_set_value_response));
|
||||
|
||||
if (param_set_rsp_fd == PX4_ERROR) {
|
||||
PX4_ERR("Subscription to parameter_primary_set_value_response failed");
|
||||
|
||||
} else {
|
||||
if (debug) {
|
||||
PX4_INFO("Subscription to parameter_primary_set_value_response succeeded");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (send_request) {
|
||||
if (debug) {
|
||||
PX4_INFO("Sending param set value request to primary for %s", param_name(req.parameter_index));
|
||||
}
|
||||
|
||||
if (param_set_value_req_h == nullptr) {
|
||||
param_set_value_req_h = orb_advertise(ORB_ID(parameter_primary_set_value_request), nullptr);
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(parameter_primary_set_value_request), param_set_value_req_h, &req);
|
||||
|
||||
param_remote_counters.set_value_request_sent++;
|
||||
|
||||
// Wait for response
|
||||
bool updated = false;
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("Waiting for parameter_client_set_value_response for %s", param_name(req.parameter_index));
|
||||
}
|
||||
|
||||
px4_usleep(TIMEOUT_WAIT);
|
||||
int count = TIMEOUT_COUNT;
|
||||
|
||||
while (--count) {
|
||||
(void) orb_check(param_set_rsp_fd, &updated);
|
||||
|
||||
struct parameter_set_value_response_s rsp;
|
||||
|
||||
while (updated) {
|
||||
|
||||
orb_copy(ORB_ID(parameter_primary_set_value_response), param_set_rsp_fd, &rsp);
|
||||
|
||||
if ((rsp.request_timestamp == req.timestamp) && (rsp.parameter_index == req.parameter_index)) {
|
||||
if (debug) {
|
||||
PX4_INFO("Got parameter_primary_set_value_response for %s", param_name(req.parameter_index));
|
||||
}
|
||||
|
||||
param_remote_counters.set_value_response_received++;
|
||||
return;
|
||||
}
|
||||
|
||||
(void) orb_check(param_set_rsp_fd, &updated);
|
||||
}
|
||||
|
||||
px4_usleep(TIMEOUT_WAIT);
|
||||
}
|
||||
|
||||
PX4_ERR("Timeout waiting for parameter_primary_set_value_response for %s", param_name(req.parameter_index));
|
||||
}
|
||||
}
|
||||
|
||||
void param_remote_get_counters(struct param_remote_counters *cnt)
|
||||
{
|
||||
*cnt = param_remote_counters;
|
||||
}
|
||||
Reference in New Issue
Block a user