wip add nic_info msg, update cellular_status

mavlink · Jan 27, 2023 · 5712b2c · 5712b2c
1 parent 096a284
commit 5712b2c
Show file tree

Hide file tree

Showing 4 changed files with 127 additions and 2 deletions.
diff --git 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 nic_info;
+    mavlink_msg_nic_info_decode(&message, &nic_info);
+
+    Telemetry::NicInfo new_nic_info;
+    new_nic_info.instance_number = nic_info.id;
+    new_nic_info.nic_id = nic_info.nic_id;
+    new_nic_info.nic_model_name = nic_info.nic_model;
+    new_nic_info.imei = nic_info.imei;
+    new_nic_info.iccid = nic_info.iccid;
+    new_nic_info.imsi = nic_info.imsi;
+    new_nic_info.firmware_version = nic_info.firmware_version;
+
+    set_nic_info(new_nic_info);
+
+    {
+        std::lock_guard<std::mutex> 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<std::mutex> lock(_nic_info_mutex);
+    return _nic_info;
+}
+
 uint64_t TelemetryImpl::unix_epoch_time() const
 {
     std::lock_guard<std::mutex> 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<std::mutex> lock(_nic_info_mutex);
+    _nic_info = nic_info;
+}
+
 void TelemetryImpl::set_unix_epoch_time_us(uint64_t time_us)
 {
     std::lock_guard<std::mutex> 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<std::mutex> lock(_subscription_mutex);
+    return _nic_info_subscriptions.subscribe(callback);
+}
+
+void TelemetryImpl::unsubscribe_nic_info(Telemetry::NicInfoHandle handle)
+{
+    std::lock_guard<std::mutex> 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
@@ -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<bool> available, std::optional<float> signal_strength_percent);
+    //TODO FIX ME?
     void set_cellular_status(
         std::optional<bool> available, std::optional<float> signal_strength_percent);
+    //TODO FIX ME?
+    void set_nic_info(
+        std::optional<bool> available, std::optional<float> signal_strength_percent);
     void set_unix_epoch_time_us(uint64_t time_us);
     void set_actuator_control_target(uint8_t group, const std::vector<float>& controls);
     void set_actuator_output_status(uint32_t active, const std::vector<float>& 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<Telemetry::LandedState> _landed_state_subscriptions{};
     CallbackList<Telemetry::RcStatus> _rc_status_subscriptions{};
     CallbackList<Telemetry::CellularStatus> _cellular_status_subscriptions{};
+    CallbackList<Telemetry::NicInfo> _nic_info_subscriptions{};
     CallbackList<uint64_t> _unix_epoch_time_subscriptions{};
     CallbackList<Telemetry::ActuatorControlTarget> _actuator_control_target_subscriptions{};
     CallbackList<Telemetry::ActuatorOutputStatus> _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
@@ -224,7 +224,7 @@ TelemetryServerImpl::publish_cellular_status(TelemetryServer::CellularStatus cel
         static_cast<uint8_t>(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<uint8_t>(nic_info.instance_number),
+        static_cast<uint8_t>(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
@@ -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