From f2972a186e563ce4b9b11da2364f95df8dbed273 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Dec 2022 16:46:32 -0500 Subject: [PATCH] mavlink: Iridium mode collect special handling - disable events, forwarding, ftp - HIGH_LATENCY2 send raw GPS lat/lon/alt if global position estimate isn't available --- src/modules/mavlink/mavlink_main.cpp | 719 +++++++++--------- src/modules/mavlink/mavlink_main.h | 5 + src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 160 ++-- 3 files changed, 452 insertions(+), 432 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 13e6c03656..e6a6b544f3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2039,7 +2039,6 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "iridium") == 0) { _mode = MAVLINK_MODE_IRIDIUM; - set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM); } else if (strcmp(myoptarg, "minimal") == 0) { _mode = MAVLINK_MODE_MINIMAL; @@ -2178,65 +2177,7 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* initialize send mutex */ - pthread_mutex_init(&_send_mutex, nullptr); - pthread_mutex_init(&_radio_status_mutex, nullptr); - - /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_forwarding_on) { - /* initialize message buffer if multiplexing is on. - * 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 * sizeof(mavlink_message_t) + 1)) { - PX4_ERR("msg buf alloc fail"); - return PX4_ERROR; - } - - /* initialize message buffer mutex */ - pthread_mutex_init(&_message_buffer_mutex, nullptr); - } - - /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ - _transmitting_enabled = true; - _transmitting_enabled_commanded = true; - - if (_mode == MAVLINK_MODE_IRIDIUM) { - _transmitting_enabled_commanded = false; - } - - /* add default streams depending on mode */ - if (_mode != MAVLINK_MODE_IRIDIUM) { - - /* HEARTBEAT is constant rate stream, rate never adjusted */ - configure_stream("HEARTBEAT", 1.0f); - - /* STATUSTEXT stream */ - configure_stream("STATUSTEXT", 20.0f); - - /* COMMAND_LONG stream: use unlimited rate to send all commands */ - configure_stream("COMMAND_LONG"); - - } - - if (configure_streams_to_default() != 0) { - PX4_ERR("configure_streams_to_default() failed"); - } - - /* set main loop delay depending on data rate to minimize CPU overhead */ - _main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate; - - /* hard limit to 1000 Hz at max */ - if (_main_loop_delay < MAVLINK_MIN_INTERVAL) { - _main_loop_delay = MAVLINK_MIN_INTERVAL; - } - - /* hard limit to 100 Hz at least */ - if (_main_loop_delay > MAVLINK_MAX_INTERVAL) { - _main_loop_delay = MAVLINK_MAX_INTERVAL; - } - - /* open the UART device after setting the instance, as it might block */ + // open the UART device after setting the instance, as it might block if (get_protocol() == Protocol::SERIAL) { _uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control); @@ -2246,6 +2187,71 @@ Mavlink::task_main(int argc, char *argv[]) } } + // initialize send mutex + pthread_mutex_init(&_send_mutex, nullptr); + pthread_mutex_init(&_radio_status_mutex, nullptr); + + _task_id = px4_getpid(); + + if (_mode != MAVLINK_MODE_IRIDIUM) { + + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_forwarding_on) { + /* initialize message buffer if multiplexing is on. + * 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 * sizeof(mavlink_message_t) + 1)) { + PX4_ERR("msg buf alloc fail"); + return PX4_ERROR; + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, nullptr); + } + + /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + // add default streams + configure_stream("HEARTBEAT", 1.f); // // HEARTBEAT is constant rate stream, rate never adjusted + configure_stream("STATUSTEXT", 20.f); + configure_stream("COMMAND_LONG"); // use unlimited rate to send all commands + + } else { + // MAVLINK_MODE_IRIDIUM + set_telemetry_status_type(telemetry_status_s::LINK_TYPE_IRIDIUM); + + set_proto_version(1); + + _forwarding_on = false; + + _ftp_on = false; + + _transmitting_enabled = true; + _transmitting_enabled_commanded = false; + + // if this instance was responsible for checking events then select a new mavlink instance + if (check_events()) { + // select next available instance + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst != this)) { + check_events_disable(); // disable events on this instance + inst->check_events_enable(); + break; + } + } + } + } + + if (configure_streams_to_default() != 0) { + PX4_ERR("configure_streams_to_default() failed"); + } + + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = math::constrain((MAIN_LOOP_DELAY * 1000) / _datarate, MAVLINK_MIN_INTERVAL, MAVLINK_MAX_INTERVAL); + #if defined(MAVLINK_UDP) /* init socket if necessary */ @@ -2255,20 +2261,15 @@ Mavlink::task_main(int argc, char *argv[]) #endif // MAVLINK_UDP - _task_id = px4_getpid(); - /* if the protocol is serial, we send the system version blindly */ if (get_protocol() == Protocol::SERIAL) { send_autopilot_capabilities(); } - _receiver.start(); - - uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink + _task_running.store(true); _mavlink_start_time = hrt_absolute_time(); - - _task_running.store(true); + _receiver.start(); while (!should_exit()) { /* main loop */ @@ -2298,289 +2299,11 @@ Mavlink::task_main(int argc, char *argv[]) configure_sik_radio(); - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; + if (_mode != MAVLINK_MODE_IRIDIUM) { + streamUpdateCycle(t); - if (_vehicle_status_sub.copy(&vehicle_status)) { - /* switch HIL mode if required */ - set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); - - if (_mode == MAVLINK_MODE_IRIDIUM) { - - if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost && - !_transmitting_enabled_commanded && _first_heartbeat_sent) { - - _transmitting_enabled = false; - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s\t", _device_name); - events::send(events::ID("mavlink_iridium_disable"), events::Log::Info, - "Disabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); - - } else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) { - _transmitting_enabled = true; - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s\t", _device_name); - events::send(events::ID("mavlink_iridium_enable"), events::Log::Info, - "Enabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); - } - } - } - } - - - // MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY - if (_mode == MAVLINK_MODE_IRIDIUM) { - int vehicle_command_updates = 0; - - while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { - vehicle_command_updates++; - const unsigned last_generation = _vehicle_command_sub.get_last_generation(); - vehicle_command_s vehicle_cmd; - - if (_vehicle_command_sub.update(&vehicle_cmd)) { - if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); - } - - if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && - _mode == MAVLINK_MODE_IRIDIUM) { - - if (vehicle_cmd.param1 > 0.5f) { - if (!_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t", - _device_name); - events::send(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info, - "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); - } - - _transmitting_enabled = true; - _transmitting_enabled_commanded = true; - - } else { - if (_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t", - _device_name); - events::send(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info, - "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); - } - - _transmitting_enabled = false; - _transmitting_enabled_commanded = false; - } - - // send positive command ack - vehicle_command_ack_s command_ack{}; - command_ack.command = vehicle_cmd.command; - command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - command_ack.from_external = !vehicle_cmd.from_external; - command_ack.target_system = vehicle_cmd.source_system; - command_ack.target_component = vehicle_cmd.source_component; - command_ack.timestamp = vehicle_cmd.timestamp; - _vehicle_command_ack_pub.publish(command_ack); - } - } - } - } - - /* send command ACK */ - bool cmd_logging_start_acknowledgement = false; - bool cmd_logging_stop_acknowledgement = false; - - if (_vehicle_command_ack_sub.updated()) { - static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) { - vehicle_command_ack_s command_ack; - const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation(); - - if (_vehicle_command_ack_sub.update(&command_ack)) { - if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation, - _vehicle_command_ack_sub.get_last_generation()); - } - - if (!command_ack.from_external && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { - mavlink_command_ack_t msg{}; - msg.result = command_ack.result; - msg.command = command_ack.command; - msg.progress = command_ack.result_param1; - msg.result_param2 = command_ack.result_param2; - msg.target_system = command_ack.target_system; - msg.target_component = command_ack.target_component; - - // TODO: always transmit the acknowledge once it is only sent over the instance the command is received - //bool _transmitting_enabled_temp = _transmitting_enabled; - //_transmitting_enabled = true; - mavlink_msg_command_ack_send_struct(get_channel(), &msg); - //_transmitting_enabled = _transmitting_enabled_temp; - - if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { - cmd_logging_start_acknowledgement = true; - - } else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP - && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { - cmd_logging_stop_acknowledgement = true; - } - } - } - } - } - - // For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands. - // We don't care about acks for these though. - if (_gimbal_v1_command_sub.updated()) { - vehicle_command_s cmd; - _gimbal_v1_command_sub.copy(&cmd); - - // FIXME: filter for target system/component - - mavlink_command_long_t msg{}; - msg.param1 = cmd.param1; - msg.param2 = cmd.param2; - msg.param3 = cmd.param3; - msg.param4 = cmd.param4; - msg.param5 = cmd.param5; - msg.param6 = cmd.param6; - msg.param7 = cmd.param7; - msg.command = cmd.command; - msg.target_system = cmd.target_system; - msg.target_component = cmd.target_component; - msg.confirmation = 0; - - mavlink_msg_command_long_send_struct(get_channel(), &msg); - } - - /* check for shell output */ - if (_mavlink_shell && _mavlink_shell->available() > 0) { - if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { - mavlink_serial_control_t msg; - msg.baudrate = 0; - msg.flags = SERIAL_CONTROL_FLAG_REPLY; - msg.timeout = 0; - msg.device = SERIAL_CONTROL_DEV_SHELL; - msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data)); - msg.target_system = _mavlink_shell->targetSysid(); - msg.target_component = _mavlink_shell->targetCompid(); - mavlink_msg_serial_control_send_struct(get_channel(), &msg); - } - } - - check_requested_subscriptions(); - - /* update streams */ - for (const auto &stream : _streams) { - stream->update(t); - - if (!_first_heartbeat_sent) { - if (_mode == MAVLINK_MODE_IRIDIUM) { - if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) { - _first_heartbeat_sent = stream->first_message_sent(); - } - - } else { - if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) { - _first_heartbeat_sent = stream->first_message_sent(); - } - } - } - } - - /* check for ulog streaming messages */ - if (_mavlink_ulog) { - if (cmd_logging_stop_acknowledgement) { - _mavlink_ulog->stop(); - _mavlink_ulog = nullptr; - - } else { - if (cmd_logging_start_acknowledgement) { - _mavlink_ulog->start_ack_received(); - } - - int ret = _mavlink_ulog->handle_update(get_channel()); - - if (ret < 0) { //abort the streaming on error - if (ret != -1) { - PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); - } - - _mavlink_ulog->stop(); - _mavlink_ulog = nullptr; - } - } - } - - /* handle new events */ - if (check_events()) { - if (_event_sub.updated()) { - LockGuard lg{mavlink_module_mutex}; - - event_s orb_event; - - while (_event_sub.update(&orb_event)) { - if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) { - ++event_sequence_offset; // skip this event - - } else { - events::Event e; - e.id = orb_event.id; - e.timestamp_ms = orb_event.timestamp / 1000; - e.sequence = orb_event.event_sequence - event_sequence_offset; - e.log_levels = orb_event.log_levels; - static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments), - "uorb message event: arguments size mismatch"); - memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments)); - _event_buffer->insert_event(e); - } - } - } - } - - _events.update(t); - - /* pass messages from other UARTs */ - if (_forwarding_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); - - resend_message(&msg); - } + } else { + streamUpdateCycleIridium(t); } /* update TX/RX rates*/ @@ -2648,6 +2371,304 @@ Mavlink::task_main(int argc, char *argv[]) return OK; } +void Mavlink::streamUpdateCycle(const hrt_abstime &t) +{ + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + /* switch HIL mode if required */ + set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); + } + } + + // send command ACK + bool cmd_logging_start_acknowledgement = false; + bool cmd_logging_stop_acknowledgement = false; + + if (_vehicle_command_ack_sub.updated()) { + static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) { + vehicle_command_ack_s command_ack; + const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation(); + + if (_vehicle_command_ack_sub.update(&command_ack)) { + if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation, + _vehicle_command_ack_sub.get_last_generation()); + } + + if (!command_ack.from_external && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + mavlink_command_ack_t msg{}; + msg.result = command_ack.result; + msg.command = command_ack.command; + msg.progress = command_ack.result_param1; + msg.result_param2 = command_ack.result_param2; + msg.target_system = command_ack.target_system; + msg.target_component = command_ack.target_component; + + // TODO: always transmit the acknowledge once it is only sent over the instance the command is received + //bool _transmitting_enabled_temp = _transmitting_enabled; + //_transmitting_enabled = true; + mavlink_msg_command_ack_send_struct(get_channel(), &msg); + //_transmitting_enabled = _transmitting_enabled_temp; + + if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + cmd_logging_start_acknowledgement = true; + + } else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP + && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { + cmd_logging_stop_acknowledgement = true; + } + } + } + } + } + + // For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands. + // We don't care about acks for these though. + if (_gimbal_v1_command_sub.updated()) { + vehicle_command_s cmd; + _gimbal_v1_command_sub.copy(&cmd); + + // FIXME: filter for target system/component + + mavlink_command_long_t msg{}; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + msg.command = cmd.command; + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.confirmation = 0; + + mavlink_msg_command_long_send_struct(get_channel(), &msg); + } + + /* check for shell output */ + if (_mavlink_shell && _mavlink_shell->available() > 0) { + if (get_free_tx_buf() >= MAVLINK_MSG_ID_SERIAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + mavlink_serial_control_t msg; + msg.baudrate = 0; + msg.flags = SERIAL_CONTROL_FLAG_REPLY; + msg.timeout = 0; + msg.device = SERIAL_CONTROL_DEV_SHELL; + msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data)); + msg.target_system = _mavlink_shell->targetSysid(); + msg.target_component = _mavlink_shell->targetCompid(); + mavlink_msg_serial_control_send_struct(get_channel(), &msg); + } + } + + check_requested_subscriptions(); + + // update streams + for (const auto &stream : _streams) { + stream->update(t); + + if (!_first_heartbeat_sent) { + if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) { + _first_heartbeat_sent = stream->first_message_sent(); + } + } + } + + // check for ulog streaming messages + if (_mavlink_ulog) { + if (cmd_logging_stop_acknowledgement) { + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + + } else { + if (cmd_logging_start_acknowledgement) { + _mavlink_ulog->start_ack_received(); + } + + int ret = _mavlink_ulog->handle_update(get_channel()); + + if (ret < 0) { //abort the streaming on error + if (ret != -1) { + PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); + } + + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + } + } + } + + // handle new events + if (check_events()) { + if (_event_sub.updated()) { + LockGuard lg{mavlink_module_mutex}; + + event_s orb_event; + + while (_event_sub.update(&orb_event)) { + if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) { + ++_event_sequence_offset; // skip this event + + } else { + events::Event e; + e.id = orb_event.id; + e.timestamp_ms = orb_event.timestamp / 1000; + e.sequence = orb_event.event_sequence - _event_sequence_offset; + e.log_levels = orb_event.log_levels; + static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments), + "uorb message event: arguments size mismatch"); + memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments)); + _event_buffer->insert_event(e); + } + } + } + } + + _events.update(t); + + // pass messages from other UARTs + if (_forwarding_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); + + resend_message(&msg); + } + } +} + +void Mavlink::streamUpdateCycleIridium(const hrt_abstime &t) +{ + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost && + !_transmitting_enabled_commanded && _first_heartbeat_sent) { + + _transmitting_enabled = false; + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s\t", _device_name); + events::send(events::ID("mavlink_iridium_disable"), events::Log::Info, + "Disabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); + + } else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) { + _transmitting_enabled = true; + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s\t", _device_name); + events::send(events::ID("mavlink_iridium_enable"), events::Log::Info, + "Enabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); + } + } + } + + // vehicle_command: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY + int vehicle_command_updates = 0; + + while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + vehicle_command_updates++; + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s vehicle_cmd; + + if (_vehicle_command_sub.update(&vehicle_cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); + } + + if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) { + + if (vehicle_cmd.param1 > 0.5f) { + if (!_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t", + _device_name); + events::send(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info, + "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); + } + + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + } else { + if (_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t", + _device_name); + events::send(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info, + "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); + } + + _transmitting_enabled = false; + _transmitting_enabled_commanded = false; + } + + // send positive command ack + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_cmd.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.from_external = !vehicle_cmd.from_external; + command_ack.target_system = vehicle_cmd.source_system; + command_ack.target_component = vehicle_cmd.source_component; + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); + } + } + } + + + + // update streams + for (const auto &stream : _streams) { + stream->update(t); + + if (!_first_heartbeat_sent) { + if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) { + _first_heartbeat_sent = stream->first_message_sent(); + } + } + } + + +} + void Mavlink::check_requested_subscriptions() { if (_subscribe_to_stream != nullptr) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3..6f19002208 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -695,6 +695,8 @@ private: static hrt_abstime _first_start_time; + uint16_t _event_sequence_offset{0}; // offset to account for skipped events, not sent via MAVLink + /** * Configure a single stream. * @param stream_name @@ -742,6 +744,9 @@ private: */ void update_rate_mult(); + void streamUpdateCycle(const hrt_abstime &t); + void streamUpdateCycleIridium(const hrt_abstime &t); + #if defined(MAVLINK_UDP) void find_broadcast_address(); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 50d97fd9d4..646a1ad625 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,18 +44,18 @@ #include #include #include +#include #include +#include #include #include +#include #include -#include #include #include -#include #include #include -#include -#include +#include #include @@ -83,8 +83,6 @@ private: _airspeed(SimpleAnalyzer::AVERAGE), _airspeed_sp(SimpleAnalyzer::AVERAGE), _climb_rate(SimpleAnalyzer::MAX), - _eph(SimpleAnalyzer::MAX), - _epv(SimpleAnalyzer::MAX), _groundspeed(SimpleAnalyzer::AVERAGE), _temperature(SimpleAnalyzer::AVERAGE), _throttle(SimpleAnalyzer::AVERAGE), @@ -119,23 +117,21 @@ private: } updated |= _climb_rate.valid(); - updated |= _eph.valid(); - updated |= _epv.valid(); updated |= _groundspeed.valid(); updated |= _temperature.valid(); updated |= _throttle.valid(); updated |= _windspeed.valid(); updated |= write_airspeed(&msg); - updated |= write_attitude_sp(&msg); + updated |= write_attitude_setpoint(&msg); updated |= write_battery_status(&msg); updated |= write_estimator_status(&msg); + updated |= write_failsafe_flags(&msg); updated |= write_fw_ctrl_status(&msg); updated |= write_geofence_result(&msg); updated |= write_global_position(&msg); updated |= write_mission_result(&msg); updated |= write_tecs_status(&msg); updated |= write_vehicle_status(&msg); - updated |= write_failsafe_flags(&msg); updated |= write_wind(&msg); if (updated) { @@ -177,14 +173,6 @@ private: _climb_rate.get_scaled(msg.climb_rate, 10.0f); } - if (_eph.valid()) { - _eph.get_scaled(msg.eph, 10.0f); - } - - if (_epv.valid()) { - _epv.get_scaled(msg.epv, 10.0f); - } - if (_groundspeed.valid()) { _groundspeed.get_scaled(msg.groundspeed, 5.0f); } @@ -230,8 +218,6 @@ private: } _climb_rate.reset(); - _eph.reset(); - _epv.reset(); _groundspeed.reset(); _temperature.reset(); _throttle.reset(); @@ -244,7 +230,7 @@ private: { airspeed_s airspeed; - if (_airspeed_sub.update(&airspeed)) { + if (_airspeed_sub.copy(&airspeed)) { if (airspeed.confidence < 0.95f) { // the same threshold as for the commander msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; } @@ -255,11 +241,11 @@ private: return false; } - bool write_attitude_sp(mavlink_high_latency2_t *msg) + bool write_attitude_setpoint(mavlink_high_latency2_t *msg) { vehicle_attitude_setpoint_s attitude_sp; - if (_attitude_sp_sub.update(&attitude_sp)) { + if (_attitude_sp_sub.copy(&attitude_sp)) { msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f); return true; } @@ -274,7 +260,7 @@ private: for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { battery_status_s battery; - if (_batteries[i].subscription.update(&battery)) { + if (_batteries[i].subscription.copy(&battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -302,7 +288,8 @@ private: estimator_status_s estimator_status; - if (_estimator_status_sub.update(&estimator_status)) { + if (_estimator_status_sub.copy(&estimator_status)) { + if (estimator_status.gps_check_fail_flags > 0 || estimator_status.filter_fault_flags > 0 || estimator_status.innovation_check_flags > 0) { @@ -320,7 +307,7 @@ private: { position_controller_status_s pos_ctrl_status; - if (_pos_ctrl_status_sub.update(&pos_ctrl_status)) { + if (_pos_ctrl_status_sub.copy(&pos_ctrl_status)) { uint16_t target_distance; convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance); msg->target_distance = target_distance; @@ -334,7 +321,7 @@ private: { geofence_result_s geofence; - if (_geofence_sub.update(&geofence)) { + if (_geofence_sub.copy(&geofence)) { if (geofence.primary_geofence_breached) { msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE; } @@ -349,8 +336,9 @@ private: { vehicle_global_position_s global_pos; vehicle_local_position_s local_pos; + sensor_gps_s sensor_gps; - if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) { + if (_global_pos_sub.update(&global_pos) && _local_pos_sub.copy(&local_pos)) { msg->latitude = global_pos.lat * 1e7; msg->longitude = global_pos.lon * 1e7; @@ -367,6 +355,22 @@ private: msg->heading = static_cast(math::degrees(matrix::wrap_2pi(local_pos.heading)) * 0.5f); + msg->eph = global_pos.eph * 10.f; // m -> dm + msg->eph = global_pos.eph * 10.f; // m -> dm + + return true; + + } else if (_vehicle_gps_position_sub.copy(&sensor_gps)) { + msg->latitude = sensor_gps.lat; + msg->longitude = sensor_gps.lon; + + msg->altitude = math::constrain((int16_t)roundf(sensor_gps.alt * 1e-3f), (int16_t)INT16_MIN, (int16_t)INT16_MAX); + + msg->heading = math::degrees(matrix::wrap_2pi(sensor_gps.cog_rad)) * 0.5f; // rad -> deg/2 + + msg->eph = sensor_gps.eph * 10.f; // m -> dm + msg->eph = sensor_gps.eph * 10.f; // m -> dm + return true; } @@ -377,7 +381,7 @@ private: { mission_result_s mission_result; - if (_mission_result_sub.update(&mission_result)) { + if (_mission_result_sub.copy(&mission_result)) { msg->wp_num = mission_result.seq_current; return true; } @@ -389,7 +393,7 @@ private: { tecs_status_s tecs_status; - if (_tecs_status_sub.update(&tecs_status)) { + if (_tecs_status_sub.copy(&tecs_status)) { int16_t target_altitude; convert_limit_safe(tecs_status.altitude_sp, target_altitude); msg->target_altitude = target_altitude; @@ -402,52 +406,56 @@ private: bool write_vehicle_status(mavlink_high_latency2_t *msg) { + // vehicle_status vehicle_status_s status; - if (_status_sub.update(&status)) { - health_report_s health_report; - - if (_health_report_sub.copy(&health_report)) { - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::absolute_pressure) { - msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; - } - - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::accel) { - msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; - } - - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::gyro) { - msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO; - } - - if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) - events::px4::enums::health_component_t::magnetometer) { - msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG; - } - } - + if (_status_sub.copy(&status)) { if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) { msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; } - - // flight mode - union px4_custom_mode custom_mode {get_px4_custom_mode(status.nav_state)}; - msg->custom_mode = custom_mode.custom_mode_hl; - - return true; } - return false; + + health_report_s health_report; + + if (_health_report_sub.copy(&health_report)) { + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::absolute_pressure) { + + msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } + + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::accel) { + + msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; + } + + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::gyro) { + + msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO; + } + + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::magnetometer) { + + msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG; + } + } + + // flight mode + union px4_custom_mode custom_mode {get_px4_custom_mode(status.nav_state)}; + msg->custom_mode = custom_mode.custom_mode_hl; + + return true; } bool write_failsafe_flags(mavlink_high_latency2_t *msg) { failsafe_flags_s failsafe_flags; - if (_failsafe_flags_sub.update(&failsafe_flags)) { + if (_failsafe_flags_sub.copy(&failsafe_flags)) { if (failsafe_flags.gps_position_invalid) { msg->failure_flags |= HL_FAILURE_FLAG_GPS; } @@ -464,7 +472,6 @@ private: msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; } - return true; } @@ -475,7 +482,7 @@ private: { wind_s wind; - if (_wind_sub.update(&wind)) { + if (_wind_sub.copy(&wind)) { msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f); return true; @@ -498,7 +505,6 @@ private: update_tecs_status(); update_battery_status(); update_local_position(); - update_gps(); update_vehicle_status(); update_wind(); } @@ -545,16 +551,6 @@ private: } } - void update_gps() - { - sensor_gps_s gps; - - if (_gps_sub.update(&gps)) { - _eph.add_value(gps.eph, _update_rate_filtered); - _epv.add_value(gps.epv, _update_rate_filtered); - } - } - void update_vehicle_status() { vehicle_status_s status; @@ -627,23 +623,21 @@ private: uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; - uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; + uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)}; uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _health_report_sub{ORB_ID(health_report)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; - uORB::Subscription _health_report_sub{ORB_ID(health_report)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; SimpleAnalyzer _climb_rate; - SimpleAnalyzer _eph; - SimpleAnalyzer _epv; SimpleAnalyzer _groundspeed; SimpleAnalyzer _temperature; SimpleAnalyzer _throttle;