From fa5e62d9e2c68acc31c182cb5bf4d1080243e85f Mon Sep 17 00:00:00 2001 From: Adrian Del Grosso <10929341+ad3154@users.noreply.github.com> Date: Thu, 13 Jul 2023 18:35:43 -0600 Subject: [PATCH] [NMEA2K]: Finished NMEA2000 message interface --- .../isobus/nmea2000_message_definitions.hpp | 54 +- .../isobus/nmea2000_message_interface.hpp | 8 +- isobus/src/nmea2000_message_definitions.cpp | 140 +++-- isobus/src/nmea2000_message_interface.cpp | 41 +- test/nmea2000_message_tests.cpp | 571 +++++++++++++++++- 5 files changed, 731 insertions(+), 83 deletions(-) diff --git a/isobus/include/isobus/isobus/nmea2000_message_definitions.hpp b/isobus/include/isobus/isobus/nmea2000_message_definitions.hpp index bc7aed63..8b029919 100644 --- a/isobus/include/isobus/isobus/nmea2000_message_definitions.hpp +++ b/isobus/include/isobus/isobus/nmea2000_message_definitions.hpp @@ -113,7 +113,7 @@ namespace isobus /// @brief Takes the current state of the object and serializes it into a buffer to be sent. /// @param[in] buffer A vector to populate with the message data - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing @@ -181,7 +181,7 @@ namespace isobus /// @brief Serializes the current state of this object into a buffer to be sent on the CAN bus /// @param[in] buffer A buffer to serialize the message data into - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing @@ -250,7 +250,7 @@ namespace isobus /// @brief Serializes the current state of this object into a buffer to be sent on the CAN bus /// @param[in] buffer A buffer to serialize the message data into - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing @@ -344,7 +344,7 @@ namespace isobus /// @brief Serializes the current state of this object into a buffer to be sent on the CAN bus /// @param[in] buffer A buffer to serialize the message data into - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing @@ -368,6 +368,7 @@ namespace isobus /// @brief This message is a way for a GNSS receiver to provide a current position without using fast packet based on /// The content of the last position data combined from the GNSS Position Data message and any prior position delta messages. + /// This PGN provides latitude and longitude referenced to WGS84. class PositionDeltaHighPrecisionRapidUpdate { public: @@ -438,7 +439,7 @@ namespace isobus /// @brief Serializes the current state of this object into a buffer to be sent on the CAN bus /// @param[in] buffer A buffer to serialize the message data into - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing @@ -550,10 +551,15 @@ namespace isobus /// @returns True if the value that was set differed from the stored value, otherwise false bool set_longitude(std::int64_t longitudeToSet); - /// @brief Returns the geoidal separation @todo units? + /// @brief Returns the geoidal separation in units of 0.01 meters /// @details This returns the difference between the earth ellipsoid and mean-sea-level (geoid) defined by the reference datum - /// @returns The geoidal separation - std::int32_t get_geoidal_separation() const; + /// @returns The geoidal separation in units of 0.01m + std::int32_t get_raw_geoidal_separation() const; + + /// @brief Returns the geoidal separation in units of meters + /// @details This returns the difference between the earth ellipsoid and mean-sea-level (geoid) defined by the reference datum + /// @returns The geoidal separation in units of meters + float get_geoidal_separation() const; /// @brief Sets the geoidal separation /// @details This value is the difference between the earth ellipsoid and mean-sea-level (geoid) defined by the reference datum @@ -643,6 +649,28 @@ namespace isobus /// @returns True if the value that was set differed from the stored value, otherwise false bool set_number_of_reference_stations(std::uint8_t stations); + /// @brief Returns the specified reference station's ID by index + /// @param[in] index The index of the reference station to get the ID of + /// @returns Reference station's ID by index + std::uint16_t get_reference_station_id(std::size_t index) const; + + /// @brief Returns the specified reference station's DGNSS corrections age by index + /// @prarm[in] index The index of the reference station to get the DGNSS corrections age for + /// @returns Reference station's DGNSS corrections age by index + std::uint16_t get_reference_station_corrections_age(std::size_t index) const; + + /// @brief Returns the specified reference station's system type by index + /// @param[in] index The index of the reference station to get the system type for + /// @returns The specified reference station's system type by index + TypeOfSystem get_reference_station_system_type(std::size_t index) const; + + /// @brief Sets the data for the specified reference station by index + /// @param[in] index The index of the reference station to set + /// @param[in] ID The station ID to set + /// @param[in] type The type of reference station + /// @param[in] ageOfCorrections Age of the DGNSS corrections in units of 0.01 seconds + bool set_reference_station(std::size_t index, std::uint16_t ID, TypeOfSystem type, std::uint16_t ageOfCorrections); + /// @brief Returns the date associated with the current position. /// @returns Number of days relative to UTC since Jan 1 1970 (0 is equal to Jan 1, 1970). Max value is 65532 days. std::uint16_t get_position_date() const; @@ -654,16 +682,16 @@ namespace isobus /// @brief Returns the number of seconds since midnight /// @returns Number of seconds since midnight (0 == midnight), range allows for up to two leap seconds per day - std::uint16_t get_position_time() const; + std::uint32_t get_position_time() const; /// @brief Sets the number of seconds since midnight /// @param[in] timeToSet Seconds since midnight (0 == midnight), range allows for up to two leap seconds per day /// @returns True if the value that was set differed from the stored value, otherwise false - bool set_position_time(std::uint16_t timeToSet); + bool set_position_time(std::uint32_t timeToSet); /// @brief Serializes the current state of this object into a buffer to be sent on the CAN bus /// @param[in] buffer A buffer to serialize the message data into - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing @@ -715,6 +743,8 @@ namespace isobus /// @brief A NMEA2000 message that describes datum (reference frame) information. PGN 129044 (0x1F814) /// A common one might be the WGS84 datum or the NSRS, for example. + /// @details This provides local geodetic datum and datum offsets from a reference datum. + /// This PGN is used to define the datum to which a position location output by the same device in other PGNs is referenced. class Datum { public: @@ -792,7 +822,7 @@ namespace isobus /// @brief Serializes the current state of this object into a buffer to be sent on the CAN bus /// @param[in] buffer A buffer to serialize the message data into - void serialize(std::vector &buffer); + void serialize(std::vector &buffer) const; /// @brief Deserializes a CAN message to populate this object's contents. Updates the timestamp when called. /// @param[in] receivedMessage The CAN message to parse when deserializing diff --git a/isobus/include/isobus/isobus/nmea2000_message_interface.hpp b/isobus/include/isobus/isobus/nmea2000_message_interface.hpp index a418b9ea..8ac5b3c6 100644 --- a/isobus/include/isobus/isobus/nmea2000_message_interface.hpp +++ b/isobus/include/isobus/isobus/nmea2000_message_interface.hpp @@ -63,12 +63,12 @@ namespace isobus /// @brief Returns a GNSSPositionData object that you can use to /// set the message's individual signal values, which will then be transmitted if the interface is configured to do so. /// @returns GNSSPositionData used to set the individual signal values sent in the GNSS position data message - NMEA2000Messages::GNSSPositionData &gnss_position_data_transmit_message(); + NMEA2000Messages::GNSSPositionData &get_gnss_position_data_transmit_message(); /// @brief Returns a PositionDeltaHighPrecisionRapidUpdate object that you can use to /// set the message's individual signal values, which will then be transmitted if the interface is configured to do so. /// @returns PositionDeltaHighPrecisionRapidUpdate used to set the individual signal values sent in the position delta message - NMEA2000Messages::PositionDeltaHighPrecisionRapidUpdate &position_delta_high_precision_rapid_update_transmit_message(); + NMEA2000Messages::PositionDeltaHighPrecisionRapidUpdate &get_position_delta_high_precision_rapid_update_transmit_message(); /// @brief Returns a PositionRapidUpdate object that you can use to /// set the message's individual signal values, which will then be transmitted if the interface is configured to do so. @@ -78,12 +78,12 @@ namespace isobus /// @brief Returns a RateOfTurn object that you can use to /// set the message's individual signal values, which will then be transmitted if the interface is configured to do so. /// @returns RateOfTurn used to set the individual signal values sent in the rate of turn message - NMEA2000Messages::RateOfTurn &rate_of_turn_transmit_message(); + NMEA2000Messages::RateOfTurn &get_rate_of_turn_transmit_message(); /// @brief Returns a VesselHeading object that you can use to /// set the message's individual signal values, which will then be transmitted if the interface is configured to do so. /// @returns VesselHeading used to set the individual signal values sent in the vessel heading message - NMEA2000Messages::VesselHeading &vessel_heading_transmit_message(); + NMEA2000Messages::VesselHeading &get_vessel_heading_transmit_message(); /// @brief Returns the number of unique senders of the COG & SOG message /// @returns The number of unique COG & SOG message senders diff --git a/isobus/src/nmea2000_message_definitions.cpp b/isobus/src/nmea2000_message_definitions.cpp index 4177ae93..cf676631 100644 --- a/isobus/src/nmea2000_message_definitions.cpp +++ b/isobus/src/nmea2000_message_definitions.cpp @@ -50,7 +50,7 @@ namespace isobus float VesselHeading::get_heading() const { - return (headingReading * 0.0001f); + return (headingReading * 10E-4f); } bool VesselHeading::set_heading(std::uint16_t heading) @@ -67,7 +67,7 @@ namespace isobus float VesselHeading::get_magnetic_deviation() const { - return (magneticDeviation * 0.0001f); + return (magneticDeviation * 10E-4f); } bool VesselHeading::set_magnetic_deviation(std::uint16_t deviation) @@ -84,7 +84,7 @@ namespace isobus float VesselHeading::get_magnetic_variation() const { - return (magneticVariation * 0.0001f); + return (magneticVariation * 10E-4f); } bool VesselHeading::set_magnetic_variation(std::int16_t variation) @@ -118,7 +118,7 @@ namespace isobus return retVal; } - void VesselHeading::serialize(std::vector &buffer) + void VesselHeading::serialize(std::vector &buffer) const { buffer.resize(CAN_DATA_LENGTH); buffer.at(0) = (sequenceID <= MAX_SEQUENCE_ID) ? sequenceID : 0xFF; @@ -186,7 +186,7 @@ namespace isobus double RateOfTurn::get_rate_of_turn() const { - return (rateOfTurn * (1.0 / 32.0 * 10E-6)); + return (static_cast(rateOfTurn) * (1.0 / 32.0 * 10E-6)); } bool RateOfTurn::set_rate_of_turn(std::int32_t turnRate) @@ -208,7 +208,7 @@ namespace isobus return retVal; } - void RateOfTurn::serialize(std::vector &buffer) + void RateOfTurn::serialize(std::vector &buffer) const { buffer.resize(CAN_DATA_LENGTH); @@ -228,7 +228,7 @@ namespace isobus if (CAN_DATA_LENGTH == receivedMessage.get_data_length()) { - std::int32_t turnRate = static_cast(receivedMessage.get_uint8_at(1)); + auto turnRate = static_cast(receivedMessage.get_uint8_at(1)); turnRate |= (static_cast(receivedMessage.get_uint8_at(2)) << 8); turnRate |= (static_cast(receivedMessage.get_uint8_at(3)) << 16); turnRate |= (static_cast(receivedMessage.get_uint8_at(4)) << 24); @@ -277,12 +277,12 @@ namespace isobus double PositionRapidUpdate::get_latitude() const { - return (latitude * 10E-7); + return (static_cast(latitude) * 10E-7); } double PositionRapidUpdate::get_longitude() const { - return (longitude * 10E-7); + return (static_cast(longitude) * 10E-7); } std::int32_t PositionRapidUpdate::get_raw_longitude() const @@ -304,7 +304,7 @@ namespace isobus return retVal; } - void PositionRapidUpdate::serialize(std::vector &buffer) + void PositionRapidUpdate::serialize(std::vector &buffer) const { buffer.resize(CAN_DATA_LENGTH); @@ -324,11 +324,11 @@ namespace isobus if (CAN_DATA_LENGTH == receivedMessage.get_data_length()) { - std::int32_t decodedLatitude = static_cast(receivedMessage.get_uint8_at(0)); + auto decodedLatitude = static_cast(receivedMessage.get_uint8_at(0)); decodedLatitude |= (static_cast(receivedMessage.get_uint8_at(1)) << 8); decodedLatitude |= (static_cast(receivedMessage.get_uint8_at(2)) << 16); decodedLatitude |= (static_cast(receivedMessage.get_uint8_at(3)) << 24); - std::int32_t decodedLongitude = static_cast(receivedMessage.get_uint8_at(4)); + auto decodedLongitude = static_cast(receivedMessage.get_uint8_at(4)); decodedLongitude |= (static_cast(receivedMessage.get_uint8_at(5)) << 8); decodedLongitude |= (static_cast(receivedMessage.get_uint8_at(6)) << 16); decodedLongitude |= (static_cast(receivedMessage.get_uint8_at(7)) << 24); @@ -377,7 +377,7 @@ namespace isobus float CourseOverGroundSpeedOverGroundRapidUpdate::get_course_over_ground() const { - return (0.0001f * courseOverGround); + return (10E-4f * courseOverGround); } bool CourseOverGroundSpeedOverGroundRapidUpdate::set_course_over_ground(std::uint16_t course) @@ -394,7 +394,7 @@ namespace isobus float CourseOverGroundSpeedOverGroundRapidUpdate::get_speed_over_ground() const { - return (0.01f * speedOverGround); + return (10E-2f * speedOverGround); } bool CourseOverGroundSpeedOverGroundRapidUpdate::set_speed_over_ground(std::uint16_t speed) @@ -428,7 +428,7 @@ namespace isobus return retVal; } - void CourseOverGroundSpeedOverGroundRapidUpdate::serialize(std::vector &buffer) + void CourseOverGroundSpeedOverGroundRapidUpdate::serialize(std::vector &buffer) const { buffer.resize(CAN_DATA_LENGTH); @@ -495,7 +495,7 @@ namespace isobus double PositionDeltaHighPrecisionRapidUpdate::get_latitude_delta() const { - return (latitudeDelta * 10E-16); + return (static_cast(latitudeDelta) * 10E-7); } bool PositionDeltaHighPrecisionRapidUpdate::set_latitude_delta(std::int32_t delta) @@ -512,7 +512,7 @@ namespace isobus double PositionDeltaHighPrecisionRapidUpdate::get_longitude_delta() const { - return (longitudeDelta * 10E-16); + return (static_cast(longitudeDelta) * 10E-7); } bool PositionDeltaHighPrecisionRapidUpdate::set_longitude_delta(std::int32_t delta) @@ -541,7 +541,7 @@ namespace isobus double PositionDeltaHighPrecisionRapidUpdate::get_time_delta() const { - return (timeDelta * (5 * 10E-3)); + return ((static_cast(timeDelta) * 5.0) / 1000.0); } bool PositionDeltaHighPrecisionRapidUpdate::set_time_delta(std::uint8_t delta) @@ -551,18 +551,36 @@ namespace isobus return retVal; } - void PositionDeltaHighPrecisionRapidUpdate::serialize(std::vector &buffer) + void PositionDeltaHighPrecisionRapidUpdate::serialize(std::vector &buffer) const { buffer.resize(CAN_DATA_LENGTH); buffer.at(0) = sequenceID; - //! @todo Finish serializer + buffer.at(1) = timeDelta; + buffer.at(2) = static_cast(latitudeDelta & 0xFF); + buffer.at(3) = static_cast((latitudeDelta >> 8) & 0xFF); + buffer.at(4) = static_cast((latitudeDelta >> 16) & 0xFF); + buffer.at(5) = static_cast(longitudeDelta & 0xFF); + buffer.at(6) = static_cast((longitudeDelta >> 8) & 0xFF); + buffer.at(7) = static_cast((longitudeDelta >> 16) & 0xFF); } bool PositionDeltaHighPrecisionRapidUpdate::deserialize(const CANMessage &receivedMessage) { bool retVal = false; - //! @todo Finish deserializer + + if (CAN_DATA_LENGTH == receivedMessage.get_data_length()) + { + retVal = set_sequence_id(receivedMessage.get_uint8_at(0)); + retVal |= set_time_delta(receivedMessage.get_uint8_at(1)); + retVal |= set_latitude_delta(receivedMessage.get_uint24_at(2)); + retVal |= set_longitude_delta(receivedMessage.get_uint24_at(5)); + retVal |= set_timestamp(SystemTiming::get_timestamp_ms()); + } + else + { + CANStackLogger::warn("[NMEA2K]: Cannot deserialize position delta high precision rapid update. DLC must be 8 bytes."); + } return retVal; } @@ -588,7 +606,7 @@ namespace isobus double GNSSPositionData::get_altitude() const { - return (altitude * 10E-6); + return (static_cast(altitude) * 1E-6); } bool GNSSPositionData::set_altitude(std::int64_t altitudeToSet) @@ -605,7 +623,7 @@ namespace isobus double GNSSPositionData::get_latitude() const { - return (latitude * 10E-16); + return (static_cast(latitude) * 1E-16); } bool GNSSPositionData::set_latitude(std::int64_t latitudeToSet) @@ -622,7 +640,7 @@ namespace isobus double GNSSPositionData::get_longitude() const { - return (longitude * 10E-16); + return (static_cast(longitude) * 1E-16); } bool GNSSPositionData::set_longitude(std::int64_t longitudeToSet) @@ -632,11 +650,16 @@ namespace isobus return retVal; } - std::int32_t GNSSPositionData::get_geoidal_separation() const + std::int32_t GNSSPositionData::get_raw_geoidal_separation() const { return geoidalSeparation; } + float GNSSPositionData::get_geoidal_separation() const + { + return (geoidalSeparation * 0.01f); + } + bool GNSSPositionData::set_geoidal_separation(std::int32_t separation) { bool retVal = (geoidalSeparation != separation); @@ -752,6 +775,53 @@ namespace isobus return retVal; } + std::uint16_t GNSSPositionData::get_reference_station_id(std::size_t index) const + { + std::uint16_t retVal = 0; + + if (index < referenceStations.size()) + { + retVal = referenceStations.at(index).stationID; + } + return retVal; + } + + std::uint16_t GNSSPositionData::get_reference_station_corrections_age(std::size_t index) const + { + std::uint16_t retVal = 0; + + if (index < referenceStations.size()) + { + retVal = referenceStations.at(index).ageOfDGNSSCorrections; + } + return retVal; + } + + GNSSPositionData::TypeOfSystem GNSSPositionData::get_reference_station_system_type(std::size_t index) const + { + TypeOfSystem retVal = TypeOfSystem::Null; + + if (index < referenceStations.size()) + { + retVal = referenceStations.at(index).stationType; + } + return retVal; + } + + bool GNSSPositionData::set_reference_station(std::size_t index, std::uint16_t ID, TypeOfSystem type, std::uint16_t ageOfCorrections) + { + bool retVal = false; + + if (index < referenceStations.size()) + { + retVal |= referenceStations.at(index).ageOfDGNSSCorrections != ageOfCorrections; + retVal |= referenceStations.at(index).stationID != ID; + retVal |= referenceStations.at(index).stationType != type; + referenceStations.at(index) = ReferenceStationData(ID, type, ageOfCorrections); + } + return retVal; + } + std::uint16_t GNSSPositionData::get_position_date() const { return positionDate; @@ -764,19 +834,19 @@ namespace isobus return retVal; } - std::uint16_t GNSSPositionData::get_position_time() const + std::uint32_t GNSSPositionData::get_position_time() const { return positionTime; } - bool GNSSPositionData::set_position_time(std::uint16_t timeToSet) + bool GNSSPositionData::set_position_time(std::uint32_t timeToSet) { bool retVal = (positionTime != timeToSet); positionTime = timeToSet; return retVal; } - void GNSSPositionData::serialize(std::vector &buffer) + void GNSSPositionData::serialize(std::vector &buffer) const { buffer.resize(MINIMUM_LENGTH_BYTES); @@ -860,9 +930,11 @@ namespace isobus for (std::uint8_t i = 0; i < get_number_of_reference_stations(); i++) { - if (receivedMessage.get_data_length() >= MINIMUM_LENGTH_BYTES + (i * 4)) + if (receivedMessage.get_data_length() >= static_cast(MINIMUM_LENGTH_BYTES + (i * 4))) { - referenceStations.push_back(std::move(ReferenceStationData((receivedMessage.get_uint16_at(MINIMUM_LENGTH_BYTES + (i * 4)) >> 4), static_cast(receivedMessage.get_uint8_at(MINIMUM_LENGTH_BYTES + (i * 4)) & 0x0F), receivedMessage.get_uint16_at(2 + MINIMUM_LENGTH_BYTES + (i * 4))))); + referenceStations.at(i) = std::move(ReferenceStationData((receivedMessage.get_uint16_at(MINIMUM_LENGTH_BYTES + (i * 4)) >> 4), + static_cast(receivedMessage.get_uint8_at(MINIMUM_LENGTH_BYTES + (i * 4)) & 0x0F), + receivedMessage.get_uint16_at(2 + MINIMUM_LENGTH_BYTES + (i * 4)))); } else { @@ -955,7 +1027,7 @@ namespace isobus double Datum::get_delta_latitude() const { - return (deltaLatitude * 10E-7); + return (static_cast(deltaLatitude) * 10E-7); } bool Datum::set_delta_latitude(std::int32_t delta) @@ -967,7 +1039,7 @@ namespace isobus double Datum::get_delta_longitude() const { - return (deltaLongitude * 10E-7); + return (static_cast(deltaLongitude) * 10E-7); } std::int32_t Datum::get_raw_delta_longitude() const @@ -989,7 +1061,7 @@ namespace isobus float Datum::get_delta_altitude() const { - return (0.02f * deltaAltitude); + return (10E-2f * deltaAltitude); } bool Datum::set_delta_altitude(std::int32_t delta) @@ -999,7 +1071,7 @@ namespace isobus return retVal; } - void Datum::serialize(std::vector &buffer) + void Datum::serialize(std::vector &buffer) const { buffer.resize(LENGTH_BYTES); diff --git a/isobus/src/nmea2000_message_interface.cpp b/isobus/src/nmea2000_message_interface.cpp index edaa6e9e..ce379344 100644 --- a/isobus/src/nmea2000_message_interface.cpp +++ b/isobus/src/nmea2000_message_interface.cpp @@ -20,6 +20,7 @@ #include "isobus/isobus/can_general_parameter_group_numbers.hpp" #include "isobus/isobus/can_network_manager.hpp" #include "isobus/isobus/can_stack_logger.hpp" +#include "isobus/isobus/nmea2000_fast_packet_protocol.hpp" #include "isobus/utility/system_timing.hpp" #include @@ -69,12 +70,12 @@ namespace isobus return datumTransmitMessage; } - NMEA2000Messages::GNSSPositionData &NMEA2000MessageInterface::gnss_position_data_transmit_message() + NMEA2000Messages::GNSSPositionData &NMEA2000MessageInterface::get_gnss_position_data_transmit_message() { return gnssPositionDataTransmitMessage; } - NMEA2000Messages::PositionDeltaHighPrecisionRapidUpdate &NMEA2000MessageInterface::position_delta_high_precision_rapid_update_transmit_message() + NMEA2000Messages::PositionDeltaHighPrecisionRapidUpdate &NMEA2000MessageInterface::get_position_delta_high_precision_rapid_update_transmit_message() { return positionDeltaHighPrecisionRapidUpdateTransmitMessage; } @@ -84,12 +85,12 @@ namespace isobus return positionRapidUpdateTransmitMessage; } - NMEA2000Messages::RateOfTurn &NMEA2000MessageInterface::rate_of_turn_transmit_message() + NMEA2000Messages::RateOfTurn &NMEA2000MessageInterface::get_rate_of_turn_transmit_message() { return rateOfTurnTransmitMessage; } - NMEA2000Messages::VesselHeading &NMEA2000MessageInterface::vessel_heading_transmit_message() + NMEA2000Messages::VesselHeading &NMEA2000MessageInterface::get_vessel_heading_transmit_message() { return vesselHeadingTransmitMessage; } @@ -315,9 +316,9 @@ namespace isobus { if (!initialized) { + CANNetworkManager::CANNetwork.get_fast_packet_protocol().register_multipacket_message_callback(static_cast(CANLibParameterGroupNumber::Datum), process_rx_message, this); + CANNetworkManager::CANNetwork.get_fast_packet_protocol().register_multipacket_message_callback(static_cast(CANLibParameterGroupNumber::GNSSPositionData), process_rx_message, this); CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::CourseOverGroundSpeedOverGroundRapidUpdate), process_rx_message, this); - CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::Datum), process_rx_message, this); - CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::GNSSPositionData), process_rx_message, this); CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::PositionDeltaHighPrecisionRapidUpdate), process_rx_message, this); CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::PositionRapidUpdate), process_rx_message, this); CANNetworkManager::CANNetwork.add_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::RateOfTurn), process_rx_message, this); @@ -335,9 +336,9 @@ namespace isobus { if (initialized) { + CANNetworkManager::CANNetwork.get_fast_packet_protocol().remove_multipacket_message_callback(static_cast(CANLibParameterGroupNumber::Datum), process_rx_message, this); + CANNetworkManager::CANNetwork.get_fast_packet_protocol().remove_multipacket_message_callback(static_cast(CANLibParameterGroupNumber::GNSSPositionData), process_rx_message, this); CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::CourseOverGroundSpeedOverGroundRapidUpdate), process_rx_message, this); - CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::Datum), process_rx_message, this); - CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::GNSSPositionData), process_rx_message, this); CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::PositionDeltaHighPrecisionRapidUpdate), process_rx_message, this); CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::PositionRapidUpdate), process_rx_message, this); CANNetworkManager::CANNetwork.remove_any_control_function_parameter_group_number_callback(static_cast(CANLibParameterGroupNumber::RateOfTurn), process_rx_message, this); @@ -391,12 +392,12 @@ namespace isobus if (nullptr != targetInterface->datumTransmitMessage.get_control_function()) { targetInterface->datumTransmitMessage.serialize(messageBuffer); - transmitSuccessful = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::Datum), - messageBuffer.data(), - messageBuffer.size(), - std::dynamic_pointer_cast(targetInterface->datumTransmitMessage.get_control_function()), - nullptr, - CANIdentifier::PriorityDefault6); + transmitSuccessful = CANNetworkManager::CANNetwork.get_fast_packet_protocol().send_multipacket_message(static_cast(CANLibParameterGroupNumber::Datum), + messageBuffer.data(), + messageBuffer.size(), + std::dynamic_pointer_cast(targetInterface->datumTransmitMessage.get_control_function()), + nullptr, + CANIdentifier::PriorityDefault6); } } break; @@ -406,12 +407,12 @@ namespace isobus if (nullptr != targetInterface->gnssPositionDataTransmitMessage.get_control_function()) { targetInterface->gnssPositionDataTransmitMessage.serialize(messageBuffer); - transmitSuccessful = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::GNSSPositionData), - messageBuffer.data(), - messageBuffer.size(), - std::dynamic_pointer_cast(targetInterface->gnssPositionDataTransmitMessage.get_control_function()), - nullptr, - CANIdentifier::Priority3); + transmitSuccessful = CANNetworkManager::CANNetwork.get_fast_packet_protocol().send_multipacket_message(static_cast(CANLibParameterGroupNumber::GNSSPositionData), + messageBuffer.data(), + messageBuffer.size(), + std::dynamic_pointer_cast(targetInterface->gnssPositionDataTransmitMessage.get_control_function()), + nullptr, + CANIdentifier::Priority3); } } break; diff --git a/test/nmea2000_message_tests.cpp b/test/nmea2000_message_tests.cpp index 6bacbe24..af2c87cc 100644 --- a/test/nmea2000_message_tests.cpp +++ b/test/nmea2000_message_tests.cpp @@ -17,6 +17,48 @@ static void test_cog_sog_callback(const std::shared_ptr, bool) +{ + wasDatumCallbackHit = true; +} + +static bool wasGNSSPositionDataCallbackHit = false; + +static void test_gnss_position_data_callback(const std::shared_ptr, bool) +{ + wasGNSSPositionDataCallbackHit = true; +} + +static bool wasPositionRapidUpdateCallbackHit = false; + +static void test_position_rapid_update_callback(const std::shared_ptr, bool) +{ + wasPositionRapidUpdateCallbackHit = true; +} + +static bool wasPositionDeltaHighSpeedRapidUpdateCallbackHit = false; + +static void test_position_delta_high_speed_rapid_update_callback(const std::shared_ptr, bool) +{ + wasPositionDeltaHighSpeedRapidUpdateCallbackHit = true; +} + +static bool wasRateOfTurnCallbackHit = false; + +static void test_rate_of_turn_callback(const std::shared_ptr, bool) +{ + wasRateOfTurnCallbackHit = true; +} + +static bool wasVesselHeadingCallbackHit = false; + +static void test_vessel_heading_callback(const std::shared_ptr, bool) +{ + wasVesselHeadingCallbackHit = true; +} + TEST(NMEA2000_TESTS, VesselHeadingDataInterface) { VesselHeading messageDataUnderTest(nullptr); @@ -35,12 +77,12 @@ TEST(NMEA2000_TESTS, VesselHeadingDataInterface) EXPECT_FALSE(messageDataUnderTest.set_sequence_id(4)); EXPECT_FALSE(messageDataUnderTest.set_timestamp(5)); - EXPECT_NEAR(0.0001f, messageDataUnderTest.get_heading(), 0.00005f); + EXPECT_NEAR(0.001f, messageDataUnderTest.get_heading(), 0.00005f); EXPECT_EQ(1, messageDataUnderTest.get_raw_heading()); EXPECT_EQ(2, messageDataUnderTest.get_raw_magnetic_deviation()); - EXPECT_NEAR(0.0002f, messageDataUnderTest.get_magnetic_deviation(), 0.00005f); + EXPECT_NEAR(0.002f, messageDataUnderTest.get_magnetic_deviation(), 0.00005f); EXPECT_EQ(-3, messageDataUnderTest.get_raw_magnetic_variation()); - EXPECT_NEAR(-0.0003f, messageDataUnderTest.get_magnetic_variation(), 0.00005f); + EXPECT_NEAR(-0.003f, messageDataUnderTest.get_magnetic_variation(), 0.00005f); EXPECT_EQ(VesselHeading::HeadingSensorReference::True, messageDataUnderTest.get_sensor_reference()); EXPECT_EQ(4, messageDataUnderTest.get_sequence_id()); EXPECT_EQ(5, messageDataUnderTest.get_timestamp()); @@ -110,11 +152,28 @@ TEST(NMEA2000_TESTS, PositionRapidUpdateDataInterface) EXPECT_EQ(1000, messageDataUnderTest.get_raw_latitude()); EXPECT_EQ(2000, messageDataUnderTest.get_raw_longitude()); + EXPECT_NEAR(1000 * 10E-7, messageDataUnderTest.get_latitude(), 0.000001); + EXPECT_NEAR(2000 * 10E-7, messageDataUnderTest.get_longitude(), 0.000001); EXPECT_EQ(3000, messageDataUnderTest.get_timestamp()); EXPECT_EQ(nullptr, messageDataUnderTest.get_control_function()); std::vector serializationBuffer; EXPECT_NO_THROW(messageDataUnderTest.serialize(serializationBuffer)); + + ASSERT_EQ(CAN_DATA_LENGTH, serializationBuffer.size()); + + std::int32_t latitude = static_cast(serializationBuffer.at(0)) | + (static_cast(serializationBuffer.at(1)) << 8) | + (static_cast(serializationBuffer.at(2)) << 16) | + (static_cast(serializationBuffer.at(3)) << 24); + + std::int32_t longitude = static_cast(serializationBuffer.at(4)) | + (static_cast(serializationBuffer.at(5)) << 8) | + (static_cast(serializationBuffer.at(6)) << 16) | + (static_cast(serializationBuffer.at(7)) << 24); + + EXPECT_EQ(latitude, 1000); + EXPECT_EQ(longitude, 2000); } TEST(NMEA2000_TESTS, CourseOverGroundSpeedOverGroundRapidUpdateDataInterface) @@ -134,12 +193,31 @@ TEST(NMEA2000_TESTS, CourseOverGroundSpeedOverGroundRapidUpdateDataInterface) EXPECT_FALSE(messageDataUnderTest.set_timestamp(87)); EXPECT_EQ(50, messageDataUnderTest.get_raw_course_over_ground()); - EXPECT_NEAR(50 * 0.0001, messageDataUnderTest.get_course_over_ground(), 0.00005); + EXPECT_NEAR(50 * 10E-4f, messageDataUnderTest.get_course_over_ground(), 0.00005); EXPECT_EQ(CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroudReference::Magnetic, messageDataUnderTest.get_course_over_ground_reference()); EXPECT_EQ(9, messageDataUnderTest.get_sequence_id()); EXPECT_EQ(75, messageDataUnderTest.get_raw_speed_over_ground()); EXPECT_EQ(87, messageDataUnderTest.get_timestamp()); EXPECT_EQ(nullptr, messageDataUnderTest.get_control_function()); + + std::vector serializationBuffer; + EXPECT_NO_THROW(messageDataUnderTest.serialize(serializationBuffer)); + + ASSERT_EQ(CAN_DATA_LENGTH, serializationBuffer.size()); + + EXPECT_EQ(9, serializationBuffer.at(0)); // Seq + EXPECT_EQ(1, serializationBuffer.at(1) & 0x03); // Ref + + std::uint16_t course = static_cast(serializationBuffer.at(2)) | + (static_cast(serializationBuffer.at(3)) << 8); + EXPECT_EQ(course, 50); + + std::uint16_t speed = static_cast(serializationBuffer.at(4)) | + (static_cast(serializationBuffer.at(5)) << 8); + EXPECT_EQ(speed, 75); + + EXPECT_EQ(0xFF, serializationBuffer.at(6)); + EXPECT_EQ(0xFF, serializationBuffer.at(7)); } TEST(NMEA2000_Tests, PositionDeltaHighPrecisionRapidUpdateDataInterface) @@ -159,15 +237,42 @@ TEST(NMEA2000_Tests, PositionDeltaHighPrecisionRapidUpdateDataInterface) EXPECT_FALSE(messageDataUnderTest.set_time_delta(7)); EXPECT_EQ(nullptr, messageDataUnderTest.get_control_function()); - EXPECT_EQ(-5000 * 10E-16, messageDataUnderTest.get_latitude_delta()); - EXPECT_EQ(-9000 * 10E-16, messageDataUnderTest.get_longitude_delta()); + EXPECT_EQ(-5000 * 1E-6, messageDataUnderTest.get_latitude_delta()); + EXPECT_EQ(-9000 * 1E-6, messageDataUnderTest.get_longitude_delta()); EXPECT_EQ(-5000, messageDataUnderTest.get_raw_latitude_delta()); EXPECT_EQ(-9000, messageDataUnderTest.get_raw_longitude_delta()); EXPECT_EQ(7, messageDataUnderTest.get_raw_time_delta()); - EXPECT_EQ(7 * (5 * 10E-3), messageDataUnderTest.get_time_delta()); + EXPECT_NEAR(0.007 * 5, messageDataUnderTest.get_time_delta(), 0.0001); + EXPECT_EQ(49, messageDataUnderTest.get_sequence_id()); std::vector messageBuffer; EXPECT_NO_THROW(messageDataUnderTest.serialize(messageBuffer)); + + ASSERT_EQ(CAN_DATA_LENGTH, messageBuffer.size()); + EXPECT_EQ(49, messageBuffer.at(0)); + EXPECT_EQ(7, messageBuffer.at(1)); + + std::int32_t deltaLatitude = static_cast(messageBuffer.at(2)) | + (static_cast(messageBuffer.at(3)) << 8) | + (static_cast(messageBuffer.at(4)) << 16); + + // Need to sign extend the value... + if (messageBuffer.at(4) & 0x80) + { + deltaLatitude |= static_cast(0xFF000000); + } + EXPECT_EQ(-5000, deltaLatitude); + + std::int32_t deltaLongitude = static_cast(messageBuffer.at(5)) | + (static_cast(messageBuffer.at(6)) << 8) | + (static_cast(messageBuffer.at(7)) << 16); + + // Need to sign extend the value... + if (messageBuffer.at(7) & 0x80) + { + deltaLongitude |= static_cast(0xFF000000); + } + EXPECT_EQ(-9000, deltaLongitude); } TEST(NMEA2000_Tests, GNSSPositionDataDataInterface) @@ -187,6 +292,9 @@ TEST(NMEA2000_Tests, GNSSPositionDataDataInterface) EXPECT_TRUE(messageDataUnderTest.set_altitude(5820000000)); EXPECT_TRUE(messageDataUnderTest.set_latitude(-72057594037298808)); EXPECT_TRUE(messageDataUnderTest.set_longitude(720575)); + EXPECT_TRUE(messageDataUnderTest.set_position_date(19551)); + EXPECT_TRUE(messageDataUnderTest.set_position_time(86400)); + EXPECT_TRUE(messageDataUnderTest.set_reference_station(0, 4, GNSSPositionData::TypeOfSystem::Galileo, 100)); EXPECT_FALSE(messageDataUnderTest.set_geoidal_separation(10000)); EXPECT_FALSE(messageDataUnderTest.set_gnss_method(GNSSPositionData::GNSSMethod::RTKFixedInteger)); @@ -201,9 +309,12 @@ TEST(NMEA2000_Tests, GNSSPositionDataDataInterface) EXPECT_FALSE(messageDataUnderTest.set_altitude(5820000000)); EXPECT_FALSE(messageDataUnderTest.set_latitude(-72057594037298808)); EXPECT_FALSE(messageDataUnderTest.set_longitude(720575)); + EXPECT_FALSE(messageDataUnderTest.set_position_date(19551)); + EXPECT_FALSE(messageDataUnderTest.set_position_time(86400)); + EXPECT_FALSE(messageDataUnderTest.set_reference_station(0, 4, GNSSPositionData::TypeOfSystem::Galileo, 100)); EXPECT_EQ(nullptr, messageDataUnderTest.get_control_function()); - EXPECT_EQ(10000, messageDataUnderTest.get_geoidal_separation()); + EXPECT_EQ(10000, messageDataUnderTest.get_raw_geoidal_separation()); EXPECT_EQ(GNSSPositionData::GNSSMethod::RTKFixedInteger, messageDataUnderTest.get_gnss_method()); EXPECT_EQ(-10, messageDataUnderTest.get_horizontal_dilution_of_precision()); EXPECT_EQ(GNSSPositionData::Integrity::Safe, messageDataUnderTest.get_integrity()); @@ -216,12 +327,87 @@ TEST(NMEA2000_Tests, GNSSPositionDataDataInterface) EXPECT_EQ(5820000000, messageDataUnderTest.get_raw_altitude()); EXPECT_EQ(-72057594037298808, messageDataUnderTest.get_raw_latitude()); EXPECT_EQ(720575, messageDataUnderTest.get_raw_longitude()); - EXPECT_NEAR(5820000000.0 * 10E-6, messageDataUnderTest.get_altitude(), 10E-4); - EXPECT_NEAR(-72057594037298808.0 * 10E-16, messageDataUnderTest.get_latitude(), 10E-4); - EXPECT_NEAR(720575.0 * 10E-16, messageDataUnderTest.get_longitude(), 10E-4); + EXPECT_NEAR(5820000000.0 * 1E-6, messageDataUnderTest.get_altitude(), 10E-4); + EXPECT_NEAR(-72057594037298808.0 * 1E-16, messageDataUnderTest.get_latitude(), 10E-4); + EXPECT_NEAR(720575.0 * 1E-16, messageDataUnderTest.get_longitude(), 10E-4); + EXPECT_EQ(19551, messageDataUnderTest.get_position_date()); + EXPECT_EQ(86400, messageDataUnderTest.get_position_time()); + EXPECT_EQ(4, messageDataUnderTest.get_reference_station_id(0)); + EXPECT_EQ(GNSSPositionData::TypeOfSystem::Galileo, messageDataUnderTest.get_reference_station_system_type(0)); + EXPECT_EQ(100, messageDataUnderTest.get_reference_station_corrections_age(0)); + EXPECT_NEAR(100, messageDataUnderTest.get_geoidal_separation(), 0.001); std::vector messageBuffer; EXPECT_NO_THROW(messageDataUnderTest.serialize(messageBuffer)); + + ASSERT_EQ(47, messageBuffer.size()); + + EXPECT_EQ(5, messageBuffer.at(0)); // Sequence + + std::uint16_t date = static_cast(messageBuffer.at(1)) | (static_cast(messageBuffer.at(2)) << 8); + EXPECT_EQ(19551, date); + + std::uint32_t time = static_cast(messageBuffer.at(3)) | + (static_cast(messageBuffer.at(4)) << 8) | + (static_cast(messageBuffer.at(5)) << 16) | + (static_cast(messageBuffer.at(6)) << 24); + EXPECT_EQ(86400, time); + + std::int64_t latitude = static_cast(messageBuffer.at(7)) | + (static_cast(messageBuffer.at(8)) << 8) | + (static_cast(messageBuffer.at(9)) << 16) | + (static_cast(messageBuffer.at(10)) << 24) | + (static_cast(messageBuffer.at(11)) << 32) | + (static_cast(messageBuffer.at(12)) << 40) | + (static_cast(messageBuffer.at(13)) << 48) | + (static_cast(messageBuffer.at(14)) << 56); + EXPECT_EQ(latitude, -72057594037298808); + + std::int64_t longitude = static_cast(messageBuffer.at(15)) | + (static_cast(messageBuffer.at(16)) << 8) | + (static_cast(messageBuffer.at(17)) << 16) | + (static_cast(messageBuffer.at(18)) << 24) | + (static_cast(messageBuffer.at(19)) << 32) | + (static_cast(messageBuffer.at(20)) << 40) | + (static_cast(messageBuffer.at(21)) << 48) | + (static_cast(messageBuffer.at(22)) << 56); + EXPECT_EQ(longitude, 720575); + + std::int64_t altitude = static_cast(messageBuffer.at(23)) | + (static_cast(messageBuffer.at(24)) << 8) | + (static_cast(messageBuffer.at(25)) << 16) | + (static_cast(messageBuffer.at(26)) << 24) | + (static_cast(messageBuffer.at(27)) << 32) | + (static_cast(messageBuffer.at(28)) << 40) | + (static_cast(messageBuffer.at(29)) << 48) | + (static_cast(messageBuffer.at(30)) << 56); + EXPECT_EQ(altitude, 5820000000); + + EXPECT_EQ(messageBuffer.at(31) & 0x0F, 4); // System type + EXPECT_EQ((messageBuffer.at(31) >> 4) & 0x0F, 4); // Method + EXPECT_EQ(messageBuffer.at(32) & 0x03, 1); // Integrity + EXPECT_EQ(messageBuffer.at(32) & 0xFC, 0xFC); // Integrity byte's reserved bits + EXPECT_EQ(messageBuffer.at(33), 4); // Number of SVs + + std::int16_t hdop = static_cast(messageBuffer.at(34)) | (static_cast(messageBuffer.at(35)) << 8); + EXPECT_EQ(-10, hdop); + + std::int16_t pdop = static_cast(messageBuffer.at(36)) | (static_cast(messageBuffer.at(37)) << 8); + EXPECT_EQ(-894, pdop); + + std::int32_t geoidalSep = static_cast(messageBuffer.at(38)) | + (static_cast(messageBuffer.at(39)) << 8) | + (static_cast(messageBuffer.at(40)) << 16) | + (static_cast(messageBuffer.at(41)) << 24); + EXPECT_EQ(10000, geoidalSep); + + EXPECT_EQ(1, messageBuffer.at(42)); + EXPECT_EQ(8, messageBuffer.at(43) & 0x0F); + + std::uint16_t stationID = static_cast(messageBuffer.at(43) >> 4) | (static_cast(messageBuffer.at(44)) << 4); + EXPECT_EQ(stationID, 4); + EXPECT_EQ(100, messageBuffer.at(45)); + EXPECT_EQ(0, messageBuffer.at(46)); } TEST(NMEA2000_Tests, NMEA2KInterface) @@ -334,12 +520,12 @@ TEST(NMEA2000_Tests, NMEA2KInterface) message.set_sequence_id(155); message.set_speed_over_ground(544); - EXPECT_NEAR(1.0, message.get_course_over_ground(), 0.001); + EXPECT_NEAR(10000 * 10E-4f, message.get_course_over_ground(), 0.001); EXPECT_EQ(CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroudReference::True, message.get_course_over_ground_reference()); EXPECT_EQ(10000, message.get_raw_course_over_ground()); EXPECT_EQ(544, message.get_raw_speed_over_ground()); EXPECT_EQ(155, message.get_sequence_id()); - EXPECT_NEAR(5.44, message.get_speed_over_ground(), 0.001); + EXPECT_NEAR(544 * 10E-2f, message.get_speed_over_ground(), 0.001); interfaceUnderTest.update(); ASSERT_TRUE(testPlugin.read_frame(testFrame)); @@ -372,6 +558,13 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_NE(nullptr, interfaceUnderTest.get_received_course_speed_over_ground_message(0)); EXPECT_TRUE(wasCourseOverGroundSpeedOverGroundRapidUpdateCallbackHit); + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + + // Make sure duplicate messages don't make more instances of the message's class + EXPECT_EQ(1, interfaceUnderTest.get_number_received_course_speed_over_ground_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_course_speed_over_ground_message(0)); } { @@ -389,6 +582,91 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_FALSE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + interfaceUnderTest.set_enable_sending_datum_cyclically(false); + EXPECT_FALSE(interfaceUnderTest.get_enable_sending_datum_cyclically()); + interfaceUnderTest.set_enable_sending_datum_cyclically(true); + EXPECT_TRUE(interfaceUnderTest.get_enable_sending_datum_cyclically()); + + auto &message = interfaceUnderTest.get_datum_transmit_message(); + + EXPECT_TRUE(message.set_delta_altitude(25000)); + EXPECT_TRUE(message.set_delta_latitude(12345)); + EXPECT_TRUE(message.set_delta_longitude(6789)); + EXPECT_TRUE(message.set_local_datum("abc1")); + EXPECT_TRUE(message.set_reference_datum("def2")); + + EXPECT_EQ(25000, message.get_raw_delta_altitude()); + EXPECT_NEAR(25000 * 10E-2f, message.get_delta_altitude(), 0.1); + + EXPECT_EQ(12345, message.get_raw_delta_latitude()); + EXPECT_NEAR(12345 * 10E-7, message.get_delta_latitude(), 0.001); + + EXPECT_EQ(6789, message.get_raw_delta_longitude()); + EXPECT_NEAR(6789 * 10E-7, message.get_delta_longitude(), 0.001); + + EXPECT_EQ("abc1", message.get_local_datum()); + EXPECT_EQ("def2", message.get_reference_datum()); + + while (SystemTiming::get_timestamp_ms() < message.get_timeout()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + interfaceUnderTest.update(); + CANNetworkManager::CANNetwork.update(); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + + // Message encoding tested elsewhere, just verify PGN in the Fast packet + EXPECT_EQ(0x1F814, (testFrame.identifier >> 8) & 0x1FFFF); + + std::vector lastFastPacketPayload; + lastFastPacketPayload.resize(20); + memcpy(lastFastPacketPayload.data(), &testFrame.data[2], 6); + + // wait for the rest of the FP... + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 2 + memcpy(lastFastPacketPayload.data() + 6, &testFrame.data[1], 7); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 3 + memcpy(lastFastPacketPayload.data() + 13, &testFrame.data[1], 7); + + std::vector comparisonBuffer; + ASSERT_NO_THROW(message.serialize(comparisonBuffer)); + + for (std::uint8_t i = 0; i < 20; i++) + { + EXPECT_EQ(comparisonBuffer.at(i), lastFastPacketPayload.at(i)); + } + + EXPECT_EQ(0, interfaceUnderTest.get_number_received_datum_message_sources()); + EXPECT_EQ(nullptr, interfaceUnderTest.get_received_datum_message(0)); + + auto listenerHandle = interfaceUnderTest.get_datum_event_publisher().add_listener(test_datum_callback); + + // Pass the fast packet back in to simulate receiving + testFrame.identifier = 0x19F81452; + testFrame.data[0] = 0x00; + testFrame.data[1] = 0x14; + memcpy(&testFrame.data[2], lastFastPacketPayload.data(), 6); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x01; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 6, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x02; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 13, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasDatumCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_datum_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_datum_message(0)); + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + + // Make sure duplicate messages don't make more instances of the message's class + EXPECT_EQ(1, interfaceUnderTest.get_number_received_datum_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_datum_message(0)); } { @@ -406,6 +684,108 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_FALSE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + interfaceUnderTest.set_enable_sending_gnss_position_data_cyclically(false); + EXPECT_FALSE(interfaceUnderTest.get_enable_sending_gnss_position_data_cyclically()); + interfaceUnderTest.set_enable_sending_gnss_position_data_cyclically(true); + EXPECT_TRUE(interfaceUnderTest.get_enable_sending_gnss_position_data_cyclically()); + + auto &message = interfaceUnderTest.get_gnss_position_data_transmit_message(); + + EXPECT_TRUE(message.set_sequence_id(15)); + EXPECT_TRUE(message.set_geoidal_separation(10000)); + EXPECT_TRUE(message.set_gnss_method(GNSSPositionData::GNSSMethod::RTKFixedInteger)); + EXPECT_TRUE(message.set_horizontal_dilution_of_precision(-10)); + EXPECT_TRUE(message.set_integrity(GNSSPositionData::Integrity::Caution)); + EXPECT_TRUE(message.set_number_of_reference_stations(1)); + EXPECT_TRUE(message.set_number_of_space_vehicles(4)); + EXPECT_TRUE(message.set_positional_dilution_of_precision(-894)); + EXPECT_TRUE(message.set_timestamp(50)); + EXPECT_TRUE(message.set_type_of_system(GNSSPositionData::TypeOfSystem::GPSPlusSBASPlusGLONASS)); + EXPECT_TRUE(message.set_altitude(582000000)); + EXPECT_TRUE(message.set_latitude(-7205759403729808)); + EXPECT_TRUE(message.set_longitude(720575)); + EXPECT_TRUE(message.set_position_date(19551)); + EXPECT_TRUE(message.set_position_time(8400)); + EXPECT_TRUE(message.set_reference_station(0, 4, GNSSPositionData::TypeOfSystem::GLONASS, 100)); + + std::vector comparisonBuffer; + ASSERT_NO_THROW(message.serialize(comparisonBuffer)); + + std::vector lastFastPacketPayload; + lastFastPacketPayload.resize(47); + + interfaceUnderTest.update(); + CANNetworkManager::CANNetwork.update(); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + + // Message encoding tested elsewhere, just verify PGN in the Fast packet + EXPECT_EQ(0x1F805, (testFrame.identifier >> 8) & 0x1FFFF); + memcpy(lastFastPacketPayload.data(), &testFrame.data[2], 6); + + // wait for the rest of the FP to complete + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 2 + memcpy(lastFastPacketPayload.data() + 6, &testFrame.data[1], 7); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 3 + memcpy(lastFastPacketPayload.data() + 13, &testFrame.data[1], 7); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 4 + memcpy(lastFastPacketPayload.data() + 20, &testFrame.data[1], 7); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 5 + memcpy(lastFastPacketPayload.data() + 27, &testFrame.data[1], 7); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 6 + memcpy(lastFastPacketPayload.data() + 34, &testFrame.data[1], 7); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); // FP Payload 7 + memcpy(lastFastPacketPayload.data() + 41, &testFrame.data[1], 5); + + for (std::uint8_t i = 0; i < 47; i++) + { + EXPECT_EQ(comparisonBuffer.at(i), lastFastPacketPayload.at(i)); + } + + auto listenerHandle = interfaceUnderTest.get_gnss_position_data_event_publisher().add_listener(test_gnss_position_data_callback); + + // Pass the fast packet back in to simulate receiving + testFrame.identifier = 0x19F80552; + testFrame.data[0] = 0x00; + testFrame.data[1] = 0x2F; + memcpy(&testFrame.data[2], lastFastPacketPayload.data(), 6); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x01; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 6, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x02; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 13, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x03; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 20, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x04; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 27, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x05; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 34, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + + testFrame.data[0] = 0x06; + testFrame.data[7] = 0xFF; + memcpy(&testFrame.data[1], lastFastPacketPayload.data() + 41, 7); + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasGNSSPositionDataCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_gnss_position_data_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_gnss_position_data_message(0)); + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + + // Make sure duplicate messages don't make more instances of the message's class + EXPECT_EQ(1, interfaceUnderTest.get_number_received_gnss_position_data_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_gnss_position_data_message(0)); } { @@ -423,6 +803,56 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_FALSE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + interfaceUnderTest.set_enable_sending_position_delta_high_precision_rapid_update_cyclically(false); + EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_delta_high_precision_rapid_update_cyclically()); + interfaceUnderTest.set_enable_sending_position_delta_high_precision_rapid_update_cyclically(true); + EXPECT_TRUE(interfaceUnderTest.get_enable_sending_position_delta_high_precision_rapid_update_cyclically()); + + auto &message = interfaceUnderTest.get_position_delta_high_precision_rapid_update_transmit_message(); + + EXPECT_TRUE(message.set_latitude_delta(-5000)); + EXPECT_TRUE(message.set_longitude_delta(-9000)); + EXPECT_TRUE(message.set_sequence_id(49)); + EXPECT_TRUE(message.set_time_delta(7)); + + interfaceUnderTest.update(); + CANNetworkManager::CANNetwork.update(); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + + // Message encoding tested elsewhere, just verify PGN + EXPECT_EQ(0x1F803, (testFrame.identifier >> 8) & 0x1FFFF); + + EXPECT_EQ(0, interfaceUnderTest.get_number_received_position_delta_high_precision_rapid_update_message_sources()); + EXPECT_EQ(nullptr, interfaceUnderTest.get_received_position_delta_high_precision_rapid_update_message(0)); + + auto listenerHandle = interfaceUnderTest.get_position_delta_high_precision_rapid_update_event_publisher().add_listener(test_position_delta_high_speed_rapid_update_callback); + + // Pass the message back in + testFrame.identifier = 0x19F80352; + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasPositionDeltaHighSpeedRapidUpdateCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_position_delta_high_precision_rapid_update_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_position_delta_high_precision_rapid_update_message(0)); + + // Update with a known message + testFrame.data[0] = 0xC2; + testFrame.data[1] = 0xBE; + testFrame.data[2] = 0x02; + testFrame.data[3] = 0x00; + testFrame.data[4] = 0x00; + testFrame.data[5] = 0x17; + testFrame.data[6] = 0x00; + testFrame.data[7] = 0x00; + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + auto delta = interfaceUnderTest.get_received_position_delta_high_precision_rapid_update_message(0); + + EXPECT_NEAR(delta->get_latitude_delta(), 2E-6, 0.0001); + EXPECT_NEAR(delta->get_longitude_delta(), 2.3E-5, 0.0001); + EXPECT_NEAR(delta->get_time_delta(), 0.95, 0.001); } { @@ -440,6 +870,44 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_FALSE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); EXPECT_TRUE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + interfaceUnderTest.set_enable_sending_position_rapid_update_cyclically(false); + EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + interfaceUnderTest.set_enable_sending_position_rapid_update_cyclically(true); + EXPECT_TRUE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + auto &message = interfaceUnderTest.get_position_rapid_update_transmit_message(); + + EXPECT_TRUE(message.set_latitude(1000)); + EXPECT_TRUE(message.set_longitude(2000)); + + interfaceUnderTest.update(); + CANNetworkManager::CANNetwork.update(); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + + // Message encoding tested elsewhere, just verify PGN in the Fast packet + EXPECT_EQ(0x1F801, (testFrame.identifier >> 8) & 0x1FFFF); + + EXPECT_EQ(0, interfaceUnderTest.get_number_received_datum_message_sources()); + EXPECT_EQ(nullptr, interfaceUnderTest.get_received_datum_message(0)); + + auto listenerHandle = interfaceUnderTest.get_position_rapid_update_event_publisher().add_listener(test_position_rapid_update_callback); + + // Pass the message back in + testFrame.identifier = 0x19F80152; + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasPositionRapidUpdateCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_position_rapid_update_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_position_rapid_update_message(0)); + + // Validate duplicates don't make more instances + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasPositionRapidUpdateCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_position_rapid_update_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_position_rapid_update_message(0)); } { @@ -457,6 +925,43 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_TRUE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + interfaceUnderTest.set_enable_sending_rate_of_turn_cyclically(false); + EXPECT_FALSE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); + interfaceUnderTest.set_enable_sending_rate_of_turn_cyclically(true); + EXPECT_TRUE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); + + auto &message = interfaceUnderTest.get_rate_of_turn_transmit_message(); + + EXPECT_TRUE(message.set_rate_of_turn(100)); + EXPECT_TRUE(message.set_sequence_id(200)); + + interfaceUnderTest.update(); + CANNetworkManager::CANNetwork.update(); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + + // Message encoding tested elsewhere, just verify PGN + EXPECT_EQ(0x1F113, (testFrame.identifier >> 8) & 0x1FFFF); + + EXPECT_EQ(0, interfaceUnderTest.get_number_received_rate_of_turn_message_sources()); + EXPECT_EQ(nullptr, interfaceUnderTest.get_received_rate_of_turn_message(0)); + + // Pass the message back in + testFrame.identifier = 0x19F11352; + + auto listenerHandle = interfaceUnderTest.get_rate_of_turn_event_publisher().add_listener(test_rate_of_turn_callback); + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasRateOfTurnCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_rate_of_turn_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_rate_of_turn_message(0)); + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasRateOfTurnCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_rate_of_turn_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_rate_of_turn_message(0)); } { @@ -474,6 +979,46 @@ TEST(NMEA2000_Tests, NMEA2KInterface) EXPECT_FALSE(interfaceUnderTest.get_enable_sending_rate_of_turn_cyclically()); EXPECT_TRUE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); EXPECT_FALSE(interfaceUnderTest.get_enable_sending_position_rapid_update_cyclically()); + + interfaceUnderTest.set_enable_sending_vessel_heading_cyclically(false); + EXPECT_FALSE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); + interfaceUnderTest.set_enable_sending_vessel_heading_cyclically(true); + EXPECT_TRUE(interfaceUnderTest.get_enable_sending_vessel_heading_cyclically()); + + auto &message = interfaceUnderTest.get_vessel_heading_transmit_message(); + + EXPECT_TRUE(message.set_heading(1)); + EXPECT_TRUE(message.set_magnetic_deviation(2)); + EXPECT_TRUE(message.set_magnetic_variation(-3)); + EXPECT_TRUE(message.set_sensor_reference(VesselHeading::HeadingSensorReference::True)); + EXPECT_TRUE(message.set_sequence_id(4)); + + interfaceUnderTest.update(); + CANNetworkManager::CANNetwork.update(); + ASSERT_TRUE(testPlugin.read_frame(testFrame)); + + // Message encoding tested elsewhere, just verify PGN in the Fast packet + EXPECT_EQ(0x1F112, (testFrame.identifier >> 8) & 0x1FFFF); + + EXPECT_EQ(0, interfaceUnderTest.get_number_received_vessel_heading_message_sources()); + EXPECT_EQ(nullptr, interfaceUnderTest.get_received_vessel_heading_message(0)); + + // Pass the message back in + testFrame.identifier = 0x19F11252; + + auto listenerHandle = interfaceUnderTest.get_vessel_heading_event_publisher().add_listener(test_vessel_heading_callback); + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasVesselHeadingCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_vessel_heading_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_vessel_heading_message(0)); + + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + EXPECT_TRUE(wasVesselHeadingCallbackHit); + EXPECT_EQ(1, interfaceUnderTest.get_number_received_vessel_heading_message_sources()); + EXPECT_NE(nullptr, interfaceUnderTest.get_received_vessel_heading_message(0)); } CANHardwareInterface::stop();