BasePigeon.h

Go to the documentation of this file.

/* Copyright (C) Cross The Road Electronics 2024 */
/*
 *  Software License Agreement
 *
 * Copyright (C) Cross The Road Electronics.  All rights
 * reserved.
 *
 * Cross The Road Electronics (CTRE) licenses to you the right to
 * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
 * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
 *
 * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
 * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
 * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
 * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
 * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
 * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
 * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
 * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
 * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
 * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
 * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
 */
 
#pragma once
 
#include <string>
#include "ctre/phoenix/CANBusAddressable.h"
#include "ctre/phoenix/CustomParamConfiguration.h"
#include "ctre/phoenix/paramEnum.h"
#include "ctre/phoenix/ErrorCode.h"
#include "ctre/phoenix/sensors/PigeonIMU_ControlFrame.h"
#include "ctre/phoenix/sensors/PigeonIMU_Faults.h"
#include "ctre/phoenix/sensors/PigeonIMU_StatusFrame.h"
#include "ctre/phoenix/sensors/PigeonIMU_StickyFaults.h"
#include "ctre/phoenix/sensors/BasePigeonSimCollection.h"
 
/* forward prototype */
namespace ctre {
namespace phoenix {
namespace motorcontrol {
namespace can {
class TalonSRX;
}
}
}
}
 
namespace ctre {
namespace phoenix {
/** sensors namespace */
namespace sensors {
 
/**
 * Configurables available to Pigeon
 */
struct BasePigeonConfiguration : CustomParamConfiguration{
    BasePigeonConfiguration() {}
 
    /**
     * @return String representation of configs
     */
    std::string toString() {
        return toString("");
    }
 
    /**
     * @param prependString
     *              String to prepend to configs
     * @return String representation of configs
     */
    std::string toString(std::string prependString) {
        std::string retstr = CustomParamConfiguration::toString(prependString);
 
        return retstr;
    }
};// struct BasePigeon
 
/**
 * Util class to help with Pigeon configurations
 */
struct BasePigeonConfigUtils {
private:
    static BasePigeonConfiguration _default;
public:
    /**
     * Determine if specified value is different from default
     * @param settings settings to compare against
     * @return if specified value is different from default
     * @{
     */
    static bool CustomParam0Different (const BasePigeonConfiguration & settings) { return (!(settings.customParam0 == _default.customParam0)) || !settings.enableOptimizations; }
    static bool CustomParam1Different (const BasePigeonConfiguration & settings) { return (!(settings.customParam1 == _default.customParam1)) || !settings.enableOptimizations; }
    /** @} */
};
 
/**
 * Pigeon IMU Class.
 * Class supports communicating over CANbus and over ribbon-cable (CAN Talon SRX).
 */
class BasePigeon: public CANBusAddressable {
public:
 
    /**
     * Create a Pigeon object that communicates with Pigeon on CAN Bus.
     *
     * @param deviceNumber
     *            CAN Device Id of Pigeon [0,62]
     * @param canbus Name of the CANbus; can be a SocketCAN interface (on Linux),
     *               or a CANivore device name or serial number
     */
    BasePigeon(int deviceNumber, std::string const &version, std::string const &canbus = "");
 
    ~BasePigeon();
 
    /**
     * Destructs all pigeon objects
     */
    static void DestroyAllBasePigeons();
 
    /**
     * Sets the Yaw register to the specified value.
     *
     * @param angleDeg  Degree of Yaw  [+/- 368,640 degrees]
     * @param timeoutMs
   *            Timeout value in ms. If nonzero, function will wait for
   *            config success and report an error if it times out.
   *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    int SetYaw(double angleDeg, int timeoutMs = 0);
    /**
     * Atomically add to the Yaw register.
     *
     * @param angleDeg  Degrees to add to the Yaw register.
     * @param timeoutMs
   *            Timeout value in ms. If nonzero, function will wait for
   *            config success and report an error if it times out.
   *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    int AddYaw(double angleDeg, int timeoutMs = 0);
    /**
     * Sets the Yaw register to match the current compass value.
     *
     * @param timeoutMs
   *            Timeout value in ms. If nonzero, function will wait for
   *            config success and report an error if it times out.
   *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    int SetYawToCompass(int timeoutMs = 0);
 
    /**
     * Sets the AccumZAngle.
     *
     * @param angleDeg  Degrees to set AccumZAngle to.
     * @param timeoutMs
   *            Timeout value in ms. If nonzero, function will wait for
   *            config success and report an error if it times out.
   *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    int SetAccumZAngle(double angleDeg, int timeoutMs = 0);
    /**
     * Call GetLastError() generated by this object.
     * Not all functions return an error code but can
     * potentially report errors.
     *
     * This function can be used to retrieve those error codes.
     *
     * @return The last ErrorCode generated.
     */
    ErrorCode GetLastError() const;
    /**
     * Get 6d Quaternion data.
     *
     * @param wxyz Array to fill with quaternion data w[0], x[1], y[2], z[3]
     * @return The last ErrorCode generated.
     */
    ErrorCode Get6dQuaternion(double wxyz[4]) const;
    /**
     * Get Yaw, Pitch, and Roll data.
     *
     * @param ypr Array to fill with yaw[0], pitch[1], and roll[2] data.
     *                  Yaw is within [-368,640, +368,640] degrees.
     *                  Pitch is within [-90,+90] degrees.
     *                  Roll is within [-90,+90] degrees.
     * @return The last ErrorCode generated.
     */
    ErrorCode GetYawPitchRoll(double ypr[3]) const;
    /**
     * Get the yaw from the Pigeon
     * @return Yaw
     */
    double GetYaw() const;
    /**
     * Get the pitch from the Pigeon
     * @return Pitch
     */
    double GetPitch() const;
    /**
     * Get the roll from the Pigeon
     * @return Roll
     */
    double GetRoll() const;
    /**
     * Get AccumGyro data.
     * AccumGyro is the integrated gyro value on each axis.
     *
     * @param xyz_deg Array to fill with x[0], y[1], and z[2] AccumGyro data
     * @return The last ErrorCode generated.
     */
    int GetAccumGyro(double xyz_deg[3]) const;
    /**
     * Get the absolute compass heading.
     * @return compass heading [0,360) degrees.
     */
    double GetAbsoluteCompassHeading() const;
    /**
     * Get the continuous compass heading.
     * @return continuous compass heading [-23040, 23040) degrees. Use
     *         SetCompassHeading to modify the wrap-around portion.
     */
    double GetCompassHeading() const;
    /**
     * Gets the compass' measured magnetic field strength.
     * @return field strength in Microteslas (uT).
     */
    double GetCompassFieldStrength() const;
    /**
     * Gets the temperature of the pigeon.
     *
     * @return Temperature in ('C)
     */
    double GetTemp() const;
    /**
     * Gets the current Pigeon uptime.
     *
     * @return How long has Pigeon been running in whole seconds. Value caps at
     *         255.
     */
    uint32_t GetUpTime() const;
    /**
     * Get Raw Magnetometer data.
     *
     * @param rm_xyz Array to fill with x[0], y[1], and z[2] data
     *              Number is equal to 0.6 microTeslas per unit.
     * @return The last ErrorCode generated.
     */
    int GetRawMagnetometer(int16_t rm_xyz[3]) const;
 
    /**
     * Get Biased Magnetometer data.
     *
     * @param bm_xyz Array to fill with x[0], y[1], and z[2] data
     *              Number is equal to 0.6 microTeslas per unit.
     * @return The last ErrorCode generated.
     */
    int GetBiasedMagnetometer(int16_t bm_xyz[3]) const;
    /**
     * Get Biased Accelerometer data.
     *
     * @param ba_xyz Array to fill with x[0], y[1], and z[2] data.
     *          These are in fixed point notation Q2.14.  eg. 16384 = 1G
     * @return The last ErrorCode generated.
     */
    int GetBiasedAccelerometer(int16_t ba_xyz[3]) const;
    /**
     * Get Raw Gyro data.
     *
     * @param xyz_dps Array to fill with x[0], y[1], and z[2] data in degrees per second.
     * @return The last ErrorCode generated.
     */
    int GetRawGyro(double xyz_dps[3]) const;
    /**
     * @return number of times Pigeon Reset
     */
    uint32_t GetResetCount() const;
    /**
     * @return Reset flags for Pigeon
     */
    uint32_t GetResetFlags() const;
    /**
     * @return firmware version of Pigeon
     */
    uint32_t GetFirmVers() const;
 
    /**
     * @return true iff a reset has occurred since last call.
     */
    bool HasResetOccurred() const;
 
    /**
     * Sets the value of a custom parameter. This is for arbitrary use.
     *
     * Sometimes it is necessary to save calibration/declination/offset
     * information in the device. Particularly if the
     * device is part of a subsystem that can be replaced.
     *
     * @param newValue
     *            Value for custom parameter.
     * @param paramIndex
     *            Index of custom parameter. [0-1]
     * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    ErrorCode ConfigSetCustomParam(int newValue, int paramIndex, int timeoutMs = 0);
    /**
     * Gets the value of a custom parameter. This is for arbitrary use.
   *
   * Sometimes it is necessary to save calibration/declination/offset
   * information in the device. Particularly if the
   * device is part of a subsystem that can be replaced.
     *
     * @param paramIndex
     *            Index of custom parameter. [0-1]
     * @param timeoutMs
 *            Timeout value in ms. If nonzero, function will wait for
 *            config success and report an error if it times out.
 *            If zero, no blocking or checking is performed.
     * @return Value of the custom param.
     */
    int ConfigGetCustomParam(int paramIndex, int timeoutMs = 0);
    /**
     * Sets a parameter. Generally this is not used.
     * This can be utilized in
     * - Using new features without updating API installation.
     * - Errata workarounds to circumvent API implementation.
     * - Allows for rapid testing / unit testing of firmware.
     *
     * @param param
     *            Parameter enumeration.
     * @param value
     *            Value of parameter.
     * @param subValue
     *            Subvalue for parameter. Maximum value of 255.
     * @param ordinal
     *            Ordinal of parameter.
     * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    ErrorCode ConfigSetParameter(ParamEnum param, double value,
            uint8_t subValue, int ordinal, int timeoutMs = 0);
    /**
     * Gets a parameter. Generally this is not used.
   * This can be utilized in
   * - Using new features without updating API installation.
   * - Errata workarounds to circumvent API implementation.
   * - Allows for rapid testing / unit testing of firmware.
     *
     * @param param
     *            Parameter enumeration.
     * @param ordinal
     *            Ordinal of parameter.
     * @param timeoutMs
 *            Timeout value in ms. If nonzero, function will wait for
 *            config success and report an error if it times out.
 *            If zero, no blocking or checking is performed.
     * @return Value of parameter.
     */
    double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs = 0);
    /**
     * Gets a parameter by passing an int by reference
     * 
     * @param param
     *            Parameter enumeration
     * @param valueToSend
     *            Value to send to parameter
     * @param valueReceived
     *            Reference to integer to receive
     * @param subValue
     *            SubValue of parameter
     * @param ordinal
     *            Ordinal of parameter
     * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    ErrorCode ConfigGetParameter(ParamEnum param, int32_t valueToSend,
            int32_t & valueReceived, uint8_t & subValue, int32_t ordinal,
            int32_t timeoutMs);
 
    /**
     * Sets the period of the given status frame.
     *
     * @param statusFrame
     *            Frame whose period is to be changed.
     * @param periodMs
     *            Period in ms for the given frame.
     * @param timeoutMs
 *            Timeout value in ms. If nonzero, function will wait for
 *            config success and report an error if it times out.
 *            If zero, no blocking or checking is performed.
     * @return Error Code generated by function. 0 indicates no error.
     */
    ErrorCode SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, uint8_t periodMs,
            int timeoutMs = 0);
 
    /**
     * Gets the period of the given status frame.
     *
     * @param frame
     *            Frame to get the period of.
     * @param timeoutMs
     *            Timeout value in ms. If nonzero, function will wait for
     *            config success and report an error if it times out.
     *            If zero, no blocking or checking is performed.
     * @return Period of the given status frame.
     */
    int GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
            int timeoutMs = 0) ;
    /**
     * Sets the period of the given control frame.
     *
     * @param frame
     *            Frame whose period is to be changed.
     * @param periodMs
     *            Period in ms for the given frame.
     * @return Error Code generated by function. 0 indicates no error.
     */
    ErrorCode SetControlFramePeriod(PigeonIMU_ControlFrame frame,
            int periodMs);
    /**
     * Gets the firmware version of the device.
     *
     * @return param holds the firmware version of the device. Device must be powered
     * cycled at least once.
     */
    int GetFirmwareVersion() ;
    /**
     * Clears the Sticky Faults
     *
     * @return Error Code generated by function. 0 indicates no error.
     */
    ErrorCode ClearStickyFaults(int timeoutMs = 0);
    
    /**
     * @return Pigeon resource handle.
     */
    void* GetLowLevelHandle() const {
        return _handle;
    }
    
    //------ All Configs ----------//
    /**
     * Configures all persistent settings.
     *
     * @param allConfigs        Object with all of the persistant settings
     * @param timeoutMs
     *              Timeout value in ms. If nonzero, function will wait for
     *              config success and report an error if it times out.
     *              If zero, no blocking or checking is performed.
     *
     * @return Error Code generated by function. 0 indicates no error. 
     */
    virtual ctre::phoenix::ErrorCode ConfigAllSettings(const BasePigeonConfiguration &allConfigs, int timeoutMs = 50);
    /**
     * Gets all persistant settings.
     *
     * @param allConfigs        Object with all of the persistant settings
     * @param timeoutMs
     *              Timeout value in ms. If nonzero, function will wait for
     *              config success and report an error if it times out.
     *              If zero, no blocking or checking is performed.
     */
    virtual void GetAllConfigs(BasePigeonConfiguration &allConfigs, int timeoutMs = 50);
    /**
     * Configures all persistent settings to defaults.
     *
     * @param timeoutMs
     *              Timeout value in ms. If nonzero, function will wait for
     *              config success and report an error if it times out.
     *              If zero, no blocking or checking is performed.
     *
     * @return Error Code generated by function. 0 indicates no error. 
     */
    virtual ErrorCode ConfigFactoryDefault(int timeoutMs = 50);
 
    /**
     * @return object that can set simulation inputs.
     */
    virtual BasePigeonSimCollection& GetSimCollection();
private:
    /** firmware state reported over CAN */
    enum MotionDriverState {
        Init0 = 0,
        WaitForPowerOff = 1,
        ConfigAg = 2,
        SelfTestAg = 3,
        StartDMP = 4,
        ConfigCompass_0 = 5,
        ConfigCompass_1 = 6,
        ConfigCompass_2 = 7,
        ConfigCompass_3 = 8,
        ConfigCompass_4 = 9,
        ConfigCompass_5 = 10,
        SelfTestCompass = 11,
        WaitForGyroStable = 12,
        AdditionalAccelAdjust = 13,
        Idle = 14,
        Calibration = 15,
        LedInstrum = 16,
        Error = 31,
    };
    /** sub command for the various Set param enums */
    enum TareType {
        SetValue = 0x00, AddOffset = 0x01, MatchCompass = 0x02, SetOffset = 0xFF,
    };
    /** data storage for reset signals */
    struct ResetStats {
        int32_t resetCount;
        int32_t resetFlags;
        int32_t firmVers;
        bool hasReset;
    };
    ResetStats _resetStats = { 0, 0, 0, false };
 
    /** Portion of the arbID for all status and control frames. */
    void* _handle;
    uint32_t _deviceNumber;
    uint32_t _usageHist = 0;
    uint64_t _cache;
    uint32_t _len;
    BasePigeonSimCollection* _simCollection;
 
    /** overall threshold for when frame data is too old */
    const uint32_t EXPECTED_RESPONSE_TIMEOUT_MS = (200);
 
    int PrivateSetParameter(ParamEnum paramEnum, TareType tareType,
            double angleDeg, int timeoutMs = 0);
 
    double GetTemp(const uint64_t & statusFrame);
    
protected:
    BasePigeon(ctre::phoenix::motorcontrol::can::TalonSRX& talonSrx);
 
};// class BasePigeon
} // namespace signals
} // namespace phoenix
} // namespace ctre