param_t param_for_used_index(unsigned index) { #if 0 int count = get_param_info_count(); if (count && index < count) { /* walk all params and count used params */ unsigned used_count = 0; for (unsigned i = 0; i < (unsigned)size_param_changed_storage_bytes; i++) { for (unsigned j = 0; j < bits_per_allocation_unit; j++) { if (param_changed_storage[i] & (1 << j)) { /* we found the right used count, * return the param value */ if (index == used_count) { return (param_t)(i * bits_per_allocation_unit + j); } used_count++; } } } } return PARAM_INVALID; #else return param_for_index(index); #endif }
void MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested */ if (_send_all_index >= 0) { /* skip if no space is available */ if (_mavlink->get_free_tx_buf() < get_size()) { return; } /* look for the first parameter which is used */ param_t p; do { /* walk through all parameters, including unused ones */ p = param_for_index(_send_all_index); _send_all_index++; } while (p != PARAM_INVALID && !param_used(p)); if (p != PARAM_INVALID) { send_param(p); } if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } } }
void MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested, but only after the system has booted */ if (_send_all_index >= 0 && _mavlink->boot_complete()) { /* skip if no space is available */ if (_mavlink->get_free_tx_buf() < get_size()) { return; } /* look for the first parameter which is used */ param_t p; do { /* walk through all parameters, including unused ones */ p = param_for_index(_send_all_index); _send_all_index++; } while (p != PARAM_INVALID && !param_used(p)); if (p != PARAM_INVALID) { send_param(p); } if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } } else if (_send_all_index == 0 && hrt_absolute_time() > 20 * 1000 * 1000) { /* the boot did not seem to ever complete, warn user and set boot complete */ _mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG."); _mavlink->set_boot_complete(); } }
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; }
bool MavlinkParametersManager::send_one() { if (_send_all_index >= 0 && _mavlink->boot_complete()) { /* send all parameters if requested, but only after the system has booted */ /* The first thing we send is a hash of all values for the ground * station to try and quickly load a cached copy of our params */ if (_send_all_index == PARAM_HASH) { /* return hash check for cached params */ uint32_t hash = param_hash_check(); /* build the one-off response message */ mavlink_param_value_t msg; msg.param_count = param_count_used(); msg.param_index = -1; strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); msg.param_type = MAV_PARAM_TYPE_UINT32; memcpy(&msg.param_value, &hash, sizeof(hash)); mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); /* after this we should start sending all params */ _send_all_index = 0; /* No further action, return now */ return true; } /* look for the first parameter which is used */ param_t p; do { /* walk through all parameters, including unused ones */ p = param_for_index(_send_all_index); _send_all_index++; } while (p != PARAM_INVALID && !param_used(p)); if (p != PARAM_INVALID) { send_param(p); } if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; return false; } else { return true; } } else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) { /* the boot did not seem to ever complete, warn user and set boot complete */ _mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG."); _mavlink->set_boot_complete(); } return false; }
int mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); mavlink_param_queue_index++; return 0; } else { return 1; } }
void MavlinkParametersManager::send(const hrt_abstime t) { /* send all parameters if requested */ if (_send_all_index >= 0) { send_param(param_for_index(_send_all_index)); _send_all_index++; if (_send_all_index >= (int) param_count()) { _send_all_index = -1; } } }
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; }
void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* request all parameters */ mavlink_param_request_list_t req_list; mavlink_msg_param_request_list_decode(msg, &req_list); if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { _send_all_index = 0; _mavlink->send_statustext_info("[pm] sending list"); } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); if (set.target_system == mavlink_system.sysid && (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ param_t param = param_find(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown param: %s", name); _mavlink->send_statustext_info(buf); } else { /* set and send parameter */ param_set(param, &(set.param_value)); send_param(param); } } } break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { /* request one parameter */ mavlink_param_request_read_t req_read; mavlink_msg_param_request_read_decode(msg, &req_read); if (req_read.target_system == mavlink_system.sysid && (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ send_param(param_find(name)); } else { /* when index is >= 0, send this parameter again */ send_param(param_for_index(req_read.param_index)); } } break; } default: break; } }
void MavlinkParametersManager::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* request all parameters */ mavlink_param_request_list_t req_list; mavlink_msg_param_request_list_decode(msg, &req_list); if (req_list.target_system == mavlink_system.sysid && (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { _send_all_index = 0; _mavlink->send_statustext_info("[pm] sending list"); } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set); if (set.target_system == mavlink_system.sysid && (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter, set and send it */ param_t param = param_find(name); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown param: %s", name); _mavlink->send_statustext_info(buf); } else { /* set and send parameter */ param_set(param, &(set.param_value)); send_param(param); } } } break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { /* request one parameter */ mavlink_param_request_read_t req_read; mavlink_msg_param_request_read_decode(msg, &req_read); if (req_read.target_system == mavlink_system.sysid && (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; /* attempt to find parameter and send it */ send_param(param_find(name)); } else { /* when index is >= 0, send this parameter again */ send_param(param_for_index(req_read.param_index)); } } break; } case MAVLINK_MSG_ID_PARAM_MAP_RC: { /* map a rc channel to a parameter */ mavlink_param_map_rc_t map_rc; mavlink_msg_param_map_rc_decode(msg, &map_rc); if (map_rc.target_system == mavlink_system.sysid && (map_rc.target_component == mavlink_system.compid || map_rc.target_component == MAV_COMP_ID_ALL)) { /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ size_t i = map_rc.parameter_rc_channel_index; _rc_param_map.param_index[i] = map_rc.param_index; strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); /* enforce null termination */ _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; _rc_param_map.value_min[i] = map_rc.param_value_min; _rc_param_map.value_max[i] = map_rc.param_value_max; if (map_rc.param_index == -2) { // -2 means unset map _rc_param_map.valid[i] = false; } else { _rc_param_map.valid[i] = true; } _rc_param_map.timestamp = hrt_absolute_time(); if (_rc_param_map_pub < 0) { _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); } else { orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); } } break; } default: break; } }
bool ParameterTest::exportImportAll() { static constexpr float MAGIC_FLOAT_VAL = 0.217828f; // backup current parameters const char *param_file_name = PX4_STORAGEDIR "/param_backup"; int fd = open(param_file_name, O_WRONLY | O_CREAT, PX4_O_MODE_666); if (fd < 0) { PX4_ERR("open '%s' failed (%i)", param_file_name, errno); return false; } int result = param_export(fd, false); if (result != PX4_OK) { PX4_ERR("param_export failed"); close(fd); return false; } close(fd); bool ret = true; int N = param_count(); // set all params to corresponding param_t value for (unsigned i = 0; i < N; i++) { param_t p = param_for_index(i); if (p == PARAM_INVALID) { PX4_ERR("param invalid: %d(%d)", p, i); break; } if (param_type(p) == PARAM_TYPE_INT32) { const int32_t set_val = p; if (param_set_no_notification(p, &set_val) != PX4_OK) { PX4_ERR("param_set_no_notification failed for: %d", p); ut_assert("param_set_no_notification failed", false); } int32_t get_val = 0; if (param_get(p, &get_val) != PX4_OK) { PX4_ERR("param_get failed for: %d", p); ut_assert("param_set_no_notification failed", false); } ut_compare("value for param doesn't match default value", p, get_val); } if (param_type(p) == PARAM_TYPE_FLOAT) { const float set_val = (float)p + MAGIC_FLOAT_VAL; if (param_set_no_notification(p, &set_val) != PX4_OK) { PX4_ERR("param_set_no_notification failed for: %d", p); ut_assert("param_set_no_notification failed", false); } float get_val = 0.0f; if (param_get(p, &get_val) != PX4_OK) { PX4_ERR("param_get failed for: %d", p); ut_assert("param_set_no_notification failed", false); } ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL); } } // save if (param_save_default() != PX4_OK) { PX4_ERR("param_save_default failed"); return false; } // zero all params and verify, but don't save for (unsigned i = 0; i < N; i++) { param_t p = param_for_index(i); if (param_type(p) == PARAM_TYPE_INT32) { const int32_t set_val = 0; if (param_set_no_notification(p, &set_val) != PX4_OK) { PX4_ERR("param set failed: %d", p); ut_assert("param_set_no_notification failed", false); } int32_t get_val = -1; if (param_get(p, &get_val) != PX4_OK) { PX4_ERR("param_get failed for: %d", p); ut_assert("param_set_no_notification failed", false); } ut_compare("value for param doesn't match default value", set_val, get_val); } if (param_type(p) == PARAM_TYPE_FLOAT) { float set_val = 0.0f; if (param_set_no_notification(p, &set_val) != PX4_OK) { PX4_ERR("param set failed: %d", p); ut_assert("param_set_no_notification failed", false); } float get_val = -1.0f; if (param_get(p, &get_val) != PX4_OK) { PX4_ERR("param_get failed for: %d", p); ut_assert("param_set_no_notification failed", false); } ut_compare("value for param doesn't match default value", set_val, get_val); } } // load saved params if (param_load_default() != PX4_OK) { PX4_ERR("param_save_default failed"); ret = true; } // check every param for (unsigned i = 0; i < N; i++) { param_t p = param_for_index(i); if (param_type(p) == PARAM_TYPE_INT32) { int32_t get_val = 0; if (param_get(p, &get_val) != PX4_OK) { PX4_ERR("param_get failed for: %d", p); ut_assert("param_set_no_notification failed", false); } ut_compare("value for param doesn't match default value", p, get_val); } if (param_type(p) == PARAM_TYPE_FLOAT) { float get_val = 0.0f; if (param_get(p, &get_val) != PX4_OK) { PX4_ERR("param_get failed for: %d", p); ut_assert("param_set_no_notification failed", false); } ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL); } } param_reset_all(); // restore original params fd = open(param_file_name, O_RDONLY); if (fd < 0) { PX4_ERR("open '%s' failed (%i)", param_file_name, errno); return false; } result = param_import(fd); close(fd); if (result < 0) { PX4_ERR("importing from '%s' failed (%i)", param_file_name, result); return false; } // save if (param_save_default() != PX4_OK) { PX4_ERR("param_save_default failed"); return false; } return ret; }
int mavlink_pm_send_param_for_index(uint16_t index) { return mavlink_pm_send_param(param_for_index(index)); }
void MavlinkParametersManager::send(const hrt_abstime t) { bool space_available = _mavlink->get_free_tx_buf() >= get_size(); /* Send parameter values received from the UAVCAN topic */ if (_uavcan_parameter_value_sub < 0) { _uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value)); } bool param_value_ready; orb_check(_uavcan_parameter_value_sub, ¶m_value_ready); if (space_available && param_value_ready) { struct uavcan_parameter_value_s value; orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); mavlink_param_value_t msg; msg.param_count = value.param_count; msg.param_index = value.param_index; strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); if (value.param_type == MAV_PARAM_TYPE_REAL32) { msg.param_type = MAVLINK_TYPE_FLOAT; msg.param_value = value.real_value; } else { int32_t val; val = (int32_t)value.int_value; memcpy(&msg.param_value, &val, sizeof(int32_t)); msg.param_type = MAVLINK_TYPE_INT32_T; } _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { /* send all parameters if requested, but only after the system has booted */ /* skip if no space is available */ if (!space_available) { return; } /* The first thing we send is a hash of all values for the ground * station to try and quickly load a cached copy of our params */ if (_send_all_index == PARAM_HASH) { /* return hash check for cached params */ uint32_t hash = param_hash_check(); /* build the one-off response message */ mavlink_param_value_t msg; msg.param_count = param_count_used(); msg.param_index = -1; strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); msg.param_type = MAV_PARAM_TYPE_UINT32; memcpy(&msg.param_value, &hash, sizeof(hash)); _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); /* after this we should start sending all params */ _send_all_index = 0; /* No further action, return now */ return; } /* look for the first parameter which is used */ param_t p; do { /* walk through all parameters, including unused ones */ p = param_for_index(_send_all_index); _send_all_index++; } while (p != PARAM_INVALID && !param_used(p)); if (p != PARAM_INVALID) { send_param(p); } if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) { _send_all_index = -1; } } else if (_send_all_index == PARAM_HASH && hrt_absolute_time() > 20 * 1000 * 1000) { /* the boot did not seem to ever complete, warn user and set boot complete */ _mavlink->send_statustext_critical("WARNING: SYSTEM BOOT INCOMPLETE. CHECK CONFIG."); _mavlink->set_boot_complete(); } }