Pigeon2.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix6/core/CorePigeon2.hpp"
 
#include <wpi/hal/SimDevice.hpp>
#include <wpi/math/geometry/Rotation2d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/telemetry/TelemetryLoggable.hpp>
 
namespace ctre {
namespace phoenix6 {
namespace hardware {
 
#if defined(_WIN32) || defined(_WIN64)
#pragma warning(push)
#pragma warning(disable : 4250)
#endif
 
/**
 * Class description for the Pigeon 2 IMU sensor that measures orientation.
 */
class Pigeon2 : public core::CorePigeon2, public wpi::telemetry::TelemetryLoggable {
    /*
     * The StatusSignal getters are copies so that calls
     * to the WPI interface do not update any references
     *
     * These are also mutable so the const getter methods are
     * properly managed.
     */
    mutable StatusSignal<wpi::units::degree_t> m_yaw = GetYaw(false);
    mutable StatusSignal<wpi::units::degree_t> m_pitch = GetPitch(false);
    mutable StatusSignal<wpi::units::degree_t> m_roll = GetRoll(false);
    mutable StatusSignal<wpi::units::scalar_t> m_quatW = GetQuatW(false);
    mutable StatusSignal<wpi::units::scalar_t> m_quatX = GetQuatX(false);
    mutable StatusSignal<wpi::units::scalar_t> m_quatY = GetQuatY(false);
    mutable StatusSignal<wpi::units::scalar_t> m_quatZ = GetQuatZ(false);
 
    wpi::hal::SimDevice m_simPigeon;
    wpi::hal::SimDouble m_simSupplyVoltage;
    wpi::hal::SimDouble m_simYaw;
    wpi::hal::SimDouble m_simRawYaw;
    wpi::hal::SimDouble m_simPitch;
    wpi::hal::SimDouble m_simRoll;
    wpi::hal::SimDouble m_simAngularVelocityX;
    wpi::hal::SimDouble m_simAngularVelocityY;
    wpi::hal::SimDouble m_simAngularVelocityZ;
 
    int32_t m_simPeriodicUid{-1};
    std::vector<int32_t> m_simValueChangedUids;
 
    static void OnValueChanged(
        const char* name, void* param, HAL_SimValueHandle handle,
        HAL_Bool readonly, const struct HAL_Value* value
    );
    static void OnPeriodic(void* param);
 
public:
    /**
     * Constructs a new Pigeon 2 sensor object.
     *
     * \param deviceId    ID of the device, as configured in Phoenix Tuner
     * \param canbus      The CAN bus this device is on
     */
    Pigeon2(int deviceId, CANBus canbus);
 
    ~Pigeon2();
 
    /**
     * \brief Constructs a stubbed-out Pigeon2, where all status signals, controls,
     * configs, etc. perform no action and immediately return OK. This can be used to
     * silence error messages for devices that have been completely removed from the robot.
     *
     * \returns Stubbed-out Pigeon2
     */
    static Pigeon2 None()
    {
        return Pigeon2{-1, CANBus{}};
    }
 
    /**
     * \brief Resets the Pigeon 2 to a heading of zero.
     *
     * \details This can be used if there is significant drift in the gyro,
     * and it needs to be recalibrated after it has been running.
     */
    void Reset()
    {
        SetYaw(0_deg);
    }
    /**
     * \brief Returns the heading of the Pigeon 2 as a wpi#math#Rotation2d.
     *
     * The angle increases as the Pigeon 2 turns counterclockwise when
     * looked at from the top. This follows the NWU axis convention.
     *
     * \returns The current heading of the Pigeon 2 as a wpi#math#Rotation2d
     */
    wpi::math::Rotation2d GetRotation2d() const
    {
        /* Rotation2d and Pigeon2 are both ccw+ */
        return wpi::math::Rotation2d{m_yaw.Refresh().GetValue()};
    }
    /**
     * \brief Returns the orientation of the Pigeon 2 as a wpi#math#Rotation3d
     * created from the quaternion signals.
     *
     * \returns The current orientation of the Pigeon 2 as a wpi#math#Rotation3d
     */
    wpi::math::Rotation3d GetRotation3d() const
    {
        return wpi::math::Rotation3d{GetQuaternion()};
    }
    /**
     * \brief Returns the orientation of the Pigeon 2 as a wpi#math#Quaternion.
     *
     * \returns The current orientation of the Pigeon 2 as a wpi#math#Quaternion
     */
    wpi::math::Quaternion GetQuaternion() const
    {
        BaseStatusSignal::RefreshAll(m_quatW, m_quatX, m_quatY, m_quatZ);
        return wpi::math::Quaternion{m_quatW.GetValue(), m_quatX.GetValue(), m_quatY.GetValue(), m_quatZ.GetValue()};
    }
 
    /**
     * \brief Logs the object to a wpi::telemetry::TelemetryTable.
     *
     * \param table Telemetry table
     */
    void LogTo(wpi::telemetry::TelemetryTable &table) const override;
 
    /**
     * \brief Gets the telemetry table type.
     *
     * \returns Table type
     */
    std::string_view GetTelemetryType() const override { return "Gyro"; }
};
 
#if defined(_WIN32) || defined(_WIN64)
#pragma warning(pop)
#endif
 
}
}
}