diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index fd8ccc095e..0b7e9978fa 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -401,8 +401,6 @@ void IridiumSBD::csq_loop(void) VERBOSE_INFO("SIGNAL QUALITY: %d", _signal_quality); _new_state = SATCOM_STATE_STANDBY; - - publish_telemetry_status(); } void IridiumSBD::sbdsession_loop(void) @@ -417,7 +415,6 @@ void IridiumSBD::sbdsession_loop(void) PX4_WARN("SBD SESSION: TIMEOUT!"); ++_failed_sbd_sessions; _new_state = SATCOM_STATE_STANDBY; - publish_telemetry_status(); pthread_mutex_unlock(&_tx_buf_mutex); } @@ -429,7 +426,6 @@ void IridiumSBD::sbdsession_loop(void) ++_failed_sbd_sessions; _new_state = SATCOM_STATE_STANDBY; - publish_telemetry_status(); pthread_mutex_unlock(&_tx_buf_mutex); return; } @@ -440,7 +436,6 @@ void IridiumSBD::sbdsession_loop(void) _new_state = SATCOM_STATE_STANDBY; ++_failed_sbd_sessions; - publish_telemetry_status(); pthread_mutex_unlock(&_tx_buf_mutex); return; } @@ -510,7 +505,6 @@ void IridiumSBD::sbdsession_loop(void) VERBOSE_INFO("SBD SESSION: FAILED (%d)", mo_status); } - publish_telemetry_status(); _new_state = SATCOM_STATE_STANDBY; pthread_mutex_unlock(&_tx_buf_mutex); } @@ -661,7 +655,6 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - _packet_length < 0) { _tx_buf_write_idx = 0; ++_num_tx_buf_reset; - publish_telemetry_status(); } // keep track of the remaining packet length and if the full message is written @@ -679,8 +672,6 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) _last_write_time = hrt_absolute_time(); _tx_buf_write_pending = true; - publish_telemetry_status(); - pthread_mutex_unlock(&_tx_buf_mutex); return buflen; @@ -958,28 +949,6 @@ pollevent_t IridiumSBD::poll_state(struct file *filp) return pollstate; } -void IridiumSBD::publish_telemetry_status() -{ - // publish telemetry status for logger - struct telemetry_status_s tstatus = {}; - - tstatus.timestamp = hrt_absolute_time(); - tstatus.telem_time = tstatus.timestamp; - tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM; - tstatus.rssi = _signal_quality; - tstatus.txbuf = ceil(100.0f * (float)_tx_buf_write_idx / SATCOM_TX_BUF_LEN); - tstatus.heartbeat_time = _last_heartbeat; - tstatus.rxerrors = _failed_sbd_sessions; - - if (_telemetry_status_pub == nullptr) { - int multi_instance; - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_LOW); - - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); - } -} - void IridiumSBD::publish_iridium_status() { bool need_to_publish = false; diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 9f0c633738..302c3079c0 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -40,7 +40,6 @@ #include #include -#include #include typedef enum { @@ -255,11 +254,6 @@ private: */ pollevent_t poll_state(struct file *filp); - /* - * Publish the up to date telemetry status - */ - void publish_telemetry_status(void); - void publish_iridium_status(void); /** @@ -306,7 +300,6 @@ private: bool _writing_mavlink_packet = false; uint16_t _packet_length = 0; - orb_advert_t _telemetry_status_pub = nullptr; orb_advert_t _iridiumsbd_status_pub = nullptr; bool _test_pending = false; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 959e55cc3c..419b99f5bc 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -168,13 +169,11 @@ private: bool high_latency = false; } _telemetry[ORB_MULTI_MAX_INSTANCES]; - // publisher - orb_advert_t _vehicle_cmd_pub = nullptr; - // Subscriptions Subscription _mission_result_sub; Subscription _global_position_sub; Subscription _local_position_sub; + Subscription _iridiumsbd_status_sub; }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89640c2d69..c3f94f8fa6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -587,7 +587,8 @@ Commander::Commander() : ModuleParams(nullptr), _mission_result_sub(ORB_ID(mission_result)), _global_position_sub(ORB_ID(vehicle_global_position)), - _local_position_sub(ORB_ID(vehicle_local_position)) + _local_position_sub(ORB_ID(vehicle_local_position)), + _iridiumsbd_status_sub(ORB_ID(iridiumsbd_status)) { } @@ -1020,10 +1021,17 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } break; case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { - // only send the acknowledge from the commander, the command actually is handled by each mavlink instance - // only send the acknowledge if the command is received from an external source - if (cmd.from_external) { - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + bool hl_exists = false; + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_telemetry[i].high_latency) { + hl_exists = true; + } + } + + // if no high latency telemetry exists send a failed acknowledge + if (!hl_exists) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED; + mavlink_log_critical(&mavlink_log_pub, "Control high latency failed, no hl telemetry available"); } } break; @@ -4100,6 +4108,17 @@ void Commander::poll_telemetry_status() _telemetry[i].last_heartbeat = telemetry.heartbeat_time; } } + + // for iridium telemetry use the iridiumsbd_status to update the heartbeat + if (_telemetry[i].high_latency) { + if (_iridiumsbd_status_sub.update()) { + const hrt_abstime isbd_timestamp = _iridiumsbd_status_sub.get().last_heartbeat; + + if (isbd_timestamp > _telemetry[i].last_heartbeat) { + _telemetry[i].last_heartbeat = isbd_timestamp; + } + } + } } } @@ -4193,21 +4212,6 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 status.high_latency_data_link_active = false; *status_changed = true; mavlink_log_critical(&mavlink_log_pub, "LOW LATENCY DATA LINKS REGAINED, DEACTIVATING HIGH LATENCY LINK"); - - vehicle_command_s vehicle_cmd; - vehicle_cmd.timestamp = hrt_absolute_time(); - vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY; - vehicle_cmd.param1 = 0.0f; - vehicle_cmd.from_external = false; - vehicle_cmd.target_system = status.system_id; - vehicle_cmd.target_component = 0; - - if (_vehicle_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd); - - } else { - _vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd); - } } } else { @@ -4216,14 +4220,6 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 status.high_latency_data_link_active = true; *status_changed = true; - vehicle_command_s vehicle_cmd; - vehicle_cmd.timestamp = hrt_absolute_time(); - vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY; - vehicle_cmd.param1 = 1.0f; - vehicle_cmd.from_external = false; - vehicle_cmd.target_system = status.system_id; - vehicle_cmd.target_component = 0; - // set heartbeat to current time for high latency so that the first message can be transmitted for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_telemetry[i].high_latency) { @@ -4231,13 +4227,6 @@ void Commander::data_link_checks(int32_t highlatencydatalink_loss_timeout, int32 } } - if (_vehicle_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vehicle_cmd); - - } else { - _vehicle_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vehicle_cmd); - } - if (!status.data_link_lost) { mavlink_log_critical(&mavlink_log_pub, "ALL LOW LATENCY DATA LINKS LOST, ACTIVATING HIGH LATENCY LINK"); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fa462f37e1..651c245282 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1972,6 +1972,9 @@ Mavlink::task_main(int argc, char *argv[]) ack_sub->subscribe_from_beginning(true); cmd_sub->subscribe_from_beginning(true); + /* command ack */ + orb_advert_t command_ack_pub = nullptr; + MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); struct vehicle_status_s status; @@ -2202,6 +2205,20 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); + + if (_mode == MAVLINK_MODE_IRIDIUM) { + if (_transmitting_enabled && + !status.high_latency_data_link_active && + !_transmitting_enabled_commanded && + (_last_write_success_time > 0u)) { // a first message is written + _transmitting_enabled = false; + mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name); + + } else if (!_transmitting_enabled && status.high_latency_data_link_active) { + _transmitting_enabled = true; + mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name); + } + } } struct vehicle_command_s vehicle_cmd; @@ -2211,7 +2228,8 @@ Mavlink::task_main(int argc, char *argv[]) (_mode == MAVLINK_MODE_IRIDIUM)) { if (vehicle_cmd.param1 > 0.5f) { if (!_transmitting_enabled) { - PX4_INFO("Enable transmitting with IRIDIUM mavlink on device %s by command", _device_name); + mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); } _transmitting_enabled = true; @@ -2219,12 +2237,33 @@ Mavlink::task_main(int argc, char *argv[]) } else { if (_transmitting_enabled) { - PX4_INFO("Disable transmitting with IRIDIUM mavlink on device %s by command", _device_name); + mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); } _transmitting_enabled = false; _transmitting_enabled_commanded = false; } + + // send positive command ack + struct vehicle_command_ack_s command_ack = { + .timestamp = vehicle_cmd.timestamp, + .result_param2 = 0, + .command = vehicle_cmd.command, + .result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED, + .from_external = !vehicle_cmd.from_external, + .result_param1 = 0, + .target_system = vehicle_cmd.source_system, + .target_component = vehicle_cmd.source_component + }; + + if (command_ack_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); + + } else { + command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); + } } } @@ -2335,13 +2374,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - if (_mode == MAVLINK_MODE_IRIDIUM) { - if ((_last_write_success_time > 0u) && !_transmitting_enabled_commanded && _transmitting_enabled) { - _transmitting_enabled = false; - PX4_INFO("Disable Iridium Mavlink after first packet is sent"); - } - } - /* pass messages from other UARTs */ if (_forwarding_on) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 48a6241f00..dc6eb1c04e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2408,6 +2408,22 @@ MavlinkReceiver::receive_thread(void *arg) // poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc. const int timeout = 10; + // publish the telemetry status once for the iridium telemetry + if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM; + + if (_telemetry_status_pub == nullptr) { + int multi_instance; + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } + #ifdef __PX4_POSIX /* 1500 is the Wifi MTU, so we make sure to fit a full packet */ uint8_t buf[1600 * 5];