NavigationBoard Class Reference

This class handles communication with the navigation board on the rover by sending RoveComm packets over the network. More...

#include <NavigationBoard.h>

Collaboration diagram for NavigationBoard:

Public Member Functions
	NavigationBoard ()
	Construct a new Navigation Board:: Navigation Board object.
	~NavigationBoard ()
	Destroy the Navigation Board:: Navigation Board object.
geoops::GPSCoordinate	GetGPSData ()
	Accessor for most recent GPS data received from NavBoard.
geoops::UTMCoordinate	GetUTMData ()
	Accessor for most recent GPS data received from NavBoard converted to UTM coords.
double	GetHeading ()
	Accessor for the most recent compass heading received from the NavBoard.
double	GetHeadingAccuracy ()
	Accessor for the most recent compass heading accuracy received from NavBoard.
double	GetVelocity ()
	The rover's current velocity based off of the distance covered over the last two GPSCoordinates.
double	GetAngularVelocity ()
	The rover's current angular velocity based off of the change in angle over the last two headings.
std::chrono::system_clock::duration	GetGPSLastUpdateTime ()
	A chrono timestamp storing the last time autonomy's GPS location was updated over RoveComm via the NavBoard.
std::chrono::system_clock::duration	GetCompassLastUpdateTime ()
	A chrono timestamp storing the last time autonomy's compass location was updated over RoveComm via the NavBoard.
bool	IsOutOfDate ()
	Checks if any of the navboard data is out of date. Expired data depends on constants::NAVBOARD_MAX_COMPASS_DATA_AGE.

Private Attributes
geoops::GPSCoordinate	m_stLocation
double	m_dHeading
double	m_dHeadingAccuracy
double	m_dVelocity
double	m_dAngularVelocity
std::shared_mutex	m_muLocationMutex
std::shared_mutex	m_muHeadingMutex
std::shared_mutex	m_muVelocityMutex
std::shared_mutex	m_muAngularVelocityMutex
std::chrono::system_clock::time_point	m_tmLastGPSUpdateTime
std::chrono::system_clock::time_point	m_tmLastCompassUpdateTime
bool	m_bNavBoardOutOfDate
const std::function< void(const rovecomm::RoveCommPacket< double > &, const sockaddr_in &)>	ProcessGPSData
	Callback function that is called whenever RoveComm receives new GPS data.
const std::function< void(const rovecomm::RoveCommPacket< float > &, const sockaddr_in &)>	ProcessAccuracyData
	Callback function that is called whenever RoveComm receives new Accuracy data.
const std::function< void(const rovecomm::RoveCommPacket< float > &, const sockaddr_in &)>	ProcessCompassData
	Callback function that is called whenever RoveComm receives new Compass data.

Detailed Description

This class handles communication with the navigation board on the rover by sending RoveComm packets over the network.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-30

Constructor & Destructor Documentation

NavigationBoard()

NavigationBoard::NavigationBoard

(

)

Construct a new Navigation Board:: Navigation Board object.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-23

{
    // Initialize member variables.
    m_stLocation              = geoops::GPSCoordinate(37.951771, -91.778114, 315.0);
    m_tmLastGPSUpdateTime     = std::chrono::system_clock::now();
    m_tmLastCompassUpdateTime = std::chrono::system_clock::now();
    m_dHeading                = 0.0;
    m_dHeadingAccuracy        = 0.0;
    m_dVelocity               = 0.0;
    m_dAngularVelocity        = 0.0;
    m_bNavBoardOutOfDate      = false;
 
    // Subscribe to NavBoard packets.
    rovecomm::RoveCommPacket<u_int8_t> stSubscribePacket;
    stSubscribePacket.unDataId    = manifest::System::SUBSCRIBE_DATA_ID;
    stSubscribePacket.unDataCount = 0;
    stSubscribePacket.eDataType   = manifest::DataTypes::UINT8_T;
    stSubscribePacket.vData       = std::vector<uint8_t>{};
    if (network::g_pRoveCommUDPNode)
    {
        // Determine the IP address to send the subscribe packet to.
        const char* cIPAddress = constants::MODE_SIM ? constants::SIM_IP_ADDRESS.c_str() : manifest::Nav::IP_ADDRESS.IP_STR.c_str();
 
        // Send subscribe packet to NavBoard.
        network::g_pRoveCommUDPNode->SendUDPPacket(stSubscribePacket, cIPAddress, constants::ROVECOMM_OUTGOING_UDP_PORT);
 
        // Set RoveComm callbacks.
        network::g_pRoveCommUDPNode->AddUDPCallback<double>(ProcessGPSData, manifest::Nav::TELEMETRY.find("GPSLATLONALT")->second.DATA_ID);
        network::g_pRoveCommUDPNode->AddUDPCallback<float>(ProcessAccuracyData, manifest::Nav::TELEMETRY.find("ACCURACYDATA")->second.DATA_ID);
        network::g_pRoveCommUDPNode->AddUDPCallback<float>(ProcessCompassData, manifest::Nav::TELEMETRY.find("COMPASSDATA")->second.DATA_ID);
    }
}

~NavigationBoard()

NavigationBoard::~NavigationBoard

(

)

Destroy the Navigation Board:: Navigation Board object.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-23

{}

Member Function Documentation

GetGPSData()

geoops::GPSCoordinate NavigationBoard::GetGPSData

(

)

Accessor for most recent GPS data received from NavBoard.

Returns

geoops::GPSCoordinate - Struct containing lat, lon, alt, and accuracy data.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-23

{
    // Create static boolean for printing out warnings.
    static bool bAlreadyPrintedWarning = false;
 
    // Acquire read lock for getting GPS struct.
    std::shared_lock<std::shared_mutex> lkGPSProcessLock(m_muLocationMutex);
    // Calculate time elapsed since last GPS data update.
    int nGPSDataAge = std::chrono::duration_cast<std::chrono::seconds>(this->GetGPSLastUpdateTime()).count();
    // Check the last time that our current GPS data has been updated.
    if (nGPSDataAge >= constants::NAVBOARD_MAX_GPS_DATA_AGE && !bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Current GPS data is out of date! GPS timestamp is {} seconds old!", nGPSDataAge);
        // Set toggle.
        bAlreadyPrintedWarning = true;
        // Set Out of Date.
        m_bNavBoardOutOfDate = true;
    }
    else if (nGPSDataAge < constants::NAVBOARD_MAX_GPS_DATA_AGE && bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_NOTICE(logging::g_qSharedLogger, "GPS data recovered!");
        // Reset toggle.
        bAlreadyPrintedWarning = false;
        // Reset Out of Date.
        m_bNavBoardOutOfDate = false;
    }
 
    // Return current GPS location.
    return m_stLocation;
}

Here is the call graph for this function:

Here is the caller graph for this function:

GetUTMData()

geoops::UTMCoordinate NavigationBoard::GetUTMData

(

)

Accessor for most recent GPS data received from NavBoard converted to UTM coords.

Returns

geoops::UTMCoordinate - Struct containing easting, northing, alt, zone, and accuracy data.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-23

{
    // Create static boolean for printing out warnings.
    static bool bAlreadyPrintedWarning = false;
 
    // Acquire read lock for getting UTM struct.
    std::shared_lock<std::shared_mutex> lkGPSProcessLock(m_muLocationMutex);
    // Calculate time elapsed since last GPS data update.
    int nGPSDataAge = std::chrono::duration_cast<std::chrono::seconds>(this->GetGPSLastUpdateTime()).count();
    // Check the last time that our current GPS data has been updated.
    if (nGPSDataAge >= constants::NAVBOARD_MAX_GPS_DATA_AGE && !bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Current GPS data is out of date! GPS timestamp is {} seconds old!", nGPSDataAge);
        // Set toggle.
        bAlreadyPrintedWarning = true;
        // Set Out of Date.
        m_bNavBoardOutOfDate = true;
    }
    else if (nGPSDataAge < constants::NAVBOARD_MAX_GPS_DATA_AGE && bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_NOTICE(logging::g_qSharedLogger, "GPS data recovered!");
        // Reset toggle.
        bAlreadyPrintedWarning = false;
        // Reset Out of Date.
        m_bNavBoardOutOfDate = false;
    }
 
    // Convert the currently stored GPS coord to UTM and return.
    return geoops::ConvertGPSToUTM(m_stLocation);
}

Here is the call graph for this function:

GetHeading()

double NavigationBoard::GetHeading

(

)

Accessor for the most recent compass heading received from the NavBoard.

Returns

double - The last known compass heading.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-03

{
    // Create static boolean for printing out warnings.
    static bool bAlreadyPrintedWarning = false;
 
    // Acquire read lock for getting compass double.
    std::shared_lock<std::shared_mutex> lkCompassProcessLock(m_muHeadingMutex);
    // Calculate time elapsed since last GPS data update.
    int nCompassDataAge = std::chrono::duration_cast<std::chrono::seconds>(this->GetCompassLastUpdateTime()).count();
    // Check the last time that our current GPS data has been updated.
    if (nCompassDataAge >= constants::NAVBOARD_MAX_COMPASS_DATA_AGE && !bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Current Compass data is out of date! Compass timestamp is {} seconds old!", nCompassDataAge);
        // Set toggle.
        bAlreadyPrintedWarning = true;
        // Set Out of Date.
        m_bNavBoardOutOfDate = true;
    }
    else if (nCompassDataAge < constants::NAVBOARD_MAX_COMPASS_DATA_AGE && bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_NOTICE(logging::g_qSharedLogger, "Compass data recovered!");
        // Reset toggle.
        bAlreadyPrintedWarning = false;
        // Reset Out of Date.
        m_bNavBoardOutOfDate = false;
    }
 
    // Return current Compass data.
    return m_dHeading;
}

Here is the call graph for this function:

Here is the caller graph for this function:

GetHeadingAccuracy()

double NavigationBoard::GetHeadingAccuracy

(

)

Accessor for the most recent compass heading accuracy received from NavBoard.

Returns

double - The last know compass heading accuracy.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-04-08

{
    // Create static boolean for printing out warnings.
    static bool bAlreadyPrintedWarning = false;
 
    // Acquire read lock for getting compass double.
    std::shared_lock<std::shared_mutex> lkCompassProcessLock(m_muHeadingMutex);
    // Calculate time elapsed since last GPS data update.
    int nCompassDataAge = std::chrono::duration_cast<std::chrono::seconds>(this->GetCompassLastUpdateTime()).count();
    // Check the last time that our current GPS data has been updated.
    if (nCompassDataAge >= constants::NAVBOARD_MAX_COMPASS_DATA_AGE && !bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Current Compass data is out of date! Compass timestamp is {} seconds old!", nCompassDataAge);
        // Set toggle.
        bAlreadyPrintedWarning = true;
        // Set Out of Date.
        m_bNavBoardOutOfDate = true;
    }
    else if (nCompassDataAge < constants::NAVBOARD_MAX_COMPASS_DATA_AGE && bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_NOTICE(logging::g_qSharedLogger, "Compass data recovered!");
        // Reset toggle.
        bAlreadyPrintedWarning = false;
        // Reset Out of Date.
        m_bNavBoardOutOfDate = false;
    }
 
    // Return current Compass data.
    return m_dHeadingAccuracy;
}

Here is the call graph for this function:

GetVelocity()

double NavigationBoard::GetVelocity

(

)

The rover's current velocity based off of the distance covered over the last two GPSCoordinates.

Returns

double - The rover's velocity in meters per second.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-13

{
    // Create static boolean for printing out warnings.
    static bool bAlreadyPrintedWarning = false;
 
    // Acquire read lock for getting velocity double.
    std::shared_lock<std::shared_mutex> lkVelocityProcessLock(m_muVelocityMutex);
    // Calculate time elapsed since last GPS data update.
    int nGPSDataAge = std::chrono::duration_cast<std::chrono::seconds>(this->GetGPSLastUpdateTime()).count();
    // Check the last time that our current GPS data has been updated.
    if (nGPSDataAge >= constants::NAVBOARD_MAX_GPS_DATA_AGE && !bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Current Velocity data is out of date! GPS timestamp is {} seconds old!", nGPSDataAge);
        // Set toggle.
        bAlreadyPrintedWarning = true;
        // Set Out of Date.
        m_bNavBoardOutOfDate = true;
    }
    else if (nGPSDataAge < constants::NAVBOARD_MAX_GPS_DATA_AGE && bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_NOTICE(logging::g_qSharedLogger, "GPS data recovered!");
        // Reset toggle.
        bAlreadyPrintedWarning = false;
        // Reset Out of Date.
        m_bNavBoardOutOfDate = false;
    }
 
    // Return current velocity.
    return m_dVelocity;
}

Here is the call graph for this function:

Here is the caller graph for this function:

GetAngularVelocity()

double NavigationBoard::GetAngularVelocity

(

)

The rover's current angular velocity based off of the change in angle over the last two headings.

Returns

double - The rover's angular velocity in degrees per second.

Author

Jason Pittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-14

{
    // Create static boolean for printing out warnings.
    static bool bAlreadyPrintedWarning = false;
 
    // Acquire read lock for getting angular velocity double.
    std::shared_lock<std::shared_mutex> lkAngularVelocityProcessLock(m_muAngularVelocityMutex);
    // Calculate time elapsed since last GPS data update.
    int nCompassDataAge = std::chrono::duration_cast<std::chrono::seconds>(this->GetCompassLastUpdateTime()).count();
    // Check the last time that our current GPS data has been updated.
    if (nCompassDataAge >= constants::NAVBOARD_MAX_COMPASS_DATA_AGE && !bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_WARNING(logging::g_qSharedLogger, "Current Angular Velocity data is out of date! Compass timestamp is {} seconds old!", nCompassDataAge);
        // Set toggle.
        bAlreadyPrintedWarning = true;
        // Set Out of Date.
        m_bNavBoardOutOfDate = true;
    }
    else if (nCompassDataAge < constants::NAVBOARD_MAX_COMPASS_DATA_AGE && bAlreadyPrintedWarning)
    {
        // Submit logger message.
        LOG_NOTICE(logging::g_qSharedLogger, "Compass data recovered!");
        // Reset toggle.
        bAlreadyPrintedWarning = false;
        // Reset Out of Date.
        m_bNavBoardOutOfDate = false;
    }
 
    // Return angular velocity.
    return m_dAngularVelocity;
}

Here is the call graph for this function:

Here is the caller graph for this function:

GetGPSLastUpdateTime()

std::chrono::system_clock::duration NavigationBoard::GetGPSLastUpdateTime

(

)

A chrono timestamp storing the last time autonomy's GPS location was updated over RoveComm via the NavBoard.

Returns

std::chrono::system_clock::time_point - The timestamp that the current GPSCoordinate location was updated.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-13

{
    // Get current time.
    std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
    // Acquire read lock for getting GPS timestamp.
    std::shared_lock<std::shared_mutex> lkGPSProcessLock(m_muLocationMutex);
    // Return the difference.
    return tmCurrentTime - m_tmLastGPSUpdateTime;
}

Here is the caller graph for this function:

GetCompassLastUpdateTime()

std::chrono::system_clock::duration NavigationBoard::GetCompassLastUpdateTime

(

)

A chrono timestamp storing the last time autonomy's compass location was updated over RoveComm via the NavBoard.

Returns

std::chrono::system_clock::time_point - The timestamp that the current heading was updated.

Author

Jason Pittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-14

{
    // Get current time.
    std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
    // Acquire read lock for getting Heading timestamp.
    std::shared_lock<std::shared_mutex> lkCompassProcessLock(m_muHeadingMutex);
    // Return the difference.
    return tmCurrentTime - m_tmLastCompassUpdateTime;
}

Here is the caller graph for this function:

IsOutOfDate()

bool NavigationBoard::IsOutOfDate

(

)

Checks if any of the navboard data is out of date. Expired data depends on constants::NAVBOARD_MAX_COMPASS_DATA_AGE.

Returns

true - The data is out of date.

false - The data is new.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2024-05-24

{
    return m_bNavBoardOutOfDate;
}

Member Data Documentation

ProcessGPSData

const std::function<void(const rovecomm::RoveCommPacket<double>&, const sockaddr_in&)> NavigationBoard::ProcessGPSData

private

Callback function that is called whenever RoveComm receives new GPS data.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Get current time.
            std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
            // Acquire read lock for getting GPS struct.
            std::shared_lock<std::shared_mutex> lkGPSReadProcessLock(m_muLocationMutex);
            // Calculate distance of new GPS coordinate to old GPS coordinate.
            geoops::GeoMeasurement geMeasurement =
                geoops::CalculateGeoMeasurement(m_stLocation, geoops::GPSCoordinate(stPacket.vData[0], stPacket.vData[1], stPacket.vData[2]));
            // Unlock mutex.
            lkGPSReadProcessLock.unlock();
 
            // Acquire write lock for writing to velocity member variable.
            std::unique_lock<std::shared_mutex> lkVelocityProcessLock(m_muVelocityMutex);
            // Calculate rover velocity based on GPS distance traveled over time.
            m_dVelocity = geMeasurement.dDistanceMeters /
                          static_cast<double>((std::chrono::duration_cast<std::chrono::microseconds>(tmCurrentTime - m_tmLastGPSUpdateTime).count() / 1e6));
            // Unlock mutex.
            lkVelocityProcessLock.unlock();
 
            // Acquire write lock for writing to GPS struct.
            std::unique_lock<std::shared_mutex> lkGPSWriteProcessLock(m_muLocationMutex);
            // Repack data from RoveCommPacket into member variable.
            m_stLocation.dLatitude   = stPacket.vData[0];
            m_stLocation.dLongitude  = stPacket.vData[1];
            m_stLocation.dAltitude   = stPacket.vData[2];
            m_stLocation.tmTimestamp = tmCurrentTime;
            // Update GPS update time.
            m_tmLastGPSUpdateTime = tmCurrentTime;
            // Unlock mutex.
            lkGPSWriteProcessLock.unlock();
 
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger, "Incoming GPS Data: ({} lat, {} lon, {} alt)", m_stLocation.dLatitude, m_stLocation.dLongitude, m_stLocation.dAltitude);
        };

ProcessAccuracyData

const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> NavigationBoard::ProcessAccuracyData

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<float>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stdAddr;
 
            
            std::unique_lock<std::shared_mutex> lkGPSProcessLock(m_muLocationMutex);
            std::unique_lock<std::shared_mutex> lkCompassProcessLock(m_muHeadingMutex);
            
            m_stLocation.d2DAccuracy                = std::fabs(stPacket.vData[0]);
            m_stLocation.d3DAccuracy                = std::fabs(stPacket.vData[1]);
            m_dHeadingAccuracy                      = std::fabs(stPacket.vData[2]);
            m_stLocation.eCoordinateAccuracyFixType = static_cast<geoops::PositionFixType>(stPacket.vData[3]);
            m_stLocation.bIsDifferential            = static_cast<bool>(stPacket.vData[4]);
            
            lkCompassProcessLock.unlock();
            lkGPSProcessLock.unlock();
 
            
            LOG_DEBUG(logging::g_qSharedLogger,
                      "Incoming Accuracy Data: (2D: {}, 3D: {}, Compass: {}, FIX_TYPE: {}, Differential?: {})",
                      stPacket.vData[0],
                      stPacket.vData[1],
                      stPacket.vData[2],
                      stPacket.vData[3],
                      stPacket.vData[4]);
        }

Callback function that is called whenever RoveComm receives new Accuracy data.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Acquire write lock for writing to GPS struct.
            std::unique_lock<std::shared_mutex> lkGPSProcessLock(m_muLocationMutex);
            std::unique_lock<std::shared_mutex> lkCompassProcessLock(m_muHeadingMutex);
            // Repack data from RoveCommPacket into member variable.
            m_stLocation.d2DAccuracy                = std::fabs(stPacket.vData[0]);
            m_stLocation.d3DAccuracy                = std::fabs(stPacket.vData[1]);
            m_dHeadingAccuracy                      = std::fabs(stPacket.vData[2]);
            m_stLocation.eCoordinateAccuracyFixType = static_cast<geoops::PositionFixType>(stPacket.vData[3]);
            m_stLocation.bIsDifferential            = static_cast<bool>(stPacket.vData[4]);
            // Unlock mutex.
            lkCompassProcessLock.unlock();
            lkGPSProcessLock.unlock();
 
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger,
                      "Incoming Accuracy Data: (2D: {}, 3D: {}, Compass: {}, FIX_TYPE: {}, Differential?: {})",
                      stPacket.vData[0],
                      stPacket.vData[1],
                      stPacket.vData[2],
                      stPacket.vData[3],
                      stPacket.vData[4]);
        };

ProcessCompassData

const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> NavigationBoard::ProcessCompassData

private

Callback function that is called whenever RoveComm receives new Compass data.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om), Jason Pittman (jspen.nosp@m.cerp.nosp@m.ittma.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Get current time.
            std::chrono::system_clock::time_point tmCurrentTime = std::chrono::system_clock::now();
 
            // Acquire read lock for heading.
            std::shared_lock<std::shared_mutex> lkCompassReadLock(m_muHeadingMutex);
            // Calculate the total change in angle with respect to the last recorded heading.
            double dNewHeading = stPacket.vData[0];
            double dDeltaAngle = dNewHeading - m_dHeading;
            // Assume that the change in angle can't be greater than 180 degrees in a single timestep.
            // This accounts for changes in angle across the 0/360 degree line.
            if (std::abs(dDeltaAngle) > 180)
            {
                dDeltaAngle = dNewHeading > m_dHeading ? -(360 - dDeltaAngle) : 360 + dDeltaAngle;
            }
            // Unlock mutex.
            lkCompassReadLock.unlock();
 
            // Acquire write lock for writing to angular velocity member variable.
            std::unique_lock<std::shared_mutex> lkAngularVelocityProcessLock(m_muAngularVelocityMutex);
            // Calculate rover angular velocity based on change in heading over time.
            m_dAngularVelocity = dDeltaAngle / (std::chrono::duration_cast<std::chrono::microseconds>(tmCurrentTime - m_tmLastCompassUpdateTime).count() / 1e6);
            // Unlock mutex.
            lkAngularVelocityProcessLock.unlock();
 
            // Acquire write lock for heading and compass timestamp.
            std::unique_lock<std::shared_mutex> lkCompassProcessLock(m_muHeadingMutex);
            // Repack data from RoveCommPacket into member variable.
            m_dHeading = dNewHeading;
            // Update compass time.
            m_tmLastCompassUpdateTime = tmCurrentTime;
            // Unlock mutex.
            lkCompassProcessLock.unlock();
 
            // Submit logger message.
            LOG_DEBUG(logging::g_qSharedLogger, "Incoming Compass Data: {}", m_dHeading);
        };

---

The documentation for this class was generated from the following files:

• src/drivers/NavigationBoard.h
• src/drivers/NavigationBoard.cpp