diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 5e1f943cec..42bf086b86 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -120,6 +120,11 @@ void TelemetryImpl::init() [this](const mavlink_message_t& message) { process_cellular_status(message); }, this); + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_NIC_INFO, + [this](const mavlink_message_t& message) { process_nic_info(message); }, + this); + _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_HEARTBEAT, [this](const mavlink_message_t& message) { process_heartbeat(message); }, @@ -377,6 +382,13 @@ Telemetry::Result TelemetryImpl::set_rate_cellular_status(double rate_hz) return Telemetry::Result::Unsupported; } +Telemetry::Result TelemetryImpl::set_rate_nic_info(double rate_hz) +{ + UNUSED(rate_hz); + LogWarn() << "System status is usually fixed at 1 Hz"; + return Telemetry::Result::Unsupported; +} + Telemetry::Result TelemetryImpl::set_rate_actuator_control_target(double rate_hz) { return telemetry_result_from_command_result( @@ -615,6 +627,15 @@ void TelemetryImpl::set_rate_cellular_status_async( _parent->call_user_callback([callback]() { callback(Telemetry::Result::Unsupported); }); } +void TelemetryImpl::set_rate_nic_info_async( + double rate_hz, Telemetry::ResultCallback callback) +{ + UNUSED(rate_hz); + LogWarn() << "System status is usually fixed at 1 Hz"; + _parent->call_user_callback([callback]() { callback(Telemetry::Result::Unsupported); }); +} + + void TelemetryImpl::set_rate_unix_epoch_time_async( double rate_hz, Telemetry::ResultCallback callback) { @@ -1264,7 +1285,7 @@ void TelemetryImpl::process_cellular_status(const mavlink_message_t& message) mavlink_msg_cellular_status_decode(&message, &cell_status); Telemetry::CellularStatus new_cell_status; - new_cell_status.id = cell_status.id; + new_cell_status.instance_number = cell_status.id; new_cell_status.status = cell_status.status; new_cell_status.failure_reason = cell_status.failure_reason; new_cell_status.type = cell_status.type; @@ -1273,6 +1294,18 @@ void TelemetryImpl::process_cellular_status(const mavlink_message_t& message) new_cell_status.mnc = cell_status.mnc; new_cell_status.lac = cell_status.lac; + new_cell_status.download_rate = cell_status.download_rate; + new_cell_status.upload_rate = cell_status.upload_rate; + new_cell_status.bit_error_rate = cell_status.ber; + new_cell_status.rx_level = cell_status.rx_level; + new_cell_status.tx_level = cell_status.tx_level; + new_cell_status.signal_to_noise = cell_status.signal_to_noise; + new_cell_status.cell_tower_id = cell_status.cell_tower_id; + new_cell_status.band_number = cell_status.band_number; + new_cell_status.band_frequency = cell_status.band_frequency; + new_cell_status.arfcn = cell_status.arfcn; + + set_cellular_status(new_cell_status); { @@ -1282,6 +1315,29 @@ void TelemetryImpl::process_cellular_status(const mavlink_message_t& message) } } +void TelemetryImpl::process_nic_info(const mavlink_message_t& message) +{ + mavlink_nic_info_t network_interface_card_info; + mavlink_msg_nic_info_decode(&message, &network_interface_card_info); + + Telemetry::NicInfo new_nic_info; + new_nic_info.instance_number = network_interface_card_info.id; + new_nic_info.nic_id = network_interface_card_info.nic_id; + new_nic_info.nic_model_name = network_interface_card_info.nic_model; + new_nic_info.imei = network_interface_card_info.imei; + new_nic_info.iccid = network_interface_card_info.iccid; + new_nic_info.imsi = network_interface_card_info.imsi; + new_nic_info.firmware_version = network_interface_card_info.firmware_version; + + set_nic_info(new_nic_info); + + { + std::lock_guard lock(_subscription_mutex); + _nic_info_subscriptions.queue( + nic_info(), [this](const auto& func) { _parent->call_user_callback(func); }); + } +} + void TelemetryImpl::process_heartbeat(const mavlink_message_t& message) { if (message.compid != MAV_COMP_ID_AUTOPILOT1) { @@ -2065,6 +2121,12 @@ Telemetry::CellularStatus TelemetryImpl::cellular_status() const return _cellular_status; } +Telemetry::NicInfo TelemetryImpl::nic_info() const +{ + std::lock_guard lock(_nic_info_mutex); + return _nic_info; +} + uint64_t TelemetryImpl::unix_epoch_time() const { std::lock_guard lock(_unix_epoch_time_mutex); @@ -2196,6 +2258,12 @@ void TelemetryImpl::set_cellular_status(Telemetry::CellularStatus cellular_statu _cellular_status = cellular_status; } +void TelemetryImpl::set_nic_info(Telemetry::NicInfo nic_info) +{ + std::lock_guard lock(_nic_info_mutex); + _nic_info = nic_info; +} + void TelemetryImpl::set_unix_epoch_time_us(uint64_t time_us) { std::lock_guard lock(_unix_epoch_time_mutex); @@ -2581,6 +2649,19 @@ void TelemetryImpl::unsubscribe_cellular_status(Telemetry::CellularStatusHandle _cellular_status_subscriptions.unsubscribe(handle); } +Telemetry::NicInfoHandle +TelemetryImpl::subscribe_nic_info(const Telemetry::NicInfoCallback& callback) +{ + std::lock_guard lock(_subscription_mutex); + return _nic_info_subscriptions.subscribe(callback); +} + +void TelemetryImpl::unsubscribe_nic_info(Telemetry::NicInfoHandle handle) +{ + std::lock_guard lock(_subscription_mutex); + _nic_info_subscriptions.unsubscribe(handle); +} + Telemetry::UnixEpochTimeHandle TelemetryImpl::subscribe_unix_epoch_time(const Telemetry::UnixEpochTimeCallback& callback) { diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.h b/src/mavsdk/plugins/telemetry/telemetry_impl.h index 8253848dc1..156a64254f 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.h +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.h @@ -45,6 +45,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_battery(double rate_hz); Telemetry::Result set_rate_rc_status(double rate_hz); Telemetry::Result set_rate_cellular_status(double rate_hz); + Telemetry::Result set_rate_nic_info(double rate_hz); Telemetry::Result set_rate_actuator_control_target(double rate_hz); Telemetry::Result set_rate_actuator_output_status(double rate_hz); Telemetry::Result set_rate_odometry(double rate_hz); @@ -72,6 +73,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_battery_async(double rate_hz, Telemetry::ResultCallback callback); void set_rate_rc_status_async(double rate_hz, Telemetry::ResultCallback callback); void set_rate_cellular_status_async(double rate_hz, Telemetry::ResultCallback callback); + void set_rate_nic_info_async(double rate_hz, Telemetry::ResultCallback callback); void set_rate_actuator_control_target_async(double rate_hz, Telemetry::ResultCallback callback); void set_rate_actuator_output_status_async(double rate_hz, Telemetry::ResultCallback callback); void set_rate_odometry_async(double rate_hz, Telemetry::ResultCallback callback); @@ -110,6 +112,7 @@ class TelemetryImpl : public PluginImplBase { bool health_all_ok() const; Telemetry::RcStatus rc_status() const; Telemetry::CellularStatus cellular_status() const; + Telemetry::NicInfo nic_info() const; Telemetry::ActuatorControlTarget actuator_control_target() const; Telemetry::ActuatorOutputStatus actuator_output_status() const; Telemetry::Odometry odometry() const; @@ -188,6 +191,9 @@ class TelemetryImpl : public PluginImplBase { Telemetry::CellularStatusHandle subscribe_cellular_status(const Telemetry::CellularStatusCallback& callback); void unsubscribe_cellular_status(Telemetry::CellularStatusHandle handle); + Telemetry::NicInfoHandle + subscribe_nic_info(const Telemetry::NicInfoCallback& callback); + void unsubscribe_nic_info(Telemetry::NicInfoHandle handle); Telemetry::UnixEpochTimeHandle subscribe_unix_epoch_time(const Telemetry::UnixEpochTimeCallback& callback); void unsubscribe_unix_epoch_time(Telemetry::UnixEpochTimeHandle handle); @@ -235,6 +241,7 @@ class TelemetryImpl : public PluginImplBase { void set_raw_gps(Telemetry::RawGps raw_gps); void set_battery(Telemetry::Battery battery); void set_cellular_status(Telemetry::CellularStatus cellular_status); + void set_nic_info(Telemetry::NicInfo nic_info); void set_health_local_position(bool ok); void set_health_global_position(bool ok); void set_health_home_position(bool ok); @@ -243,8 +250,12 @@ class TelemetryImpl : public PluginImplBase { void set_health_magnetometer_calibration(bool ok); void set_health_armable(bool ok); void set_rc_status(std::optional available, std::optional signal_strength_percent); + //TODO FIX ME? void set_cellular_status( std::optional available, std::optional signal_strength_percent); + //TODO FIX ME? + void set_nic_info( + std::optional available, std::optional signal_strength_percent); void set_unix_epoch_time_us(uint64_t time_us); void set_actuator_control_target(uint8_t group, const std::vector& controls); void set_actuator_output_status(uint32_t active, const std::vector& actuators); @@ -271,6 +282,7 @@ class TelemetryImpl : public PluginImplBase { void process_sys_status(const mavlink_message_t& message); void process_battery_status(const mavlink_message_t& message); void process_cellular_status(const mavlink_message_t& message); + void process_nic_info(const mavlink_message_t& message); void process_heartbeat(const mavlink_message_t& message); void process_rc_channels(const mavlink_message_t& message); void process_unix_epoch_time(const mavlink_message_t& message); @@ -395,6 +407,9 @@ class TelemetryImpl : public PluginImplBase { mutable std::mutex _cellular_status_mutex{}; Telemetry::CellularStatus _cellular_status{}; + mutable std::mutex _nic_info_mutex{}; + Telemetry::NicInfo _nic_info{}; + mutable std::mutex _unix_epoch_time_mutex{}; uint64_t _unix_epoch_time_us{}; @@ -448,6 +463,7 @@ class TelemetryImpl : public PluginImplBase { CallbackList _landed_state_subscriptions{}; CallbackList _rc_status_subscriptions{}; CallbackList _cellular_status_subscriptions{}; + CallbackList _nic_info_subscriptions{}; CallbackList _unix_epoch_time_subscriptions{}; CallbackList _actuator_control_target_subscriptions{}; CallbackList _actuator_output_status_subscriptions{}; diff --git a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp index c33953c90f..d9a3e9da64 100644 --- a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp +++ b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp @@ -224,7 +224,7 @@ TelemetryServerImpl::publish_cellular_status(TelemetryServer::CellularStatus cel static_cast(cellular_status.instance_number), cellular_status.download_rate, cellular_status.upload_rate, - cellular_status.ber, + cellular_status.bit_error_rate, cellular_status.rx_level, cellular_status.tx_level, cellular_status.signal_to_noise, @@ -240,6 +240,31 @@ TelemetryServerImpl::publish_cellular_status(TelemetryServer::CellularStatus cel TelemetryServer::Result::Unsupported; } +TelemetryServer::Result +TelemetryServerImpl::publish_nic_info(TelemetryServer::NicInfo nic_info) +{ + mavlink_message_t msg; + + nic_info.nic_model_name.resize(sizeof(mavlink_nic_info_t::nic_model)); + + mavlink_msg_nic_info_pack( + _server_component_impl->get_own_system_id(), + _server_component_impl->get_own_component_id(), + &msg, + static_cast(nic_info.instance_number), + static_cast(nic_info.nic_id), + nic_info.nic_model_name.data(), + nic_info.imei, + nic_info.iccid, + nic_info.imsi, + nic_info.firmware_version); + + add_msg_cache(MAVLINK_MSG_ID_NIC_INFO, msg); + + return _server_component_impl->send_message(msg) ? TelemetryServer::Result::Success : + TelemetryServer::Result::Unsupported; +} + TelemetryServer::Result TelemetryServerImpl::publish_status_text(TelemetryServer::StatusText status_text) { diff --git a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h index 36cb4103e1..47d691616b 100644 --- a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h +++ b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h @@ -44,6 +44,9 @@ class TelemetryServerImpl : public ServerPluginImplBase { TelemetryServer::Result publish_cellular_status(TelemetryServer::CellularStatus cellular_status); + TelemetryServer::Result + publish_nic_info(TelemetryServer::NicInfo nic_info); + TelemetryServer::Result publish_odometry(TelemetryServer::Odometry odometry); TelemetryServer::Result