PigeonIMU.h

/*
 *  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/sensors/BasePigeon.h"
 
/* forward prototype */
namespace ctre {
namespace phoenix {
namespace motorcontrol {
namespace can {
class TalonSRX;
}
}
}
}
 
namespace ctre {
namespace phoenix {
namespace sensors {
 
struct PigeonIMUConfiguration : public BasePigeonConfiguration{
    PigeonIMUConfiguration() {}
 
    std::string toString() {
        return toString("");
    }
 
    std::string toString(std::string prependString) {
        std::string retstr = BasePigeonConfiguration::toString(prependString);
 
        return retstr;
    }
};// struct PigeonIMU
 
struct PigeonIMUConfigUtils : public BasePigeonConfigUtils {
private:
    static PigeonIMUConfiguration _default;
public:
};
 
class PigeonIMU: public BasePigeon {
public:
    struct FusionStatus {
        double heading;
        bool bIsValid;
        bool bIsFusing;
        std::string description;
        int lastError;
    };
    enum CalibrationMode {
        BootTareGyroAccel = 0,
        Temperature = 1,
        Magnetometer12Pt = 2,
        Magnetometer360 = 3,
        Accelerometer = 5,
    };
    enum PigeonState {
        NoComm, 
        Initializing, 
        Ready, 
        UserCalibration,
    };
    struct GeneralStatus {
        PigeonIMU::PigeonState state;
        PigeonIMU::CalibrationMode currentMode;
        int calibrationError;
        bool bCalIsBooting;
        std::string description;
        double tempC;
        int upTimeSec;
        int noMotionBiasCount;
        int tempCompensationCount;
        int lastError;
    };
 
    PigeonIMU(int deviceNumber);
    PigeonIMU(ctre::phoenix::motorcontrol::can::TalonSRX * talonSrx);
    PigeonIMU(ctre::phoenix::motorcontrol::can::TalonSRX& talonSrx);
 
    int SetFusedHeading(double angleDeg, int timeoutMs = 0);
    int AddFusedHeading(double angleDeg, int timeoutMs = 0);
    int SetFusedHeadingToCompass(int timeoutMs = 0);
 
    int SetTemperatureCompensationDisable(bool bTempCompDisable,
            int timeoutMs = 0);
    int SetCompassDeclination(double angleDegOffset, int timeoutMs = 0);
    int SetCompassAngle(double angleDeg, int timeoutMs = 0);
 
    int EnterCalibrationMode(CalibrationMode calMode, int timeoutMs = 0);
    int GetGeneralStatus(PigeonIMU::GeneralStatus & statusToFill);
    PigeonState GetState();
    int GetAccelerometerAngles(double tiltAngles[3]);
    double GetFusedHeading(FusionStatus & status);
    double GetFusedHeading() const;
    uint32_t GetResetCount();
    uint32_t GetResetFlags();
 
    static std::string ToString(PigeonIMU::PigeonState state);
    static std::string ToString(CalibrationMode cm);
 
    ErrorCode ConfigSetCustomParam(int newValue, int paramIndex, int timeoutMs = 0);
    int ConfigGetCustomParam(int paramIndex, int timeoutMs = 0);
 
    ErrorCode SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, uint8_t periodMs,
            int timeoutMs = 0);
 
    int GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
            int timeoutMs = 0) ;
    ErrorCode SetControlFramePeriod(PigeonIMU_ControlFrame frame,
            int periodMs);
    int GetFirmwareVersion() ;
    ErrorCode GetFaults(PigeonIMU_Faults & toFill) ;
    ErrorCode GetStickyFaults(PigeonIMU_StickyFaults & toFill);
    ErrorCode ClearStickyFaults(int timeoutMs = 0);
    
    //------ All Configs ----------//
    virtual ctre::phoenix::ErrorCode ConfigAllSettings(const PigeonIMUConfiguration &allConfigs, int timeoutMs = 50);
    virtual void GetAllConfigs(PigeonIMUConfiguration &allConfigs, int timeoutMs = 50);
    virtual ErrorCode ConfigFactoryDefault(int timeoutMs = 50);
private:
    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,
    };
    enum TareType {
        SetValue = 0x00, AddOffset = 0x01, MatchCompass = 0x02, SetOffset = 0xFF,
    };
    struct ResetStats {
        int32_t resetCount;
        int32_t resetFlags;
        int32_t firmVers;
        bool hasReset;
    };
    ResetStats _resetStats = { 0, 0, 0, false };
 
    uint32_t _usageHist = 0;
    uint64_t _cache;
    uint32_t _len;
 
    const uint32_t EXPECTED_RESPONSE_TIMEOUT_MS = (200);
 
    int PrivateSetParameter(ParamEnum paramEnum, TareType tareType,
            double angleDeg, int timeoutMs = 0);
 
    PigeonIMU::PigeonState GetState(int errCode, const uint64_t & statusFrame);
    double GetTemp(const uint64_t & statusFrame);
    
 
 
 
};// class PigeonIMU
} // namespace signals
} // namespace phoenix
} // namespace ctre