Browse Source

GCS_MAVLink: added logging of NAMED_VALUE_FLOAT

this is useful when running sensors on a companion computer and
wanting values logged in main ArduPilot log.
gps-1.3.1
Andrew Tridgell 4 years ago
parent
commit
ae615de4ce
  1. 1
      libraries/GCS_MAVLink/GCS.h
  2. 26
      libraries/GCS_MAVLink/GCS_Common.cpp

1
libraries/GCS_MAVLink/GCS.h

@ -469,6 +469,7 @@ protected: @@ -469,6 +469,7 @@ protected:
} _timesync_request;
void handle_statustext(const mavlink_message_t &msg) const;
void handle_named_value(const mavlink_message_t &msg) const;
bool telemetry_delayed() const;
virtual uint32_t telem_delay() const = 0;

26
libraries/GCS_MAVLink/GCS_Common.cpp

@ -2950,6 +2950,28 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const @@ -2950,6 +2950,28 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const
}
/*
handle logging of named values from mavlink.
*/
void GCS_MAVLINK::handle_named_value(const mavlink_message_t &msg) const
{
auto *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return;
}
mavlink_named_value_float_t p;
mavlink_msg_named_value_float_decode(&msg, &p);
char s[11] {};
strncpy(s, p.name, sizeof(s)-1);
logger->Write("NVAL", "TimeUS,TimeBootMS,Name,Value,SSys,SCom", "ss#---", "FC----", "QINfBB",
AP_HAL::micros64(),
p.time_boot_ms,
s,
p.value,
msg.sysid,
msg.compid);
}
void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg)
{
mavlink_system_time_t packet;
@ -3522,6 +3544,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) @@ -3522,6 +3544,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_LANDING_TARGET:
handle_landing_target(msg);
break;
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
handle_named_value(msg);
break;
}
}

Loading…
Cancel
Save