/*update value and param's change bit in shared memory*/
void update_to_shmem(param_t param, union param_value_u value)
{
unsigned int byte_changed, bit_changed;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
shmem_info_p->params_val[param] = value;
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
PX4_DEBUG("set %d bit on krait changed index[%d] to %d", bit_changed, byte_changed,
shmem_info_p->krait_changed_index[byte_changed]);
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d",
value.i, param_name(param), byte_changed, bit_changed);
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d",
(double)value.f, param_name(param), byte_changed, bit_changed);
}
#endif
release_shmem_lock();
}
static void update_value_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
*value = shmem_info_p->params_val[param];
/*also clear the index since we are holding the lock*/
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
release_shmem_lock();
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO(
"Got value %d for param %s from shmem, cleared adsp index %d:%d",
value->i, param_name(param), byte_changed, bit_changed);
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO(
"Got value %f for param %s from shmem, cleared adsp index %d:%d",
(double)value->f, param_name(param), byte_changed, bit_changed);
}
#endif
}
void copy_params_to_shmem(struct param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_ERR("Could not get shmem lock");
return;
}
PX4_DEBUG("%d krait params allocated", param_count());
for (param = 0; param < param_count(); param++) {
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
} else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
{
PX4_INFO("%d: written %d for param %s to shared mem",
param, shmem_info_p->params_val[param].i, param_name(param));
}
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
{
PX4_INFO("%d: written %f for param %s to shared mem",
param, (double)shmem_info_p->params_val[param].f, param_name(param));
}
}
#endif
}
//PX4_DEBUG("written %u params to shmem offset %lu", param_count(), (unsigned char*)&shmem_info_p->params_count-(unsigned char*)shmem_info_p);
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->krait_changed_index[i] = 0;
adsp_changed_index[i] = 0;
}
release_shmem_lock();
}
/** Return pointer to shader symbol definition for argument specified by symbol name */
SLO_VISSYMDEF* Slo_GetArgByName ( char* name )
{
std::string param_name(name);
if (slovissymdef_data.find(std::string(name)) == slovissymdef_data.end())
{
SLX_VISSYMDEF* slxdef = SLX_GetArgByName(name);
param_name = slxdef->svd_name;
// convert SLX_VISSYMDEF to SLO_VISSYMDEF
SLO_VISSYMDEF slodef;
initParamStruct(&slodef);
if (SlxLastError == RIE_NOERROR)
{
convertVISSYMDEFStruct (slxdef, &slodef);
}
else
{
return NULL;
}
slovissymdef_data[param_name] = slodef;
// slovissymdef_namemap becomes out of sync here as we don't have an id
// for this param, Slo_GetArgById will catch this
}
return &slovissymdef_data[param_name];
}
/** Return pointer to shader symbol definition for argument specified by symbol ID */
SLO_VISSYMDEF* Slo_GetArgById ( int id )
{
// check to see if we have converted this yet
if (slovissymdef_namemap.find(id) == slovissymdef_namemap.end())
{
SLX_VISSYMDEF* slxdef = SLX_GetArgById(id - 1); // SLX_* is 0 based
std::string param_name(slxdef->svd_name);
// catch the case where the namemap might be out of sync because of
// other calls to Slo_GetArgByName which can't update the namemap
if (slovissymdef_data.find(param_name) != slovissymdef_data.end())
{
slovissymdef_namemap[id] = param_name;
return &slovissymdef_data[slovissymdef_namemap[id]];
}
// convert SLX_VISSYMDEF to SLO_VISSYMDEF
SLO_VISSYMDEF slodef;
initParamStruct(&slodef);
if (SlxLastError == RIE_NOERROR) {
convertVISSYMDEFStruct (slxdef, &slodef);
} else {
return NULL;
}
slovissymdef_data[param_name] = slodef;
slovissymdef_namemap[id] = param_name;
}
return &slovissymdef_data[slovissymdef_namemap[id]];
}
void
param_reset_excludes(const char *excludes[], int num_excludes)
{
param_lock();
param_t param;
for (param = 0; handle_in_range(param); param++) {
const char *name = param_name(param);
bool exclude = false;
for (int index = 0; index < num_excludes; index ++) {
int len = strlen(excludes[index]);
if ((excludes[index][len - 1] == '*'
&& strncmp(name, excludes[index], len - 1) == 0)
|| strcmp(name, excludes[index]) == 0) {
exclude = true;
break;
}
}
if (!exclude) {
param_reset(param);
}
}
param_unlock();
param_notify_changes();
}
int
param_get(param_t param, void *val)
{
int result = -1;
param_lock();
if (!handle_in_range(param)) {
return result;
}
union param_value_u value;
if (update_from_shmem(param, &value)) {
set_called_from_get = 1;
param_set_internal(param, &value, true, false, false);
set_called_from_get = 0;
}
const void *v = param_get_value_ptr(param);
if (val != NULL) {
memcpy(val, v, param_size(param));
result = 0;
}
#ifdef ENABLE_SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("param_get for %s : %d\n", param_name(param), ((union param_value_u *)val)->i);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("param_get 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
param_unlock();
return result;
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char*)arg;
/* print nothing if search string valid and not matching */
if (arg != NULL && (strcmp(search_string, param_name(param) != 0)))
return;
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("%d\n", i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
return;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
return;
}
printf("<error fetching parameter %d>\n", param);
}
bool
MavlinkParametersManager::send_untransmitted()
{
bool sent_one = false;
// Check for untransmitted system parameters
if (_mavlink_parameter_sub < 0) {
_mavlink_parameter_sub = orb_subscribe(ORB_ID(parameter_update));
}
bool param_ready;
orb_check(_mavlink_parameter_sub, ¶m_ready);
if (param_ready) {
// Clear the ready flag
struct parameter_update_s value;
orb_copy(ORB_ID(parameter_update), _mavlink_parameter_sub, &value);
// Schedule an update if not already the case
if (_param_update_time == 0) {
_param_update_time = value.timestamp;
_param_update_index = 0;
}
}
if ((_param_update_time != 0) && ((_param_update_time + 5 * 1000) < hrt_absolute_time())) {
param_t param = 0;
// send out all changed values
do {
// skip over all parameters which are not invalid and not used
do {
param = param_for_index(_param_update_index);
++_param_update_index;
} while (param != PARAM_INVALID && !param_used(param));
// send parameters which are untransmitted while there is
// space in the TX buffer
if ((param != PARAM_INVALID) && param_value_unsaved(param)) {
int ret = send_param(param);
char buf[100];
strncpy(&buf[0], param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
sent_one = true;
if (ret != PX4_OK) {
break;
}
}
} while ((_mavlink->get_free_tx_buf() >= get_size()) && (_param_update_index < (int) param_count()));
// Flag work as done once all params have been sent
if (_param_update_index >= (int) param_count()) {
_param_update_time = 0;
}
}
return sent_one;
}
static void
do_set(const char* name, const char* val)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
errx(1, "Error: Parameter %s not found.", name);
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("curr: %d", i);
/* convert string */
char* end;
i = strtol(val,&end,10);
param_set(param, &i);
printf(" -> new: %d\n", i);
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("curr: %4.4f", (double)f);
/* convert string */
char* end;
f = strtod(val,&end);
param_set(param, &f);
printf(" -> new: %4.4f\n", (double)f);
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
exit(0);
}
void
setPPstyle (ppOption pps, int i, const char *argv)
{
static int setBy = 0;
static const char *setArg = NULL;
if (argv == NULL) {
printf ("Set by arg %d (%-10s), PrettyPrint style = %s\n",
setBy, setArg, getPPStyleStr());
return;
}
// if the style has already been set, and is trying to be reset by default
// rules, override the default (i.e., don't make a change)
if (setBy != 0 && i == 0)
return;
// If -long or -xml or -format are specified, do not reset to
// "normal" style when followed by a flag such as -startd.
if( ppStyle == PP_XML || ppStyle == PP_VERBOSE || ppStyle == PP_CUSTOM )
{
if( pps != PP_XML && pps != PP_VERBOSE && pps != PP_CUSTOM ) {
// ignore this style setting and keep our existing setting
return;
}
}
// If setting a 'normal' output, check to see if there is a user-defined normal output
if ( ! disable_user_print_files && ! explicit_format
&& pps != PP_XML && pps != PP_VERBOSE && pps != PP_CUSTOM && pps != ppStyle) {
MyString param_name("STATUS_DEFAULT_"); param_name += getTypeStr(); param_name += "_PRINT_FORMAT_FILE";
char * pf_file = param(param_name.c_str());
if (pf_file) {
struct stat stat_buff;
if (0 != stat(pf_file, &stat_buff)) {
// do nothing, this is not an error.
} else if (set_status_print_mask_from_stream(pf_file, true) < 0) {
fprintf(stderr, "Warning: default %s select file '%s' is invalid\n", getTypeStr(), pf_file);
} else {
using_print_format = true;
}
free(pf_file);
}
}
if ( (PP_XML == pps) || PP_VERBOSE == pps || (ppStyle <= pps || setBy == 0) ) {
ppStyle = pps;
setBy = i;
setArg = argv;
} else {
fprintf (stderr, "Error: arg %d (%s) contradicts arg %d (%s)\n",
i, argv, setBy, setArg);
exit (1);
}
}
static void
at_i(void)
{
switch (at_cmd[3]) {
case '\0':
case '0':
printf("%s\n", g_banner_string);
return;
case '1':
printf("%s\n", g_version_string);
return;
case '2':
printf("%u\n", BOARD_ID);
break;
case '3':
printf("%u\n", g_board_frequency);
break;
case '4':
printf("%u\n", g_board_bl_version);
return;
case '5': {
register enum ParamID id;
register uint8_t start = 0;
register uint8_t end = PARAM_MAX-1;
if (at_cmd[4] == ':' && isdigit(at_cmd[5])) {
idx = 5;
at_parse_number();
start = at_num;
if (at_cmd[idx] == ':' && isdigit(at_cmd[idx+1])) {
idx++;
at_parse_number();
end = at_num;
}
}
// convenient way of showing all parameters
for (id = start; id <= end; id++) {
printf("S%u:%s=%lu\n",
(unsigned)id,
param_name(id),
(unsigned long)param_get(id));
}
return;
}
case '6':
tdm_report_timing();
return;
case '7':
tdm_show_rssi();
return;
default:
at_error();
return;
}
}
int Mavlink::mavlink_pm_send_param(param_t param)
{
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
float val_buf;
mavlink_message_t tx_msg;
/* query parameter type */
param_type_t type = param_type(param);
/* copy parameter name */
strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
uint8_t mavlink_type;
if (type == PARAM_TYPE_INT32) {
mavlink_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
mavlink_type = MAVLINK_TYPE_FLOAT;
} else {
mavlink_type = MAVLINK_TYPE_FLOAT;
}
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
int ret;
if ((ret = param_get(param, &val_buf)) != OK) {
return ret;
}
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
_channel,
&tx_msg,
name_buf,
val_buf,
mavlink_type,
param_count(),
param_get_index(param));
send_message(&tx_msg);
return OK;
}
static int
do_show_index(const char *index, bool used_index)
{
char *end;
int i = strtol(index, &end, 10);
param_t param;
int32_t ii;
float ff;
if (used_index) {
param = param_for_used_index(i);
} else {
param = param_for_index(i);
}
if (param == PARAM_INVALID) {
PX4_ERR("param not found for index %u", i);
return 1;
}
PARAM_PRINT("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '),
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param), param_get_used_index(param), param_get_index(param));
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &ii)) {
PARAM_PRINT("%ld\n", (long)ii);
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &ff)) {
PARAM_PRINT("%4.4f\n", (double)ff);
}
break;
default:
PARAM_PRINT("<unknown type %d>\n", 0 + param_type(param));
}
return 0;
}
int
MavlinkParametersManager::send_param(param_t param)
{
if (param == PARAM_INVALID) {
return 1;
}
mavlink_param_value_t msg;
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return 2;
}
msg.param_count = param_count_used();
msg.param_index = param_get_used_index(param);
/* copy parameter name */
strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* query parameter type */
param_type_t type = param_type(param);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
if (type == PARAM_TYPE_INT32) {
msg.param_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
msg.param_type = MAVLINK_TYPE_FLOAT;
} else {
msg.param_type = MAVLINK_TYPE_FLOAT;
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
return 0;
}
void MsgfplusReader::searchEngineSpecificParsing(
const ::mzIdentML_ns::SpectrumIdentificationItemType & item, const int itemCount) {
// First, check whether addFeatures was set to 1, in MS-GF+
if (!additionalMsgfFeatures) {
BOOST_FOREACH (const ::mzIdentML_ns::UserParamType & up, item.userParam()) {
if (up.value().present()) {
std::string param_name(up.name().c_str());
// Check whether the mzid-file seem to include the additional features
if (param_name == "ExplainedIonCurrentRatio") { // If one additional feature is found
additionalMsgfFeatures = true;
}
}
}
if (!additionalMsgfFeatures) { // If no additional features were found in first PSM
ostringstream temp;
temp << "Error: No features for learning were found in the mzid-file."
<< " Run MS-GF+ with the addFeatures option set to 1." << std::endl;
throw MyException(temp.str());
}
}
int write_parameters(int fd)
{
/* construct parameter message */
struct {
LOG_PACKET_HEADER;
struct log_PARM_s body;
} log_msg_PARM = {
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
};
int written = 0;
param_t params_cnt = param_count();
for (param_t param = 0; param < params_cnt; param++) {
/* fill parameter message and write it */
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
float value = NAN;
switch (param_type(param)) {
case PARAM_TYPE_INT32: {
int32_t i;
param_get(param, &i);
value = i; // cast integer to float
break;
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
default:
break;
}
log_msg_PARM.body.value = value;
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
}
return written;
}
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;
}
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);
}
static void
do_compare(const char* name, const char* vals[], unsigned comparisons)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
errx(1, "Error: Parameter %s not found.", name);
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
int ret = 1;
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char* end;
for (unsigned k = 0; k < comparisons; k++) {
int j = strtol(vals[k],&end,10);
if (i == j) {
printf(" %d: ", i);
ret = 0;
}
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char* end;
for (unsigned k = 0; k < comparisons; k++) {
float g = strtod(vals[k], &end);
if (fabsf(f - g) < 1e-7f) {
printf(" %4.4f: ", (double)f);
ret = 0;
}
}
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
if (ret == 0) {
printf("%c %s: match\n",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
}
exit(ret);
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char*)arg;
const char *p_name = (const char*)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL)) {
/* start search */
char *ss = search_string;
char *pp = p_name;
bool mismatch = false;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
if (*ss == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
warnx("* symbol only allowed at end of search string.");
exit(1);
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*'))
return;
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("%d\n", i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
return;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
return;
}
printf("<error fetching parameter %d>\n", param);
}
void copy_params_to_shmem(const param_info_s *param_info_base)
{
param_t param;
unsigned int i;
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return;
}
//else PX4_INFO("Got lock\n");
for (param = 0; param < param_count(); param++) {
//{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);}
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
}
else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param));
}
#endif
}
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->adsp_changed_index[i] = 0;
krait_changed_index[i] = 0;
}
release_shmem_lock(__FILE__, __LINE__);
//PX4_INFO("Released lock\n");
}
int
MavlinkParametersManager::send_param(param_t param, int component_id)
{
if (param == PARAM_INVALID) {
return 1;
}
/* no free TX buf to send this param */
if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
return 1;
}
mavlink_param_value_t msg;
/*
* get param value, since MAVLink encodes float and int params in the same
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return 2;
}
msg.param_count = param_count_used();
msg.param_index = param_get_used_index(param);
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic ignored "-Wstringop-truncation"
#endif
/*
* coverity[buffer_size_warning : FALSE]
*
* The MAVLink spec does not require the string to be NUL-terminated if it
* has length 16. In this case the receiving end needs to terminate it
* when copying it.
*/
strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic pop
#endif
/* query parameter type */
param_type_t type = param_type(param);
/*
* Map onboard parameter type to MAVLink type,
* endianess matches (both little endian)
*/
if (type == PARAM_TYPE_INT32) {
msg.param_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) {
msg.param_type = MAVLINK_TYPE_FLOAT;
} else {
msg.param_type = MAVLINK_TYPE_FLOAT;
}
/* default component ID */
if (component_id < 0) {
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
} else {
// Re-pack the message with a different component ID
mavlink_message_t mavlink_packet;
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink->get_channel(), &mavlink_packet, &msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
}
return 0;
}
int
param_export(int fd, bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
param_bus_lock(true);
bson_encoder_init_file(&encoder, fd);
param_bus_lock(false);
/* 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) {
int32_t i;
float f;
/*
* 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;
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32: {
param_get(s->param, &i);
const char *name = param_name(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_int(&encoder, name, i)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_FLOAT: {
param_get(s->param, &f);
const char *name = param_name(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_double(&encoder, name, f)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: {
const char *name = param_name(s->param);
const size_t size = param_size(s->param);
const void *value_ptr = param_get_value_ptr(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_binary(&encoder,
name,
BSON_BIN_BINARY,
size,
value_ptr)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
default:
debug("unrecognized parameter type");
goto out;
}
param_bus_lock(false);
/* allow this process to be interrupted by another process / thread */
usleep(5);
}
result = 0;
out:
param_unlock();
if (result == 0) {
result = bson_encoder_fini(&encoder);
}
return result;
}
static int
do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OPERATOR cmp_op)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found */
PX4_ERR("Parameter %s not found", name);
return 1;
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
int ret = 1;
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
for (unsigned k = 0; k < comparisons; k++) {
int j = strtol(vals[k], &end, 10);
if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (i == j)) ||
((cmp_op == COMPARE_OPERATOR_GREATER) && (i > j))) {
PX4_DEBUG(" %ld: ", (long)i);
ret = 0;
}
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
for (unsigned k = 0; k < comparisons; k++) {
float g = strtod(vals[k], &end);
if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) ||
((cmp_op == COMPARE_OPERATOR_GREATER) && (f > g))) {
PX4_DEBUG(" %4.4f: ", (double)f);
ret = 0;
}
}
}
break;
default:
PX4_ERR("<unknown / unsupported type %d>", 0 + param_type(param));
return 1;
}
if (ret == 0) {
PX4_DEBUG("%c %s: match\n",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
}
return ret;
}
static void
do_set(const char *name, const char *val, bool fail_on_not_found)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found - fail silenty in scripts as it prevents booting */
errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name);
}
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
int32_t newval = strtol(val, &end, 10);
if (i == newval) {
printf("unchanged\n");
} else {
printf("curr: %d", i);
param_set(param, &newval);
printf(" -> new: %d\n", newval);
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
float newval = strtod(val, &end);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (f == newval) {
#pragma GCC diagnostic pop
printf("unchanged\n");
} else {
printf("curr: %4.4f", (double)f);
param_set(param, &newval);
printf(" -> new: %4.4f\n", (double)newval);
}
}
break;
default:
errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
}
exit(0);
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
const char *search_string = (const char *)arg;
const char *p_name = (const char *)param_name(param);
/* print nothing if search string is invalid and not matching */
if (!(arg == NULL)) {
/* start search */
const char *ss = search_string;
const char *pp = p_name;
/* XXX this comparison is only ok for trailing wildcards */
while (*ss != '\0' && *pp != '\0') {
// case insensitive comparison (param_name is always upper case)
if (toupper(*ss) == *pp) {
ss++;
pp++;
} else if (*ss == '*') {
if (*(ss + 1) != '\0') {
PX4_WARN("* symbol only allowed at end of search string");
// FIXME - should exit
return;
}
pp++;
} else {
/* param not found */
return;
}
}
/* the search string must have been consumed */
if (!(*ss == '\0' || *ss == '*') || *pp != '\0') {
return;
}
}
PARAM_PRINT("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '),
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param), param_get_used_index(param), param_get_index(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
PARAM_PRINT("%ld\n", (long)i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
PARAM_PRINT("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
PARAM_PRINT("<struct type %d size %zu>\n", 0 + param_type(param), param_size(param));
return;
default:
PARAM_PRINT("<unknown type %d>\n", 0 + param_type(param));
return;
}
PARAM_PRINT("<error fetching parameter %lu>\n", (unsigned long)param);
}
static int
do_set(const char *name, const char *val, bool fail_on_not_found)
{
int32_t i;
float f;
param_t param = param_find(name);
/* set nothing if parameter cannot be found */
if (param == PARAM_INVALID) {
/* param not found - fail silenty in scripts as it prevents booting */
PX4_ERR("Parameter %s not found.", name);
return (fail_on_not_found) ? 1 : 0;
}
/*
* Set parameter if type is known and conversion from string to value turns out fine
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
/* convert string */
char *end;
int32_t newval = strtol(val, &end, 10);
if (i != newval) {
PARAM_PRINT("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
PARAM_PRINT("curr: %ld", (long)i);
param_set_no_autosave(param, &newval);
PARAM_PRINT(" -> new: %ld\n", (long)newval);
}
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
/* convert string */
char *end;
float newval = strtod(val, &end);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (f != newval) {
#pragma GCC diagnostic pop
PARAM_PRINT("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
PARAM_PRINT("curr: %4.4f", (double)f);
param_set_no_autosave(param, &newval);
PARAM_PRINT(" -> new: %4.4f\n", (double)newval);
}
}
break;
default:
PX4_ERR("<unknown / unsupported type %d>\n", 0 + param_type(param));
return 1;
}
int ret = param_save_default();
if (ret) {
PX4_ERR("Param save failed (%i)", ret);
return 1;
} else {
return 0;
}
}
int
param_export(int fd, bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
bson_encoder_init_file(&encoder, fd);
/* 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) {
int32_t i;
float f;
/*
* 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;
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32:
param_get(s->param, &i);
if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
case PARAM_TYPE_FLOAT:
param_get(s->param, &f);
if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
if (bson_encoder_append_binary(&encoder,
param_name(s->param),
BSON_BIN_BINARY,
param_size(s->param),
param_get_value_ptr(s->param))) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
default:
debug("unrecognized parameter type");
goto out;
}
}
result = 0;
out:
param_unlock();
if (result == 0) {
result = bson_encoder_fini(&encoder);
}
return result;
}
static int
param_export_internal(bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
/* Use realloc */
bson_encoder_init_buf(&encoder, NULL, 0);
/* 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) {
int32_t i;
float f;
/*
* 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;
/* append the appropriate BSON type object */
switch (param_type(s->param)) {
case PARAM_TYPE_INT32:
param_get(s->param, &i);
if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
case PARAM_TYPE_FLOAT:
param_get(s->param, &f);
if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
if (bson_encoder_append_binary(&encoder,
param_name(s->param),
BSON_BIN_BINARY,
param_size(s->param),
param_get_value_ptr_external(s->param))) {
debug("BSON append failed for '%s'", param_name(s->param));
goto out;
}
break;
default:
debug("unrecognized parameter type");
goto out;
}
}
result = 0;
out:
param_unlock();
if (result == 0) {
/* Finalize the bison encoding*/
bson_encoder_fini(&encoder);
/* Get requiered space */
size_t buf_size = bson_encoder_buf_size(&encoder);
/* Get a buffer from the flash driver with enough space */
uint8_t *buffer;
result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size);
if (result == OK) {
/* Check for a write that has no changes */
uint8_t *was_buffer;
size_t was_buf_size;
int was_result = parameter_flashfs_read(parameters_token, &was_buffer, &was_buf_size);
void *enc_buff = bson_encoder_buf_data(&encoder);
bool commit = was_result < OK || was_buf_size != buf_size || 0 != memcmp(was_buffer, enc_buff, was_buf_size);
if (commit) {
memcpy(buffer, enc_buff, buf_size);
result = parameter_flashfs_write(parameters_token, buffer, buf_size);
result = result == buf_size ? OK : -EFBIG;
}
free(enc_buff);
}
}
return result;
}
