/** unlock the parameter store */
static void
param_unlock_reader(void)
{
do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0);
--reader_lock_holders;
if (reader_lock_holders == 0) {
// the last reader releases the lock
px4_sem_post(¶m_sem);
}
px4_sem_post(&reader_lock_holders_lock);
}
static void
stop(void)
{
/* Tell the worker task to shut down */
g_task_should_exit = true;
px4_sem_post(&g_work_queued_sema);
}
int UavcanNode::start_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
rv = UavcanServers::start(_node);
if (rv >= 0) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().installRxFrameListener(driver.get());
* would require a dynamic cast and rtti is not enabled.
*/
UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
}
}
_fw_server_action = None;
px4_sem_post(&_server_command_sem);
return rv;
}
static int
enqueue_work_item_and_wait_for_result(work_q_item_t *item)
{
/* put the work item at the end of the work queue */
lock_queue(&g_work_q);
sq_addlast(&item->link, &(g_work_q.q));
/* Adjust the queue size and potentially the maximum queue size */
if (++g_work_q.size > g_work_q.max_size) {
g_work_q.max_size = g_work_q.size;
}
unlock_queue(&g_work_q);
/* tell the work thread that work is available */
px4_sem_post(&g_work_queued_sema);
/* wait for the result */
px4_sem_wait(&item->wait_sem);
int result = item->result;
destroy_work_item(item);
return result;
}
void
PX4IO_serial_f7::_do_rx_dma_callback(unsigned status)
{
/* on completion of a reply, wake the waiter */
if (_rx_dma_status == _dma_status_waiting) {
/* check for packet overrun - this will occur after DMA completes */
uint32_t sr = rISR;
if (sr & (USART_ISR_ORE | USART_ISR_RXNE)) {
(void)rRDR;
rICR = sr & (USART_ISR_ORE | USART_ISR_RXNE);
status = DMA_STATUS_TEIF;
}
/* save RX status */
_rx_dma_status = status;
/* disable UART DMA */
rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
/* complete now */
px4_sem_post(&_completion_semaphore);
}
}
void
CDev::poll_notify_one(struct pollfd *fds, pollevent_t events)
{
/* update the reported event set */
fds->revents |= fds->events & events;
/* if the state is now interesting, wake the waiter if it's still asleep */
/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
if ((fds->revents != 0) && (fds->sem->semcount <= 0)) {
px4_sem_post(fds->sem);
}
}
int
VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
{
PX4_DEBUG("VDev::Poll %s", setup ? "setup" : "teardown");
int ret = PX4_OK;
/*
* Lock against pollnotify() (and possibly other callers)
*/
lock();
if (setup) {
/*
* Save the file pointer in the pollfd for the subclass'
* benefit.
*/
fds->priv = (void *)filep;
PX4_DEBUG("VDev::poll: fds->priv = %p", filep);
/*
* Handle setup requests.
*/
ret = store_poll_waiter(fds);
if (ret == PX4_OK) {
/*
* Check to see whether we should send a poll notification
* immediately.
*/
fds->revents |= fds->events & poll_state(filep);
/* yes? post the notification */
if (fds->revents != 0) {
px4_sem_post(fds->sem);
}
} else {
PX4_WARN("Store Poll Waiter error.");
}
} else {
/*
* Handle a teardown request.
*/
ret = remove_poll_waiter(fds);
}
unlock();
return ret;
}
/** lock the parameter store for read access */
static void
param_lock_reader(void)
{
do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0);
++reader_lock_holders;
if (reader_lock_holders == 1) {
// the first reader takes the lock, the next ones are allowed to just continue
do {} while (px4_sem_wait(¶m_sem) != 0);
}
px4_sem_post(&reader_lock_holders_lock);
}
int
UavcanNode::teardown()
{
px4_sem_post(&_server_command_sem);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
return (_armed_sub >= 0) ? ::close(_armed_sub) : 0;
}
__EXPORT void
dm_unlock(dm_item_t item)
{
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit) {
return;
}
if (item >= DM_KEY_NUM_KEYS) {
return;
}
if (g_item_locks[item]) {
px4_sem_post(g_item_locks[item]);
}
}
int UavcanNode::request_fw_check()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
_servers->requestCheckAllNodesFirmwareAndUpdate();
rv = 0;
}
_fw_server_action = None;
px4_sem_post(&_server_command_sem);
return rv;
}
int
CDev::poll(file_t *filp, struct pollfd *fds, bool setup)
{
int ret = OK;
/*
* Lock against pollnotify() (and possibly other callers)
*/
lock();
if (setup) {
/*
* Save the file pointer in the pollfd for the subclass'
* benefit.
*/
fds->priv = (void *)filp;
/*
* Handle setup requests.
*/
ret = store_poll_waiter(fds);
if (ret == OK) {
/*
* Check to see whether we should send a poll notification
* immediately.
*/
fds->revents |= fds->events & poll_state(filp);
/* yes? post the notification */
if (fds->revents != 0) {
px4_sem_post(fds->sem);
}
}
} else {
/*
* Handle a teardown request.
*/
ret = remove_poll_waiter(fds);
}
unlock();
return ret;
}
void
VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
{
PX4_DEBUG("VDev::poll_notify_one");
int value;
px4_sem_getvalue(fds->sem, &value);
/* update the reported event set */
fds->revents |= fds->events & events;
PX4_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value);
/* if the state is now interesting, wake the waiter if it's still asleep */
/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
if ((fds->revents != 0) && (value <= 0)) {
px4_sem_post(fds->sem);
}
}
int UavcanNode::stop_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
_tx_injector = nullptr;
rv = _servers->stop();
}
_fw_server_action = None;
px4_sem_post(&_server_command_sem);
return rv;
}
static int
task_main(int argc, char *argv[])
{
char buffer[DM_MAX_DATA_SIZE];
hrt_abstime wstart, wend, rstart, rend;
warnx("Starting dataman test task %s", argv[1]);
/* try to read an invalid item */
int my_id = atoi(argv[1]);
/* try to read an invalid item */
if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
warnx("%d read an invalid item failed", my_id);
goto fail;
}
/* try to read an invalid index */
if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
warnx("%d read an invalid index failed", my_id);
goto fail;
}
srand(hrt_absolute_time() ^ my_id);
unsigned hit = 0, miss = 0;
wstart = hrt_absolute_time();
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
memset(buffer, my_id, sizeof(buffer));
buffer[1] = i;
unsigned hash = i ^ my_id;
unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
warnx("ret: %d", ret);
if (ret != len) {
warnx("%d write failed, index %d, length %d", my_id, hash, len);
goto fail;
}
usleep(rand() & ((64 * 1024) - 1));
}
rstart = hrt_absolute_time();
wend = rstart;
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
unsigned hash = i ^ my_id;
unsigned len2;
unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
;
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
warnx("%d read failed length test, index %d", my_id, hash);
goto fail;
}
if (buffer[0] == my_id) {
hit++;
if (len2 != len) {
warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
goto fail;
}
if (buffer[1] != i) {
warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
goto fail;
}
} else {
miss++;
}
}
rend = hrt_absolute_time();
warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
px4_sem_post(sems + my_id);
return 0;
fail:
warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x",
my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
px4_sem_post(sems + my_id);
return -1;
}
static int
task_main(int argc, char *argv[])
{
char buffer[DM_MAX_DATA_SIZE];
PX4_INFO("Starting dataman test task %s", argv[1]);
/* try to read an invalid item */
int my_id = atoi(argv[1]);
/* try to read an invalid item */
if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) {
PX4_ERR("%d read an invalid item failed", my_id);
goto fail;
}
/* try to read an invalid index */
if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
PX4_ERR("%d read an invalid index failed", my_id);
goto fail;
}
srand(hrt_absolute_time() ^ my_id);
unsigned hit = 0, miss = 0;
hrt_abstime wstart = hrt_absolute_time();
for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) {
memset(buffer, my_id, sizeof(buffer));
buffer[1] = i;
unsigned hash = i ^ my_id;
unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
//PX4_INFO("ret: %d", ret);
if (ret != len) {
PX4_WARN("task %d: write failed, index %d, length %d", my_id, hash, len);
goto fail;
}
if (i % (NUM_MISSIONS_TEST / 10) == 0) {
PX4_INFO("task %d: %.0f%%", my_id, (double)i * 100.0f / NUM_MISSIONS_TEST);
}
usleep(rand() & ((64 * 1024) - 1));
}
hrt_abstime rstart = hrt_absolute_time();
hrt_abstime wend = rstart;
for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) {
unsigned hash = i ^ my_id;
unsigned len2;
unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2;
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
PX4_WARN("task %d: read failed length test, index %d", my_id, hash);
goto fail;
}
if (buffer[0] == my_id) {
hit++;
if (len2 != len) {
PX4_WARN("task %d: read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2);
goto fail;
}
if (buffer[1] != i) {
PX4_WARN("task %d: data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]);
goto fail;
}
} else {
miss++;
}
}
hrt_abstime rend = hrt_absolute_time();
PX4_INFO("task %d pass, hit %d, miss %d, io time read %llums. write %llums.",
my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_TEST / 1000, (wend - wstart) / NUM_MISSIONS_TEST / 1000);
px4_sem_post(sems + my_id);
return 0;
fail:
PX4_ERR("test_dataman FAILED: task %d, buffer %02x %02x %02x %02x %02x %02x",
my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
px4_sem_post(sems + my_id);
task_returned_error[my_id] = true;
return -1;
}
static inline void
unlock_queue(work_q_t *q)
{
px4_sem_post(&(q->mutex)); /* Release the queue lock */
}
void
Syslink::handle_message(syslink_message_t *msg)
{
hrt_abstime t = hrt_absolute_time();
if (t - _lasttime > 1000000) {
pktrate = _count;
rxrate = _count_in;
txrate = _count_out;
nullrate = _null_count;
_lasttime = t;
_count = 0;
_null_count = 0;
_count_in = 0;
_count_out = 0;
}
_count++;
if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
// When the power button is hit
} else if (msg->type == SYSLINK_PM_BATTERY_STATE) {
if (msg->length != 9) {
return;
}
uint8_t flags = msg->data[0];
int charging = flags & 1;
int powered = flags & 2;
float vbat; //, iset;
memcpy(&vbat, &msg->data[1], sizeof(float));
//memcpy(&iset, &msg->data[5], sizeof(float));
_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status);
// Update battery charge state
if (charging) {
_bstate = BAT_CHARGING;
}
/* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */
else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) {
_bstate = BAT_CHARGED;
} else {
_bstate = BAT_DISCHARGING;
}
// announce the battery status if needed, just publish else
if (_battery_pub != nullptr) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
} else if (msg->type == SYSLINK_RADIO_RSSI) {
uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
_rssi = 140 - rssi * 100 / (100 - 40);
} else if (msg->type == SYSLINK_RADIO_RAW) {
handle_raw(msg);
_lastrxtime = t;
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
handle_radio(msg);
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t));
px4_sem_post(&memory_sem);
} else {
PX4_INFO("GOT %d", msg->type);
}
//Send queued messages
if (!_queue.empty()) {
_queue.get(msg, sizeof(syslink_message_t));
send_message(msg);
}
float p = (t % 500000) / 500000.0f;
/* Use LED_GREEN for charging indicator */
if (_bstate == BAT_CHARGED) {
led_on(LED_GREEN);
} else if (_bstate == BAT_CHARGING && p < 0.25f) {
led_on(LED_GREEN);
} else {
led_off(LED_GREEN);
}
/* Alternate RX/TX LEDS when transfering */
bool rx = t - _lastrxtime < 200000,
tx = t - _lasttxtime < 200000;
if (rx && p < 0.25f) {
led_on(LED_RX);
} else {
led_off(LED_RX);
}
if (tx && p > 0.5f && p > 0.75f) {
led_on(LED_TX);
} else {
led_off(LED_TX);
}
// resend parameters if they haven't been acknowledged
if (_params_ack[0] == 0 && t - _params_update[0] > 10000) {
set_channel(_channel);
} else if (_params_ack[1] == 0 && t - _params_update[1] > 10000) {
set_datarate(_rate);
} else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) {
set_address(_addr);
}
}
static int
task_main(int argc, char *argv[])
{
work_q_item_t *work;
/* Initialize global variables */
g_key_offsets[0] = 0;
for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) {
g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
}
unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
for (unsigned i = 0; i < dm_number_of_funcs; i++) {
g_func_counts[i] = 0;
}
/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) {
g_item_locks[i] = NULL;
}
g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
g_task_should_exit = false;
init_q(&g_work_q);
init_q(&g_free_q);
px4_sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
#ifndef __PX4_POSIX
// XXX on Mac OS and Linux the file is not a multiple of the sector sizes
// this might need further inspection.
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
PX4_WARN("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
PX4_WARN("Size: %u, sector size: %d", file_size, k_sector_size);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
#else
close(g_task_fd);
#endif
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
if (g_task_fd < 0) {
PX4_WARN("Could not open data manager file %s", k_data_manager_device_path);
px4_sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path);
px4_sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
fsync(g_task_fd);
/* see if we need to erase any items based on restart type */
int sys_restart_val;
const char *restart_type_str = "Unkown restart";
if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
restart_type_str = "Power on restart";
_restart(DM_INIT_REASON_POWER_ON);
} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
restart_type_str = "In flight restart";
_restart(DM_INIT_REASON_IN_FLIGHT);
}
}
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
/* worker thread is shutting down but still processing requests */
g_fd = g_task_fd;
PX4_INFO("%s, data manager file '%s' size is %d bytes",
restart_type_str, k_data_manager_device_path, max_offset);
/* Tell startup that the worker thread has completed its initialization */
px4_sem_post(&g_init_sema);
/* Start the endless loop, waiting for then processing work requests */
while (true) {
/* do we need to exit ??? */
if ((g_task_should_exit) && (g_fd >= 0)) {
/* Close the file handle to stop further queuing */
g_fd = -1;
}
if (!g_task_should_exit) {
/* wait for work */
px4_sem_wait(&g_work_queued_sema);
}
/* Empty the work queue */
while ((work = dequeue_work_item())) {
/* handle each work item with the appropriate handler */
switch (work->func) {
case dm_write_func:
g_func_counts[dm_write_func]++;
work->result =
_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf,
work->write_params.count);
break;
case dm_read_func:
g_func_counts[dm_read_func]++;
work->result =
_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
break;
case dm_clear_func:
g_func_counts[dm_clear_func]++;
work->result = _clear(work->clear_params.item);
break;
case dm_restart_func:
g_func_counts[dm_restart_func]++;
work->result = _restart(work->restart_params.reason);
break;
default: /* should never happen */
work->result = -1;
break;
}
/* Inform the caller that work is done */
px4_sem_post(&work->wait_sem);
}
/* time to go???? */
if ((g_task_should_exit) && (g_fd < 0)) {
break;
}
}
close(g_task_fd);
g_task_fd = -1;
/* The work queue is now empty, empty the free queue */
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) {
break;
}
if (work->first) {
free(work);
}
}
destroy_q(&g_work_q);
destroy_q(&g_free_q);
px4_sem_destroy(&g_work_queued_sema);
px4_sem_destroy(&g_sys_state_mutex);
return 0;
}
/** unlock the parameter store */
static void
param_unlock_writer(void)
{
px4_sem_post(¶m_sem);
}
static void hrt_unlock(void)
{
px4_sem_post(&_hrt_lock);
}
void unlock() { px4_sem_post(&_lock); }
int
param_export(int fd, bool only_unsaved)
{
perf_begin(param_export_perf);
struct param_wbuf_s *s = NULL;
int result = -1;
struct bson_encoder_s encoder;
int shutdown_lock_ret = px4_shutdown_lock();
if (shutdown_lock_ret) {
PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret);
}
// take the file lock
do {} while (px4_sem_wait(¶m_sem_save) != 0);
param_lock_reader();
uint8_t bson_buffer[256];
bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer));
/* no modified parameters -> we are done */
if (param_values == NULL) {
result = 0;
goto out;
}
while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
/*
* If we are only saving values changed since last save, and this
* one hasn't, then skip it
*/
if (only_unsaved && !s->unsaved) {
continue;
}
s->unsaved = false;
const char *name = param_name(s->param);
const size_t size = param_size(s->param);
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32: {
const int32_t i = s->val.i;
debug("exporting: %s (%d) size: %d val: %d", name, s->param, size, i);
if (bson_encoder_append_int(&encoder, name, i)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_FLOAT: {
const float f = s->val.f;
debug("exporting: %s (%d) size: %d val: %.3f", name, s->param, size, (double)f);
if (bson_encoder_append_double(&encoder, name, f)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: {
const void *value_ptr = param_get_value_ptr(s->param);
/* lock as short as possible */
if (bson_encoder_append_binary(&encoder,
name,
BSON_BIN_BINARY,
size,
value_ptr)) {
PX4_ERR("BSON append failed for '%s'", name);
goto out;
}
}
break;
default:
PX4_ERR("unrecognized parameter type");
goto out;
}
}
result = 0;
out:
if (result == 0) {
if (bson_encoder_fini(&encoder) != PX4_OK) {
PX4_ERR("bson encoder finish failed");
}
}
param_unlock_reader();
px4_sem_post(¶m_sem_save);
if (shutdown_lock_ret == 0) {
px4_shutdown_unlock();
}
perf_end(param_export_perf);
return result;
}