int Mavlink::configure_stream(const char *stream_name, const float rate) { /* calculate interval in us, 0 means disabled stream */ unsigned int interval = interval_from_rate(rate); /* search if stream exists */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { if (strcmp(stream_name, stream->get_name()) == 0) { if (interval > 0) { /* set new interval */ stream->set_interval(interval); } else { /* delete stream */ LL_DELETE(_streams, stream); delete stream; warnx("deleted stream %s", stream->get_name()); } return OK; } } if (interval == 0) { /* stream was not active and is requested to be disabled, do nothing */ return OK; } /* search for stream with specified name in supported streams list */ for (unsigned int i = 0; streams_list[i] != nullptr; i++) { if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ stream = streams_list[i]->new_instance(this); stream->set_interval(interval); LL_APPEND(_streams, stream); return OK; } } /* if we reach here, the stream list does not contain the stream */ warnx("stream %s not found", stream_name); return ERROR; }
void Mavlink::adjust_stream_rates(const float multiplier) { /* do not allow to push us to zero */ if (multiplier < 0.0005f) { return; } /* search if stream exists */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { /* set new interval */ unsigned interval = stream->get_interval(); interval /= multiplier; /* allow max ~2000 Hz */ if (interval < 1600) { interval = 500; } /* set new interval */ stream->set_interval(interval * multiplier); }
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); }