Skip to content

Commit

Permalink
add nic_info msg, update cellular_status
Browse files Browse the repository at this point in the history
  • Loading branch information
@asimopunov
asimopunov committed Jan 29, 2023
1 parent 096a284 commit dfef397
Show file tree
Hide file tree
Showing 4 changed files with 127 additions and 2 deletions.
83 changes: 82 additions & 1 deletion src/mavsdk/plugins/telemetry/telemetry_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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); },
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand All @@ -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);

{
Expand All @@ -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<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) {
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
{
Expand Down
16 changes: 16 additions & 0 deletions src/mavsdk/plugins/telemetry/telemetry_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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{};

Expand Down Expand Up @@ -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{};
Expand Down
27 changes: 26 additions & 1 deletion src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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)
{
Expand Down
3 changes: 3 additions & 0 deletions src/mavsdk/plugins/telemetry_server/telemetry_server_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit dfef397

Please sign in to comment.