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(); }
static void do_show(const char *search_string) { printf("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, false, false); printf("\n %u parameters total, %u used.\n", param_count(), param_count_used()); }
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; }
/* TAC to stdout */ void print_tac(tac_quad *quad) { if (quad==NULL) return; switch(quad->type) { case TT_LABEL: printf("%s:\n", to_string(quad->operand1)); break; case TT_FN_DEF: printf("_%s:\n", to_string(quad->operand1)); break; case TT_FN_CALL: printf("%s = CallFn _%s\n", correct_string_rep(quad->result), correct_string_rep(quad->operand1)); break; case TT_INIT_FRAME: printf("InitFrame %s\n", correct_string_rep(quad->operand1)); break; case TT_POP_PARAM: printf("PopParam %s\n", quad->operand1->identifier); break; case TT_PUSH_PARAM: printf("PushParam %s\n", correct_string_rep(quad->operand1)); break; case TT_IF: printf("If %s Goto %s\n", correct_string_rep(quad->operand1), correct_string_rep(quad->result)); break; case TT_ASSIGN: printf("%s %s %s\n", correct_string_rep(quad->result), quad->op, correct_string_rep(quad->operand1)); break; case TT_GOTO: printf("Goto %s\n", correct_string_rep(quad->operand1)); break; case TT_OP: printf("%s = %s %s %s\n", correct_string_rep(quad->result), correct_string_rep(quad->operand1), quad->op, correct_string_rep(quad->operand2)); break; case TT_RETURN: if (quad->operand1) { printf("Return %s\n", correct_string_rep(quad->operand1)); } else { printf("Return"); } break; case TT_PREPARE: printf("PrepareToCall %d\n", param_count(quad->operand1)); break; case TT_BEGIN_FN: printf("BeginFn %s\n", correct_string_rep(quad->operand1)); break; case TT_FN_BODY: printf("FnBody\n"); break; case TT_END_FN: printf("EndFn\n"); break; default: fatal("Unknown TAC Quad type '%d'", quad->type); break; } print_tac(quad->next); }
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; }
static int do_show(const char *search_string, bool only_changed) { PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, (char *)search_string, only_changed, false); PARAM_PRINT("\n %u parameters total, %u used.\n", param_count(), param_count_used()); return 0; }
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; } } }
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; }
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"); }
void MavlinkParametersManager::send_param(param_t param) { if (param == PARAM_INVALID) { return; } 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; } msg.param_count = param_count(); msg.param_index = param_get_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); }
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; }
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; }
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(); } }
int Mavlink::task_main(int argc, char *argv[]) { int ch; _baudrate = 57600; _datarate = 0; _mode = MAVLINK_MODE_NORMAL; /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); if (_baudrate < 9600 || _baudrate > 921600) { warnx("invalid baud rate '%s'", optarg); err_flag = true; } break; case 'r': _datarate = strtoul(optarg, NULL, 10); if (_datarate < 10 || _datarate > MAX_DATA_RATE) { warnx("invalid data rate '%s'", optarg); err_flag = true; } break; case 'd': _device_name = optarg; break; // case 'e': // mavlink_link_termination_allowed = true; // break; case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; } else if (strcmp(optarg, "camera") == 0) { _mode = MAVLINK_MODE_CAMERA; } break; case 'f': _forwarding_on = true; break; case 'p': _passing_on = true; break; case 'v': _verbose = true; break; case 'w': _wait_to_transmit = true; break; case 'x': _ftp_on = true; break; default: err_flag = true; break; } } if (err_flag) { usage(); return ERROR; } if (_datarate == 0) { /* convert bits to bytes and use 1/2 of bandwidth by default */ _datarate = _baudrate / 20; } if (_datarate > MAX_DATA_RATE) { _datarate = MAX_DATA_RATE; } if (Mavlink::instance_exists(_device_name, this)) { warnx("mavlink instance for %s already running", _device_name); return ERROR; } /* inform about mode */ switch (_mode) { case MAVLINK_MODE_NORMAL: warnx("mode: NORMAL"); break; case MAVLINK_MODE_CUSTOM: warnx("mode: CUSTOM"); break; case MAVLINK_MODE_CAMERA: warnx("mode: CAMERA"); break; default: warnx("ERROR: Unknown mode"); break; } warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); struct termios uart_config_original; /* default values for arguments */ _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart); if (_uart_fd < 0) { warn("could not open %s", _device_name); return ERROR; } /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ if (_passing_on || _ftp_on) { /* initialize message buffer if multiplexing is on or its needed for FTP. * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { errx(1, "can't allocate message buffer, exiting"); } /* initialize message buffer mutex */ pthread_mutex_init(&_message_buffer_mutex, NULL); } /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); /* Initialize system properties */ mavlink_update_system(); /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); /* create mission manager */ _mission_manager = new MavlinkMissionManager(this); _mission_manager->set_verbose(_verbose); _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); uint64_t status_time = 0; struct vehicle_status_s status; status_sub->update(&status_time, &status); MavlinkCommandsStream commands_stream(this, _channel); /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = get_rate_mult(); configure_stream("HEARTBEAT", 1.0f); switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 15.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break; default: break; } /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); while (!_task_should_exit) { /* main loop */ usleep(_main_loop_delay); perf_begin(_loop_perf); hrt_abstime t = hrt_absolute_time(); if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); } if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == HIL_STATE_ON); } /* update commands stream */ commands_stream.update(t); /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); } delete _subscribe_to_stream; _subscribe_to_stream = nullptr; } /* update streams */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { stream->update(t); } if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); _mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { send_statustext(msg.severity, msg.text); } } } /* pass messages from other UARTs or FTP worker */ if (_passing_on || _ftp_on) { bool is_part; uint8_t *read_ptr; uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { // Reconstruct message from buffer mavlink_message_t msg; write_ptr = (uint8_t *)&msg; // Pull a single message from the buffer size_t read_count = available; if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } memcpy(write_ptr, read_ptr, read_count); // We hold the mutex until after we complete the second part of the buffer. If we don't // we may end up breaking the empty slot overflow detection semantics when we mark the // possibly partial read below. pthread_mutex_lock(&_message_buffer_mutex); message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { write_ptr += read_count; available = message_buffer_get_ptr((void **)&read_ptr, &is_part); read_count = sizeof(mavlink_message_t) - read_count; memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } pthread_mutex_unlock(&_message_buffer_mutex); _mavlink_resend_uart(_channel, &msg); } } /* update TX/RX rates*/ if (t > _bytes_timestamp + 1000000) { if (_bytes_timestamp != 0) { float dt = (t - _bytes_timestamp) / 1000.0f; _rate_tx = _bytes_tx / dt; _rate_txerr = _bytes_txerr / dt; _rate_rx = _bytes_rx / dt; _bytes_tx = 0; _bytes_txerr = 0; _bytes_rx = 0; } _bytes_timestamp = t; } perf_end(_loop_perf); }
pthread_addr_t UavcanServers::run(pthread_addr_t) { prctl(PR_SET_NAME, "uavcan fw srv", 0); Lock lock(_subnode_mutex); /* Copy any firmware bundled in the ROMFS to the appropriate location on the SD card, unless the user has copied other firmware for that device. */ unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); /* the subscribe call needs to happen in the same thread, * so not in the constructor */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); _param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); _enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin)); _enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication)); _enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset)); _enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save)); _count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false; memset(_param_counts, 0, sizeof(_param_counts)); _esc_enumeration_active = false; memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; while (!_subnode_thread_should_exit) { if (_check_fw == true) { _check_fw = false; _node_info_retriever.invalidateAll(); } const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); if (spin_res < 0) { warnx("node spin error %i", spin_res); } // Check for parameter requests (get/set/list) bool param_request_ready; orb_check(param_request_sub, ¶m_request_ready); if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { struct uavcan_parameter_request_s request; orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request); if (_param_counts[request.node_id]) { /* * We know how many parameters are exposed by this node, so * process the request. */ if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { req.index = request.param_index; } else { req.name = (char*)request.param_id; } int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; _param_index = request.param_index; } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { req.index = request.param_index; } else { req.name = (char*)request.param_id; } if (request.param_type == MAV_PARAM_TYPE_REAL32) { req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value; } else if (request.param_type == MAV_PARAM_TYPE_UINT8) { req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value; } else { req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value; } // Set the dirty bit for this node set_node_params_dirty(request.node_id); int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; _param_index = request.param_index; } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { // This triggers the _param_list_in_progress case below. _param_index = 0; _param_list_in_progress = true; _param_list_node_id = request.node_id; _param_list_all_nodes = false; warnx("UAVCAN command bridge: starting component-specific param list"); } } else if (request.node_id == MAV_COMP_ID_ALL) { if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { /* * This triggers the _param_list_in_progress case below, * but additionally iterates over all active nodes. */ _param_index = 0; _param_list_in_progress = true; _param_list_node_id = get_next_active_node_id(1); _param_list_all_nodes = true; warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } } } else { /* * Need to know how many parameters this node has before we can * continue; count them now and then process the request. */ param_count(request.node_id); } } // Handle parameter listing index/node ID advancement if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { if (_param_index >= _param_counts[_param_list_node_id]) { warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); // Reached the end of the current node's parameter set. _param_list_in_progress = false; if (_param_list_all_nodes) { // We're listing all parameters for all nodes -- get the next node ID uint8_t next_id = get_next_active_node_id(_param_list_node_id); if (next_id < 128) { _param_list_node_id = next_id; /* * If there is a next node ID, check if that node's parameters * have been counted before. If not, do it now. */ if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } // Keep on listing. _param_index = 0; _param_list_in_progress = true; warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); } } } } // Check if we're still listing, and need to get the next parameter if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { // Ready to request the next value -- _param_index is incremented // after each successful fetch by cb_getset uavcan::protocol::param::GetSet::Request req; req.index = _param_index; int call_res = _param_getset_client.call(_param_list_node_id, req); if (call_res < 0) { _param_list_in_progress = false; warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; } } // Check for ESC enumeration commands bool cmd_ready; orb_check(cmd_sub, &cmd_ready); if (cmd_ready && !_cmd_in_progress) { struct vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { int command_id = static_cast<int>(cmd.param1 + 0.5f); int node_id = static_cast<int>(cmd.param2 + 0.5f); int call_res; warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); switch (command_id) { case 0: case 1: { _esc_enumeration_active = command_id; _esc_enumeration_index = 0; _esc_count = 0; uavcan::protocol::enumeration::Begin::Request req; // TODO: Incorrect implementation; the parameter name field should be left empty. // Leaving it as-is to avoid breaking compatibility with non-compliant nodes. req.parameter_name = "esc_index"; req.timeout_sec = _esc_enumeration_active ? 65535 : 0; call_res = _enumeration_client.call(get_next_active_node_id(1), req); if (call_res < 0) { warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); beep(BeepFrequencyError); } else { beep(BeepFrequencyGenericIndication); } break; } default: { warnx("UAVCAN command bridge: unknown command ID %d", command_id); break; } } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) { int command_id = static_cast<int>(cmd.param1 + 0.5f); warnx("UAVCAN command bridge: received storage command ID %d", command_id); switch (command_id) { case 1: { // Param save request int node_id; node_id = get_next_dirty_node_id(1); if (node_id < 128) { _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; param_opcode(node_id); } break; } case 2: { // Command is a param erase request -- apply it to all active nodes by setting the dirty bit _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; for (int i = 1; i < 128; i = get_next_active_node_id(i)) { set_node_params_dirty(i); } param_opcode(get_next_dirty_node_id(1)); break; } } } } // Shut down once armed // TODO (elsewhere): start up again once disarmed? bool updated; orb_check(armed_sub, &updated); if (updated) { struct actuator_armed_s armed; orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); if (armed.armed && !armed.lockdown) { warnx("UAVCAN command bridge: system armed, exiting now."); break; } } } _subnode_thread_should_exit = false; warnx("exiting"); return (pthread_addr_t) 0; }