int px4_task_kill(px4_task_t id, int sig)
{
int rv = 0;
pthread_t pid;
PX4_DEBUG("Called px4_task_kill %d", sig);
if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0)
pid = taskmap[id].pid;
else
return -EINVAL;
// If current thread then exit, otherwise cancel
rv = pthread_kill(pid, sig);
return rv;
}
int
VDev::remove_poll_waiter(px4_pollfd_struct_t *fds)
{
PX4_DEBUG("VDev::remove_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (fds == _pollset[i]) {
_pollset[i] = nullptr;
return PX4_OK;
}
}
PX4_WARN("poll: bad fd state");
return -EINVAL;
}
int
AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) {
if (recv_len > 0) {
// this is equivalent to the collect phase
Simulator *sim = Simulator::getInstance();
if (sim == NULL) {
PX4_ERR("Error BARO_SIM::transfer no simulator");
return -ENODEV;
}
PX4_DEBUG("BARO_SIM::transfer getting sample");
sim->getAirspeedSample(recv, recv_len);
} else {
// we don't need measure phase
}
return 0;
}
static void update_index_from_shmem(void)
{
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
PX4_DEBUG("Updating index from shmem");
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
adsp_changed_index[i] = shmem_info_p->adsp_changed_index[i];
}
release_shmem_lock();
}
int
MPU9250::select_register_bank(uint8_t bank)
{
uint8_t ret;
uint8_t buf;
uint8_t retries = 3;
if (_selected_bank != bank) {
ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);
if (ret != OK) {
return ret;
}
}
/*
* Making sure the right register bank is selected (even if it should be). Observed some
* unexpected changes to this, don't risk writing to the wrong register bank.
*/
_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
while (bank != buf && retries > 0) {
//PX4_WARN("user bank: expected %d got %d",bank,buf);
ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);
if (ret != OK) {
return ret;
}
retries--;
//PX4_WARN("BANK retries: %d", 4-retries);
_interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
}
_selected_bank = bank;
if (bank != buf) {
PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf);
return PX4_ERROR;
} else {
return PX4_OK;
}
}
VDev::VDev(const char *name,
const char *devname) :
// base class
Device(name),
// public
// protected
_pub_blocked(false),
// private
_devname(devname),
_registered(false),
_open_count(0)
{
PX4_DEBUG("VDev::VDev");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
_pollset[i] = nullptr;
}
}
int px4_task_kill(px4_task_t id, int sig)
{
int rv = 0;
pthread_t pid;
PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str());
if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) {
pid = taskmap[id].pid;
} else {
return -EINVAL;
}
// If current thread then exit, otherwise cancel
rv = pthread_kill(pid, sig);
return rv;
}
/**
* Print a little info about the driver.
*/
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
PX4_DEBUG("state @ %p", g_dev);
int ret = g_dev->print_info();
if (ret != 0) {
PX4_ERR("Unable to print info");
return ret;
}
return 0;
}
int
VDev::store_poll_waiter(px4_pollfd_struct_t *fds)
{
/*
* Look for a free slot.
*/
PX4_DEBUG("VDev::store_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (nullptr == _pollset[i]) {
/* save the pollfd */
_pollset[i] = fds;
return PX4_OK;
}
}
return -ENOMEM;
}
int
VDev::store_poll_waiter(px4_pollfd_struct_t *fds)
{
/*
* Look for a free slot.
*/
PX4_DEBUG("VDev::store_poll_waiter");
for (unsigned i = 0; i < _max_pollwaiters; i++) {
if (nullptr == _pollset[i]) {
/* save the pollfd */
_pollset[i] = fds;
return PX4_OK;
}
}
/* No free slot found. Resize the pollset */
if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t
return -ENOMEM;
}
const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1;
px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count];
if (!new_pollset) {
return -ENOMEM;
}
if (_max_pollwaiters > 0) {
memset(new_pollset + _max_pollwaiters, 0, sizeof(px4_pollfd_struct_t *) * (new_count - _max_pollwaiters));
memcpy(new_pollset, _pollset, sizeof(px4_pollfd_struct_t *) * _max_pollwaiters);
delete[](_pollset);
}
_pollset = new_pollset;
_pollset[_max_pollwaiters] = fds;
_max_pollwaiters = new_count;
return PX4_OK;
}
int
MPU9250::probe()
{
int ret = PX4_ERROR;
// Try first for mpu9250/6500
_whoami = read_reg(MPUREG_WHOAMI);
// If it's not an MPU it must be an ICM
if ((_whoami != MPU_WHOAMI_9250) && (_whoami != MPU_WHOAMI_6500)) {
// Make sure selected register bank is bank 0 (which contains WHOAMI)
select_register_bank(REG_BANK(ICMREG_20948_WHOAMI));
_whoami = read_reg(ICMREG_20948_WHOAMI);
}
if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
_num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS;
_checked_registers = _mpu9250_checked_registers;
memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS);
memset(_checked_bad, 0, MPU9250_NUM_CHECKED_REGISTERS);
ret = PX4_OK;
} else if (_whoami == ICM_WHOAMI_20948) {
_num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS;
_checked_registers = _icm20948_checked_registers;
memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS);
memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS);
ret = PX4_OK;
}
_checked_values[0] = _whoami;
_checked_bad[0] = _whoami;
if (ret != PX4_OK) {
PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
}
return ret;
}
int
SF0X::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
char cmd = SF0X_TAKE_RANGE_REG;
ret = ::write(_fd, &cmd, 1);
if (ret != sizeof(cmd)) {
perf_count(_comms_errors);
PX4_DEBUG("write fail %d", ret);
return ret;
}
ret = OK;
return ret;
}
VDev *VDev::getDev(const char *path)
{
PX4_DEBUG("VDev::getDev");
int i = 0;
pthread_mutex_lock(&devmutex);
for (; i < PX4_MAX_DEV; ++i) {
//if (devmap[i]) {
// printf("%s %s\n", devmap[i]->name, path);
//}
if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) {
pthread_mutex_unlock(&devmutex);
return (VDev *)(devmap[i]->cdev);
}
}
pthread_mutex_unlock(&devmutex);
return nullptr;
}
void px4_task_exit(int ret)
{
int i;
pthread_t pid = pthread_self();
// Get pthread ID from the opaque ID
for (i=0; i<PX4_MAX_TASKS; ++i) {
if (taskmap[i].pid == pid) {
taskmap[i].isused = false;
break;
}
}
if (i>=PX4_MAX_TASKS) {
PX4_ERR("px4_task_exit: self task not found!");
}
else {
PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str());
}
pthread_exit((void *)(unsigned long)ret);
}
/**
* worker callback method to save the parameters
* @param arg unused
*/
static void
autosave_worker(void *arg)
{
bool disabled = false;
param_lock_writer();
last_autosave_timestamp = hrt_absolute_time();
autosave_scheduled = false;
disabled = autosave_disabled;
param_unlock_writer();
if (disabled) {
return;
}
PX4_DEBUG("Autosaving params");
int ret = param_save_default();
if (ret != 0) {
PX4_ERR("param save failed (%i)", ret);
}
}
static void *map_memory(off_t target)
{
if ((mem_fd = open(MEMDEVICE, O_RDWR | O_SYNC)) == -1) {
PX4_ERR("Cannot open %s", MEMDEVICE);
exit(1);
}
/* Map one page */
map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE,
MAP_SHARED, mem_fd, target & ~MAP_MASK);
if (map_base == (void *) - 1) {
PX4_ERR("Cannot mmap /dev/atl_mem");
exit(1);
}
PX4_DEBUG("Initializing map memory: mem_fd: %d, 0x%X", mem_fd, map_base + (target & MAP_MASK) + LOCK_SIZE);
return (map_base + (target & MAP_MASK) + LOCK_SIZE);
}
int px4_task_delete(px4_task_t id)
{
int rv = 0;
pthread_t pid;
PX4_DEBUG("Called px4_task_delete");
if (id < PX4_MAX_TASKS && taskmap[id].isused)
pid = taskmap[id].pid;
else
return -EINVAL;
// If current thread then exit, otherwise cancel
if (pthread_self() == pid) {
taskmap[id].isused = false;
pthread_exit(0);
} else {
rv = pthread_cancel(pid);
}
taskmap[id].isused = false;
return rv;
}
int16_t uORB::Manager::process_received_message(const char *messageName, int32_t length, uint8_t *data)
{
int16_t rc = -1;
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, messageName);
DeviceMaster *device_master = get_device_master();
if (ret == OK && device_master) {
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
// get the node name.
if (node == nullptr) {
PX4_DEBUG("No existing subscriber found for message: [%s] nodepath:[%s]", messageName, nodepath);
} else {
// node is present.
node->process_received_message(length, data);
rc = 0;
}
}
return rc;
}
/*
* Default implementations of the character device interface
*/
int
VDev::open(file_t *filep)
{
PX4_DEBUG("VDev::open");
int ret = PX4_OK;
lock();
/* increment the open count */
_open_count++;
if (_open_count == 1) {
/* first-open callback may decline the open */
ret = open_first(filep);
if (ret != PX4_OK)
_open_count--;
}
unlock();
return ret;
}
int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status)
{
int16_t rc = 0;
if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) {
*status = 1;
//PX4_DEBUG("******* Found subscriber for message[%s]....", messageName);
} else {
*status = 0;
//PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size());
int i = 0;
for (std::list<std::string>::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) {
if (*it == messageName) {
PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i);
}
++i;
}
}
return rc;
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
hrt_lock();
//PX4_INFO("hrt_call_internal after lock");
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0) {
sq_rem(&entry->link, &callout_queue);
}
#if 1
// Use this to debug busy CPU that keeps rescheduling with 0 period time
/*if (interval < HRT_INTERVAL_MIN) {*/
/*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/
/*PX4_BACKTRACE();*/
/*}*/
#endif
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
hrt_unlock();
}
int
VDev::register_class_devname(const char *class_devname)
{
PX4_DEBUG("VDev::register_class_devname %s", class_devname);
if (class_devname == nullptr) {
return -EINVAL;
}
int class_instance = 0;
int ret = -ENOSPC;
while (class_instance < 4) {
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, (void *)this);
if (ret == OK) break;
class_instance++;
}
if (class_instance == 4) {
return ret;
}
return class_instance;
}
int get_shmem_lock(const char *caller_file_name, int caller_line_number)
{
unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
unsigned int i = 0;
while (!atomic_compare_and_set(lock, 1, 0)) {
PX4_INFO("Could not get lock, file name: %s, line number: %d.\n",
caller_file_name, caller_line_number);
i++;
usleep(1000);
if (i > 100) { break; }
}
if (i > 100) {
return -1;
} else {
PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", caller_file_name, caller_line_number);
}
return 0; //got the lock
}
int
VDev::close(file_t *filep)
{
PX4_DEBUG("VDev::close");
int ret = PX4_OK;
lock();
if (_open_count > 0) {
/* decrement the open count */
_open_count--;
/* callback cannot decline the close */
if (_open_count == 0)
ret = close_last(filep);
} else {
ret = -EBADF;
}
unlock();
return ret;
}
int
VDev::init()
{
PX4_DEBUG("VDev::init");
// base class init first
int ret = Device::init();
if (ret != PX4_OK)
goto out;
// now register the driver
if (_devname != nullptr) {
ret = register_driver(_devname, (void *)this);
if (ret != PX4_OK)
goto out;
_registered = true;
}
out:
return ret;
}
int16_t uORB::FastRpcChannel::get_bulk_data
(
uint8_t *buffer,
int32_t max_buffer_in_bytes,
int32_t *returned_bytes,
int32_t *topic_count
)
{
int16_t rc = 0;
// wait for data availability
static hrt_abstime check_time = 0;
hrt_abstime t1 = hrt_absolute_time();
_DataAvailableSemaphore.wait();
hrt_abstime t2 = hrt_absolute_time();
_QueueMutex.lock();
int32_t bytes_copied = 0;
int32_t copy_result = 0;
*returned_bytes = 0;
*topic_count = 0;
int32_t topic_count_to_return = 0;
if (DataQSize() != 0) {
//PX4_DEBUG( "get_bulk_data: QSize: %d", DataQSize() );
topic_count_to_return = DataQSize();
while (DataQSize() != 0) {
// this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait.
//_DataAvailableSemaphore.wait();
if (get_data_msg_size_at(_DataQOutIndex) < (max_buffer_in_bytes - bytes_copied)) {
// there is enough space in the buffer, copy the data.
//PX4_DEBUG( "Coping Data to buffer..." );
copy_result = copy_data_to_buffer(_DataQOutIndex, buffer, bytes_copied, max_buffer_in_bytes);
if (copy_result == -1) {
if (bytes_copied == 0) {
rc = -1;
}
break;
} else {
//PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\
// buffer[bytes_copied], \
// buffer[bytes_copied+1], \
// buffer[bytes_copied+2], \
// buffer[bytes_copied+3] );
bytes_copied += copy_result;
(*topic_count)++;
*returned_bytes = bytes_copied;
_DataQOutIndex++;
if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_DataQOutIndex = 0;
}
}
} else {
if (bytes_copied == 0) {
rc = -1;
PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned");
} else {
PX4_DEBUG("Exiting out of the while loop...");
}
break;
}
}
} else {
PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues");
rc = -1;
}
if (topic_count_to_return != *topic_count) {
PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count);
}
_QueueMutex.unlock();
hrt_abstime t3 = hrt_absolute_time();
if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); }
if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); }
if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); }
if ((unsigned long)(t3 - check_time) > 10000000) {
//PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3);
//PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize());
//PX4_DEBUG("ADSP RPC Stats: _get_bulk_min: %lu _get_bulk_max: %lu _dropped_pkts: %lu", _get_bulk_min, _get_bulk_max,
// _dropped_pkts);
//PX4_DEBUG(" .... topic_count_min: %lu topic_count_max: %lu", _bulk_topic_count_min, _bulk_topic_count_max);
_get_bulk_max = 0;
_get_bulk_min = 0xFFFFFF;
_bulk_topic_count_min = 0xFFFFFF;
_bulk_topic_count_max = 0;
check_time = t3;
}
//PX4_DEBUG( "Returning topics: %d bytes_returned: %d", *topic_count, *returned_bytes );
return rc;
}
int16_t uORB::FastRpcChannel::get_data
(
int32_t *msg_type,
char *topic_name,
int32_t topic_name_len,
uint8_t *data,
int32_t data_len_in_bytes,
int32_t *bytes_returned
)
{
int16_t rc = 0;
PX4_DEBUG("Get data should not be called...");
return -1;
// wait for data availability
static hrt_abstime check_time = 0;
hrt_abstime t1 = hrt_absolute_time();
_DataAvailableSemaphore.wait();
hrt_abstime t2 = hrt_absolute_time();
_QueueMutex.lock();
if (DataQSize() != 0 || ControlQSize() != 0) {
if (ControlQSize() > 0) {
// read the first element of the Control Queue.
*msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type;
if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) {
memcpy
(
topic_name,
_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(),
_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()
);
topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0;
*bytes_returned = 0;
_ControlQOutIndex++;
if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_ControlQOutIndex = 0;
}
} else {
PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]",
topic_name_len,
_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()
);
rc = -1;
}
} else {
// read the first element of the Control Queue.
*msg_type = _DATA_MSG_TYPE;
if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) ||
(_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) {
memcpy
(
topic_name,
_DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(),
_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()
);
topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0;
*bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length;
memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length);
_DataQOutIndex++;
if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_DataQOutIndex = 0;
}
} else {
PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ",
topic_name_len,
_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()
);
PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ",
data_len_in_bytes,
_DataMsgQueue[ _DataQOutIndex ]._Length
);
rc = -1;
}
}
} else {
PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues");
rc = -1;
}
_QueueMutex.unlock();
hrt_abstime t3 = hrt_absolute_time();
if ((unsigned long)(t3 - t1) > _get_max) { _get_max = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - t1) < _get_min) { _get_min = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - check_time) > 1000000) {
if (rc != 0) {
topic_name[0] = '\0';
}
/*
PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2,
(unsigned long)t3);
PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize());
PX4_DEBUG("ADSP RPC Stats: _get_min: %lu _get_max: %lu _dropped_pkts: %lu", _get_min, _get_max, _dropped_pkts);
*/
check_time = t3;
}
return rc;
}
int16_t uORB::FastRpcChannel::unblock_get_data_method()
{
PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()");
_DataAvailableSemaphore.post();
return 0;
}
void init(int argc, char *argv[], const char *app_name)
{
PX4_DEBUG("App name: %s\n", app_name);
}
static int
param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved)
{
int result = -1;
bool params_changed = false;
PX4_DEBUG("param_set_internal params: param = %d, val = 0x%X, mark_saved: %d, notify_changes: %d",
param, val, (int)mark_saved, (int)notify_changes);
param_lock();
if (!handle_in_range(param)) {
return result;
}
mark_saved = true; //mark all params as saved
if (param_values == NULL) {
utarray_new(param_values, ¶m_icd);
}
if (param_values == NULL) {
debug("failed to allocate modified values array");
goto out;
}
if (handle_in_range(param)) {
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
/* construct a new parameter */
struct param_wbuf_s buf = {
.param = param,
.val.p = NULL,
.unsaved = false
};
/* add it to the array and sort */
utarray_push_back(param_values, &buf);
utarray_sort(param_values, param_compare_values);
/* find it after sorting */
s = param_find_changed(param);
}
/* update the changed value */
switch (param_type(param)) {
case PARAM_TYPE_INT32:
s->val.i = *(int32_t *)val;
break;
case PARAM_TYPE_FLOAT:
s->val.f = *(float *)val;
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
if (s->val.p == NULL) {
s->val.p = malloc(param_size(param));
if (s->val.p == NULL) {
debug("failed to allocate parameter storage");
goto out;
}
}
memcpy(s->val.p, val, param_size(param));
break;
default:
goto out;
}
s->unsaved = !mark_saved;
params_changed = true;
result = 0;
}
out:
param_unlock();
/*
* If we set something, now that we have unlocked, go ahead and advertise that
* a thing has been set.
*/
if (!param_import_done) { notify_changes = 0; }
if (params_changed && notify_changes) {
param_notify_changes(is_saved);
}
if (result == 0 && !set_called_from_get) {
update_to_shmem(param, *(union param_value_u *)val);
}
#ifdef ENABLE_SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("param_set for %s : %d\n", param_name(param), ((union param_value_u *)val)->i);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("param_set for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f);
}
else {
PX4_INFO("Unknown param type for %s\n", param_name(param));
}
#endif
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false, true, false);
}
int
param_set_no_autosave(param_t param, const void *val)
{
return param_set_internal(param, val, false, true, true);
}
int
param_set_no_notification(param_t param, const void *val)
{
return param_set_internal(param, val, false, false, false);
}
bool
param_used(param_t param)
{
// TODO FIXME: for now all params are used
return true;
int param_index = param_get_index(param);
if (param_index < 0) {
return false;
}
return param_changed_storage[param_index / bits_per_allocation_unit] &
(1 << param_index % bits_per_allocation_unit);
}
void param_set_used_internal(param_t param)
{
int param_index = param_get_index(param);
if (param_index < 0) {
return;
}
param_changed_storage[param_index / bits_per_allocation_unit] |=
(1 << param_index % bits_per_allocation_unit);
}
