BasePigeon.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/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 {
namespace sensors {
 
struct BasePigeonConfiguration : CustomParamConfiguration{
    BasePigeonConfiguration() {}
 
    std::string toString() {
        return toString("");
    }
 
    std::string toString(std::string prependString) {
        std::string retstr = CustomParamConfiguration::toString(prependString);
 
        return retstr;
    }
};// struct BasePigeon
 
struct BasePigeonConfigUtils {
private:
    static BasePigeonConfiguration _default;
public:
    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; }
};
 
class BasePigeon: public CANBusAddressable {
public:
 
    BasePigeon(int deviceNumber, std::string const &version, std::string const &canbus = "");
 
    ~BasePigeon();
 
    static void DestroyAllBasePigeons();
 
    int SetYaw(double angleDeg, int timeoutMs = 0);
    int AddYaw(double angleDeg, int timeoutMs = 0);
    int SetYawToCompass(int timeoutMs = 0);
 
    int SetAccumZAngle(double angleDeg, int timeoutMs = 0);
    ErrorCode GetLastError() const;
    ErrorCode Get6dQuaternion(double wxyz[4]) const;
    ErrorCode GetYawPitchRoll(double ypr[3]) const;
    double GetYaw() const;
    double GetPitch() const;
    double GetRoll() const;
    int GetAccumGyro(double xyz_deg[3]) const;
    double GetAbsoluteCompassHeading() const;
    double GetCompassHeading() const;
    double GetCompassFieldStrength() const;
    double GetTemp() const;
    uint32_t GetUpTime() const;
    int GetRawMagnetometer(int16_t rm_xyz[3]) const;
 
    int GetBiasedMagnetometer(int16_t bm_xyz[3]) const;
    int GetBiasedAccelerometer(int16_t ba_xyz[3]) const;
    int GetRawGyro(double xyz_dps[3]) const;
    uint32_t GetResetCount() const;
    uint32_t GetResetFlags() const;
    uint32_t GetFirmVers() const;
 
    bool HasResetOccurred() const;
 
    ErrorCode ConfigSetCustomParam(int newValue, int paramIndex, int timeoutMs = 0);
    int ConfigGetCustomParam(int paramIndex, int timeoutMs = 0);
    ErrorCode ConfigSetParameter(ParamEnum param, double value,
            uint8_t subValue, int ordinal, int timeoutMs = 0);
    double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs = 0);
    ErrorCode ConfigGetParameter(ParamEnum param, int32_t valueToSend,
            int32_t & valueReceived, uint8_t & subValue, int32_t ordinal,
            int32_t timeoutMs);
 
    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 ClearStickyFaults(int timeoutMs = 0);
    
    void* GetLowLevelHandle() const {
        return _handle;
    }
    
    //------ All Configs ----------//
    virtual ctre::phoenix::ErrorCode ConfigAllSettings(const BasePigeonConfiguration &allConfigs, int timeoutMs = 50);
    virtual void GetAllConfigs(BasePigeonConfiguration &allConfigs, int timeoutMs = 50);
    virtual ErrorCode ConfigFactoryDefault(int timeoutMs = 50);
 
    virtual BasePigeonSimCollection& GetSimCollection();
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 };
 
    void* _handle;
    uint32_t _deviceNumber;
    uint32_t _usageHist = 0;
    uint64_t _cache;
    uint32_t _len;
    BasePigeonSimCollection* _simCollection;
 
    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