/* send a statustext message to specific MAVLink channel, zero indexed */ void GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY severity, uint8_t dest_chan, const char *fmt, ...) { char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {}; va_list arg_list; va_start(arg_list, fmt); hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list); va_end(arg_list); send_statustext(severity, (1<<dest_chan), text); }
/* send a statustext message to all active MAVLink connections */ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ...) { char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {}; va_list arg_list; va_start(arg_list, fmt); hal.util->vsnprintf((char *)text, sizeof(text), fmt, arg_list); va_end(arg_list); send_statustext(severity, mavlink_active, text); }
// try to send a message, return false if it won't fit in the serial tx buffer static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) { int16_t payload_space = serial_free(); if (telemetry_delayed(chan)) { return false; } switch(id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); send_heartbeat(chan); break; case MSG_ATTITUDE: CHECK_PAYLOAD_SIZE(ATTITUDE); send_attitude(chan); break; case MSG_LOCATION: CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); send_location(chan); break; case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs(chan); break; case MSG_STATUSTEXT: CHECK_PAYLOAD_SIZE(STATUSTEXT); send_statustext(chan); break; default: break; } return true; }
void Mavlink::send_statustext_emergency(const char *string) { send_statustext(MAV_SEVERITY_EMERGENCY, string); }
void Mavlink::send_statustext_critical(const char *string) { send_statustext(MAV_SEVERITY_CRITICAL, string); }
void Mavlink::send_statustext_info(const char *string) { send_statustext(MAV_SEVERITY_INFO, string); }
/* send a text message to all GCS */ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) { char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] {}; hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list); send_statustext(severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), text); }
int Mavlink::send_statustext_emergency(const char *string) { return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string); }
int Mavlink::send_statustext_critical(const char *string) { return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string); }
int Mavlink::send_statustext_info(const char *string) { return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string); }
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); }