AMiRo-OS

/*

AMiRo-LLD is a compilation of low-level hardware drivers for the Autonomous Mini Robot (AMiRo) platform.

Copyright (C) 2016..2019  Thomas Schöpping et al.

This program is free software: you can redistribute it and/or modify

it under the terms of the GNU Lesser General Public License as published by

the Free Software Foundation, either version 3 of the License, or

(at your option) any later version.

This program is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License

along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

/*! ------------------------------------------------------------------------------------------------------------------

 * @file    deca_device.c

 * @brief   Decawave device configuration and control functions

 *

 * @attention

 *

 * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.

 *

 * All rights reserved.

 *

 */

#include <alld_DW1000.h>

#if (defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1)) || defined(__DOXYGEN__)

#include <v1/alld_dw1000_regs_v1.h>

#include <assert.h>

#include <string.h>

#include <stdlib.h>

#include <math.h>

// HW dependent implementation (see bottom of file)

static int writetospi(uint16_t headerLength,

               const        uint8_t *headerBuffer,

               uint32_t bodyLength,

               const        uint8_t *bodyBuffer);

static int readfromspi(uint16_t headerLength,

                const uint8_t *headerBuffer,

                uint32_t readlength,

                uint8_t *readBuffer);

// Defines for enable_clocks function

#define FORCE_SYS_XTI  0

#define ENABLE_ALL_SEQ 1

#define FORCE_SYS_PLL  2

#define READ_ACC_ON    7

#define READ_ACC_OFF   8

#define FORCE_OTP_ON   11

#define FORCE_OTP_OFF  12

#define FORCE_TX_PLL   13

#define FORCE_LDE      14

// Defines for ACK request bitmask in DATA and MAC COMMAND frame control (first byte) - Used to detect AAT bit wrongly set.

#define FCTRL_ACK_REQ_MASK 0x20

// Frame control maximum length in bytes.

#define FCTRL_LEN_MAX 2

typedef struct {

    uint32_t lo32;

    uint16_t target[NUM_PRF];

} agc_cfg_struct ;

extern const agc_cfg_struct agc_config ;

//SFD threshold settings for 110k, 850k, 6.8Mb standard and non-standard

extern const uint16_t sftsh[NUM_BR][NUM_SFD];

extern const uint16_t dtune1[NUM_PRF];

#define XMLPARAMS_VERSION   (1.17f)

extern const uint32_t fs_pll_cfg[NUM_CH];

extern const uint8_t fs_pll_tune[NUM_CH];

extern const uint8_t rx_config[NUM_BW];

extern const uint32_t tx_config[NUM_CH];

extern const uint8_t dwnsSFDlen[NUM_BR]; //length of SFD for each of the bitrates

extern const uint32_t digital_bb_config[NUM_PRF][NUM_PACS];

//extern const uint8_t chan_idx[NUM_CH_SUPPORTED]; // move to header file

extern const double txpwr_compensation[NUM_CH];

#define PEAK_MULTPLIER  (0x60) //3 -> (0x3 * 32) & 0x00E0

#define N_STD_FACTOR    (13)

#define LDE_PARAM1      (PEAK_MULTPLIER | N_STD_FACTOR)

#define LDE_PARAM3_16 (0x1607)

#define LDE_PARAM3_64 (0x0607)

#define MIXER_GAIN_STEP (0.5)

#define DA_ATTN_STEP    (2.5)

// #define DWT_API_ERROR_CHECK     // define so API checks config input parameters

//-----------------------------------------

// map the channel number to the index in the configuration arrays below

// 0th element is chan 1, 1st is chan 2, 2nd is chan 3, 3rd is chan 4, 4th is chan 5, 5th is chan 7

const uint8_t chan_idx[NUM_CH_SUPPORTED] = {0, 0, 1, 2, 3, 4, 0, 5};

//-----------------------------------------

const uint32_t tx_config[NUM_CH] =

{

    RF_TXCTRL_CH1,

    RF_TXCTRL_CH2,

    RF_TXCTRL_CH3,

    RF_TXCTRL_CH4,

    RF_TXCTRL_CH5,

    RF_TXCTRL_CH7,

};

//Frequency Synthesiser - PLL configuration

const uint32_t fs_pll_cfg[NUM_CH] =

{

    FS_PLLCFG_CH1,

    FS_PLLCFG_CH2,

    FS_PLLCFG_CH3,

    FS_PLLCFG_CH4,

    FS_PLLCFG_CH5,

    FS_PLLCFG_CH7

};

//Frequency Synthesiser - PLL tuning

const uint8_t fs_pll_tune[NUM_CH] =

{

    FS_PLLTUNE_CH1,

    FS_PLLTUNE_CH2,

    FS_PLLTUNE_CH3,

    FS_PLLTUNE_CH4,

    FS_PLLTUNE_CH5,

    FS_PLLTUNE_CH7

};

//bandwidth configuration

const uint8_t rx_config[NUM_BW] =

{

    RF_RXCTRLH_NBW,

    RF_RXCTRLH_WBW

};

const agc_cfg_struct agc_config =

{

    AGC_TUNE2_VAL,

    { AGC_TUNE1_16M , AGC_TUNE1_64M }  //adc target

};

//DW non-standard SFD length for 110k, 850k and 6.81M

const uint8_t dwnsSFDlen[NUM_BR] =

{

    DW_NS_SFD_LEN_110K,

    DW_NS_SFD_LEN_850K,

    DW_NS_SFD_LEN_6M8

};

// SFD Threshold

const uint16_t sftsh[NUM_BR][NUM_SFD] =

{

    {

        DRX_TUNE0b_110K_STD,

        DRX_TUNE0b_110K_NSTD

    },

    {

        DRX_TUNE0b_850K_STD,

        DRX_TUNE0b_850K_NSTD

    },

    {

        DRX_TUNE0b_6M8_STD,

        DRX_TUNE0b_6M8_NSTD

    }

};

const uint16_t dtune1[NUM_PRF] =

{

    DRX_TUNE1a_PRF16,

    DRX_TUNE1a_PRF64

};

const uint32_t digital_bb_config[NUM_PRF][NUM_PACS] =

{

    {

        DRX_TUNE2_PRF16_PAC8,

        DRX_TUNE2_PRF16_PAC16,

        DRX_TUNE2_PRF16_PAC32,

        DRX_TUNE2_PRF16_PAC64

    },

    {

        DRX_TUNE2_PRF64_PAC8,

        DRX_TUNE2_PRF64_PAC16,

        DRX_TUNE2_PRF64_PAC32,

        DRX_TUNE2_PRF64_PAC64

    }

};

const uint16_t lde_replicaCoeff[PCODES] =

{

    0, // No preamble code 0

    LDE_REPC_PCODE_1,

    LDE_REPC_PCODE_2,

    LDE_REPC_PCODE_3,

    LDE_REPC_PCODE_4,

    LDE_REPC_PCODE_5,

    LDE_REPC_PCODE_6,

    LDE_REPC_PCODE_7,

    LDE_REPC_PCODE_8,

    LDE_REPC_PCODE_9,

    LDE_REPC_PCODE_10,

    LDE_REPC_PCODE_11,

    LDE_REPC_PCODE_12,

    LDE_REPC_PCODE_13,

    LDE_REPC_PCODE_14,

    LDE_REPC_PCODE_15,

    LDE_REPC_PCODE_16,

    LDE_REPC_PCODE_17,

    LDE_REPC_PCODE_18,

    LDE_REPC_PCODE_19,

    LDE_REPC_PCODE_20,

    LDE_REPC_PCODE_21,

    LDE_REPC_PCODE_22,

    LDE_REPC_PCODE_23,

    LDE_REPC_PCODE_24

};

const double txpwr_compensation[NUM_CH] = {

    0.0,

    0.035,

    0.0,

    0.0,

    0.065,

    0.0

};

const uint8_t chan_idxnb[NUM_CH_SUPPORTED] = {0, 0, 1, 2, 0, 3, 0, 0}; //only channels 1,2,3 and 5 are in the narrow band tables

const uint8_t chan_idxwb[NUM_CH_SUPPORTED] = {0, 0, 0, 0, 0, 0, 0, 1}; //only channels 4 and 7 are in in the wide band tables

//---------------------------------------------------------------------------------------------------------------------------

// Range Bias Correction TABLES of range values in integer units of 25 CM, for 8-bit unsigned storage, MUST END IN 255 !!!!!!

//---------------------------------------------------------------------------------------------------------------------------

// offsets to nearest centimeter for index 0, all rest are +1 cm per value

#define CM_OFFSET_16M_NB    (-23)   // for normal band channels at 16 MHz PRF

#define CM_OFFSET_16M_WB    (-28)   // for wider  band channels at 16 MHz PRF

#define CM_OFFSET_64M_NB    (-17)   // for normal band channels at 64 MHz PRF

#define CM_OFFSET_64M_WB    (-30)   // for wider  band channels at 64 MHz PRF

//---------------------------------------------------------------------------------------------------------------------------

// range25cm16PRFnb: Range Bias Correction table for narrow band channels at 16 MHz PRF, NB: !!!! each MUST END IN 255 !!!!

//---------------------------------------------------------------------------------------------------------------------------

const uint8_t range25cm16PRFnb[4][NUM_16M_OFFSET] =

{

    // ch 1 - range25cm16PRFnb

    {

           1,

           3,

           4,

           5,

           7,

           9,

          11,

          12,

          13,

          15,

          18,

          20,

          23,

          25,

          28,

          30,

          33,

          36,

          40,

          43,

          47,

          50,

          54,

          58,

          63,

          66,

          71,

          76,

          82,

          89,

          98,

         109,

         127,

         155,

         222,

         255,

         255

    },

    // ch 2 - range25cm16PRFnb

    {

           1,

           2,

           4,

           5,

           6,

           8,

           9,

          10,

          12,

          13,

          15,

          18,

          20,

          22,

          24,

          27,

          29,

          32,

          35,

          38,

          41,

          44,

          47,

          51,

          55,

          58,

          62,

          66,

          71,

          78,

          85,

          96,

         111,

         135,

         194,

         240,

         255

    },

    // ch 3 - range25cm16PRFnb

    {

           1,

           2,

           3,

           4,

           5,

           7,

           8,

           9,

          10,

          12,

          14,

          16,

          18,

          20,

          22,

          24,

          26,

          28,

          31,

          33,

          36,

          39,

          42,

          45,

          49,

          52,

          55,

          59,

          63,

          69,

          76,

          85,

          98,

         120,

         173,

         213,

         255

    },

    // ch 5 - range25cm16PRFnb

    {

           1,

           1,

           2,

           3,

           4,

           5,

           6,

           6,

           7,

           8,

           9,

          11,

          12,

          14,

          15,

          16,

          18,

          20,

          21,

          23,

          25,

          27,

          29,

          31,

          34,

          36,

          38,

          41,

          44,

          48,

          53,

          59,

          68,

          83,

         120,

         148,

         255

    }

}; // end range25cm16PRFnb

//---------------------------------------------------------------------------------------------------------------------------

// range25cm16PRFwb: Range Bias Correction table for wide band channels at 16 MHz PRF, NB: !!!! each MUST END IN 255 !!!!

//---------------------------------------------------------------------------------------------------------------------------

const uint8_t range25cm16PRFwb[2][NUM_16M_OFFSETWB] =

{

    // ch 4 - range25cm16PRFwb

    {

           7,

           7,

           8,

           9,

           9,

          10,

          11,

          11,

          12,

          13,

          14,

          15,

          16,

          17,

          18,

          19,

          20,

          21,

          22,

          23,

          24,

          26,

          27,

          28,

          30,

          31,

          32,

          34,

          36,

          38,

          40,

          42,

          44,

          46,

          48,

          50,

          52,

          55,

          57,

          59,

          61,

          63,

          66,

          68,

          71,

          74,

          78,

          81,

          85,

          89,

          94,

          99,

         104,

         110,

         116,

         123,

         130,

         139,

         150,

         164,

         182,

         207,

         238,

         255,

         255,

         255,

         255,

         255

    },

    // ch 7 - range25cm16PRFwb

    {

           4,

           5,

           5,

           5,

           6,

           6,

           7,

           7,

           7,

           8,

           9,

           9,

          10,

          10,

          11,

          11,

          12,

          13,

          13,

          14,

          15,

          16,

          17,

          17,

          18,

          19,

          20,

          21,

          22,

          23,

          25,

          26,

          27,

          29,

          30,

          31,

          32,

          34,

          35,

          36,

          38,

          39,

          40,

          42,

          44,

          46,

          48,

          50,

          52,

          55,

          58,

          61,

          64,

          68,

          72,

          75,

          80,

          85,

          92,

         101,

         112,

         127,

         147,

         168,

         182,

         194,

         205,

         255

    }

}; // end range25cm16PRFwb

//---------------------------------------------------------------------------------------------------------------------------

// range25cm64PRFnb: Range Bias Correction table for narrow band channels at 64 MHz PRF, NB: !!!! each MUST END IN 255 !!!!

//---------------------------------------------------------------------------------------------------------------------------

const uint8_t range25cm64PRFnb[4][NUM_64M_OFFSET] =

{

    // ch 1 - range25cm64PRFnb

    {

           1,

           2,

           2,

           3,

           4,

           5,

           7,

          10,

          13,

          16,

          19,

          22,

          24,

          27,

          30,

          32,

          35,

          38,

          43,

          48,

          56,

          78,

         101,

         120,

         157,

         255

    },

    // ch 2 - range25cm64PRFnb

    {

           1,

           2,

           2,

           3,

           4,

           4,

           6,

           9,

          12,

          14,

          17,

          19,

          21,

          24,

          26,

          28,

          31,

          33,

          37,

          42,

          49,

          68,

          89,

         105,

         138,

         255

    },

    // ch 3 - range25cm64PRFnb

    {

           1,

           1,

           2,

           3,

           3,

           4,

           5,

           8,

          10,

          13,

          15,

          17,

          19,

          21,

          23,

          25,

          27,

          30,

          33,

          37,

          44,

          60,

          79,

          93,

         122,

         255

    },

    // ch 5 - range25cm64PRFnb

    {

           1,

           1,

           1,

           2,

           2,

           3,

           4,

           6,

           7,

           9,

          10,

          12,

          13,

          15,

          16,

          17,

          19,

          21,

          23,

          26,

          30,

          42,

          55,

          65,

          85,

         255

    }

}; // end range25cm64PRFnb

//---------------------------------------------------------------------------------------------------------------------------

// range25cm64PRFwb: Range Bias Correction table for wide band channels at 64 MHz PRF, NB: !!!! each MUST END IN 255 !!!!

//---------------------------------------------------------------------------------------------------------------------------

const uint8_t range25cm64PRFwb[2][NUM_64M_OFFSETWB] =

{

    // ch 4 - range25cm64PRFwb

    {

           7,

           8,

           8,

           9,

           9,

          10,

          11,

          12,

          13,

          13,

          14,

          15,

          16,

          16,

          17,

          18,

          19,

          19,

          20,

          21,

          22,

          24,

          25,

          27,

          28,

          29,

          30,

          32,

          33,

          34,

          35,

          37,

          39,

          41,

          43,

          45,

          48,

          50,

          53,

          56,

          60,

          64,

          68,

          74,

          81,

          89,

          98,

         109,

         122,

         136,

         146,

         154,

         162,

         178,

         220,

         249,

         255,

         255,

         255

    },

    // ch 7 - range25cm64PRFwb

    {

           4,

           5,

           5,

           5,

           6,

           6,

           7,

           7,

           8,

           8,

           9,

           9,

          10,

          10,

          10,

          11,

          11,

          12,

          13,

          13,

          14,

          15,

          16,

          16,

          17,

          18,

          19,

          19,

          20,

          21,

          22,

          23,

          24,

          25,

          26,

          28,

          29,

          31,

          33,

          35,

          37,

          39,

          42,

          46,

          50,

          54,

          60,

          67,

          75,

          83,

          90,

          95,

         100,

         110,

         135,

         153,

         172,

         192,

         255

    }

}; // end range25cm64PRFwb

/*! ------------------------------------------------------------------------------------------------------------------

 * Function: dwt_getrangebias()

 *

 * Description: This function is used to return the range bias correction need for TWR with DW1000 units.

 *

 * input parameters:        

 * @param chan  - specifies the operating channel (e.g. 1, 2, 3, 4, 5, 6 or 7) 

 * @param range - the calculated distance before correction

 * @param prf        - this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M

 *

 * output parameters

 *

 * returns correction needed in meters

 */

double dwt_getrangebias(uint8_t chan, float range, uint8_t prf)

{

    //first get the lookup index that corresponds to given range for a particular channel at 16M PRF

    int i = 0 ;

    int chanIdx ;

    int cmoffseti ;                                 // integer number of CM offset

    double mOffset ;                                // final offset result in metres

    // NB: note we may get some small negitive values e.g. up to -50 cm.

    int rangeint25cm = (int) ((double)range * 4.00) ;       // convert range to integer number of 25cm values.

    if (rangeint25cm > 255) rangeint25cm = 255 ;    // make sure it matches largest value in table (all tables end in 255 !!!!)

    if (prf == DWT_PRF_16M)

    {

        switch(chan)

        {

            case 4:

            case 7:

            {

                chanIdx = chan_idxwb[chan];

                while (rangeint25cm > range25cm16PRFwb[chanIdx][i]) i++ ;       // find index in table corresponding to range

                cmoffseti = i + CM_OFFSET_16M_WB ;                              // nearest centimeter correction

            }

            break;

            default:

            {

                chanIdx = chan_idxnb[chan];

                while (rangeint25cm > range25cm16PRFnb[chanIdx][i]) i++ ;       // find index in table corresponding to range

                cmoffseti = i + CM_OFFSET_16M_NB ;                              // nearest centimeter correction

            }

        }//end of switch

    }

    else // 64M PRF

    {

        switch(chan)

        {

            case 4:

            case 7:

            {

                chanIdx = chan_idxwb[chan];

                while (rangeint25cm > range25cm64PRFwb[chanIdx][i]) i++ ;       // find index in table corresponding to range

                cmoffseti = i + CM_OFFSET_64M_WB ;                              // nearest centimeter correction

            }

            break;

            default:

            {

                chanIdx = chan_idxnb[chan];

                while (rangeint25cm > range25cm64PRFnb[chanIdx][i]) i++ ;       // find index in table corresponding to range

                cmoffseti = i + CM_OFFSET_64M_NB ;                              // nearest centimeter correction

            }

        }//end of switch

    } // end else

    mOffset = (double) cmoffseti ;                                       // offset result in centimmetres

    mOffset *= 0.01 ;                                                   // convert to metres

    return (mOffset) ;

}

// -------------------------------------------------------------------------------------------------------------------

//

// Internal functions for controlling and configuring the device

//

// -------------------------------------------------------------------------------------------------------------------

// Enable and Configure specified clocks

void _dwt_enableclocks(int clocks) ;

// Configure the ucode (FP algorithm) parameters

void _dwt_configlde(int prf);

// Load ucode from OTP/ROM

void _dwt_loaducodefromrom(void);

// Read non-volatile memory

uint32_t _dwt_otpread(uint32_t address);

// Program the non-volatile memory

int32_t _dwt_otpprogword32(uint32_t data, uint16_t address);

// Upload the device configuration into always on memory

void _dwt_aonarrayupload(void);

// -------------------------------------------------------------------------------------------------------------------

/*!

 * Static data for DW1000 DecaWave Transceiver control

 */

static dwt_local_data_t dw1000local[DWT_NUM_DW_DEV] ; // Static local device data, can be an array to support multiple DW1000 testing applications/platforms

static dwt_local_data_t *pdw1000local = dw1000local ; // Static local data structure pointer

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_setdevicedataptr()

 *

 * @brief This function sets the local data structure pointer to point to the structure in the local array as given by the index.

 *

 * input parameters

 * @param index    - selects the array object to point to. Must be within the array bounds, i.e. < DWT_NUM_DW_DEV

 *

 * output parameters

 *

 * returns DWT_SUCCESS for success, or DWT_ERROR for error

 */

int dwt_setdevicedataptr(unsigned int index)

{

    // Check the index is within the array bounds

    if (DWT_NUM_DW_DEV > index) // return error if index outside the array bounds

    {

        return DWT_ERROR ;

    }

    pdw1000local = &dw1000local[index];

    return DWT_SUCCESS ;

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_initialise()

 *

 * @brief This function initiates communications with the DW1000 transceiver

 * and reads its DEV_ID register (address 0x00) to verify the IC is one supported

 * by this software (e.g. DW1000 32-bit device ID value is 0xDECA0130).  Then it

 * does any initial once only device configurations needed for use and initialises

 * as necessary any static data items belonging to this low-level driver.

 *

 * NOTES:

 * 1.this function needs to be run before dwt_configuresleep, also the SPI frequency has to be < 3MHz

 * 2.it also reads and applies LDO tune and crystal trim values from OTP memory

 *

 * input parameters

 * @param config    -   specifies what configuration to load

 *                  DWT_LOADUCODE     0x1 - load the LDE microcode from ROM - enabled accurate RX timestamp

 *                  DWT_LOADNONE      0x0 - do not load any values from OTP memory

 *

 * output parameters

 *

 * returns DWT_SUCCESS for success, or DWT_ERROR for error

 */

// OTP addresses definitions

#define LDOTUNE_ADDRESS (0x04)

#define PARTID_ADDRESS (0x06)

#define LOTID_ADDRESS  (0x07)

#define VBAT_ADDRESS   (0x08)

#define VTEMP_ADDRESS  (0x09)

#define XTRIM_ADDRESS  (0x1E)

int dwt_initialise(const uint16_t config, DW1000Driver* drv)

{

    uint16_t otp_addr = 0;

    uint32_t ldo_tune = 0;

    pdw1000local->dblbuffon = 0; // Double buffer mode off by default

    pdw1000local->wait4resp = 0;

    pdw1000local->sleep_mode = 0;

    pdw1000local->cbTxDone = NULL;

    pdw1000local->cbRxOk = NULL;

    pdw1000local->cbRxTo = NULL;

    pdw1000local->cbRxErr = NULL;

    pdw1000local->driver = drv;

    // Read and validate device ID return -1 if not recognised

    if (DWT_DEVICE_ID != dwt_readdevid()) // MP IC ONLY (i.e. DW1000) FOR THIS CODE

    {

        return DWT_ERROR ;

    }

    // Make sure the device is completely reset before starting initialisation

    dwt_softreset();

    _dwt_enableclocks(FORCE_SYS_XTI); // NOTE: set system clock to XTI - this is necessary to make sure the values read by _dwt_otpread are reliable

    // Configure the CPLL lock detect

    dwt_write8bitoffsetreg(EXT_SYNC_ID, EC_CTRL_OFFSET, EC_CTRL_PLLLCK);

    // Read OTP revision number

    otp_addr = _dwt_otpread(XTRIM_ADDRESS) & 0xffff;        // Read 32 bit value, XTAL trim val is in low octet-0 (5 bits)

    pdw1000local->otprev = (otp_addr >> 8) & 0xff;            // OTP revision is next byte

    // Load LDO tune from OTP and kick it if there is a value actually programmed.

    ldo_tune = _dwt_otpread(LDOTUNE_ADDRESS);

    if((ldo_tune & 0xFF) != 0)

    {

        // Kick LDO tune

        dwt_write8bitoffsetreg(OTP_IF_ID, OTP_SF, OTP_SF_LDO_KICK); // Set load LDE kick bit

        pdw1000local->sleep_mode |= AON_WCFG_ONW_LLDO; // LDO tune must be kicked at wake-up

    }

    // Load Part and Lot ID from OTP

    pdw1000local->partID = _dwt_otpread(PARTID_ADDRESS);

    pdw1000local->lotID = _dwt_otpread(LOTID_ADDRESS);

    // XTAL trim value is set in OTP for DW1000 module and EVK/TREK boards but that might not be the case in a custom design

    pdw1000local->init_xtrim = otp_addr & 0x1F;

    if (!pdw1000local->init_xtrim) // A value of 0 means that the crystal has not been trimmed

    {

        pdw1000local->init_xtrim = FS_XTALT_MIDRANGE ; // Set to mid-range if no calibration value inside

    }

    // Configure XTAL trim

    dwt_setxtaltrim(pdw1000local->init_xtrim);

    // Load leading edge detect code

    if(config & DWT_LOADUCODE)

    {

        _dwt_loaducodefromrom();

        pdw1000local->sleep_mode |= AON_WCFG_ONW_LLDE; // microcode must be loaded at wake-up

    }

    else // Should disable the LDERUN enable bit in 0x36, 0x4

    {

        uint16_t rega = dwt_read16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET+1) ;

        rega &= 0xFDFF ; // Clear LDERUN bit

        dwt_write16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET+1, rega) ;

    }

    _dwt_enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing

    // The 3 bits in AON CFG1 register must be cleared to ensure proper operation of the DW1000 in DEEPSLEEP mode.

    dwt_write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, 0x00);

    // Read system register / store local copy

    pdw1000local->sysCFGreg = dwt_read32bitreg(SYS_CFG_ID) ; // Read sysconfig register

    return DWT_SUCCESS ;

} // end dwt_initialise()

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_otprevision()

 *

 * @brief This is used to return the read OTP revision

 *

 * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.

 *

 * input parameters

 *

 * output parameters

 *

 * returns the read OTP revision value

 */

uint8_t dwt_otprevision(void)

{

    return pdw1000local->otprev ;

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_setfinegraintxseq()

 *

 * @brief This function enables/disables the fine grain TX sequencing (enabled by default).

 *

 * input parameters

 * @param enable - 1 to enable fine grain TX sequencing, 0 to disable it.

 *

 * output parameters none

 *

 * no return value

 */

void dwt_setfinegraintxseq(int enable)

{

    if (enable)

    {

        dwt_write16bitoffsetreg(PMSC_ID, PMSC_TXFINESEQ_OFFSET, PMSC_TXFINESEQ_ENABLE);

    }

    else

    {

        dwt_write16bitoffsetreg(PMSC_ID, PMSC_TXFINESEQ_OFFSET, PMSC_TXFINESEQ_DISABLE);

    }

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_setlnapamode()

 *

 * @brief This is used to enable GPIO for external LNA or PA functionality - HW dependent, consult the DW1000 User Manual.

 *        This can also be used for debug as enabling TX and RX GPIOs is quite handy to monitor DW1000's activity.

 *

 * NOTE: Enabling PA functionality requires that fine grain TX sequencing is deactivated. This can be done using

 *       dwt_setfinegraintxseq().

 *

 * input parameters

 * @param lna - 1 to enable LNA functionality, 0 to disable it

 * @param pa - 1 to enable PA functionality, 0 to disable it

 *

 * output parameters

 *

 * no return value

 */

void dwt_setlnapamode(int lna, int pa)

{

    uint32_t gpio_mode = dwt_read32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET);

    gpio_mode &= ~(GPIO_MSGP4_MASK | GPIO_MSGP5_MASK | GPIO_MSGP6_MASK);

    if (lna)

    {

        gpio_mode |= GPIO_PIN6_EXTRXE;

    }

    if (pa)

    {

        gpio_mode |= (GPIO_PIN5_EXTTXE | GPIO_PIN4_EXTPA);

    }

    dwt_write32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET, gpio_mode);

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_setgpiodirection()

 *

 * @brief This is used to set GPIO direction as an input (1) or output (0)

 *

 * input parameters

 * @param gpioNum    -   this is the GPIO to configure - see GxM0... GxM8 in the deca_regs.h file

 * @param direction  -   this sets the GPIO direction - see GxP0... GxP8 in the deca_regs.h file

 *

 * output parameters

 *

 * no return value

 */

void dwt_setgpiodirection(uint32_t gpioNum, uint32_t direction)

{

    uint8_t buf[GPIO_DIR_LEN];

    uint32_t command = direction | gpioNum;

    buf[0] = command & 0xff;

    buf[1] = (command >> 8) & 0xff;

    buf[2] = (command >> 16) & 0xff;

    dwt_writetodevice(GPIO_CTRL_ID, GPIO_DIR_OFFSET, GPIO_DIR_LEN, buf);

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_setgpiovalue()

 *

 * @brief This is used to set GPIO value as (1) or (0) only applies if the GPIO is configured as output

 *

 * input parameters

 * @param gpioNum    -   this is the GPIO to configure - see GxM0... GxM8 in the deca_regs.h file

 * @param value  -   this sets the GPIO value - see GDP0... GDP8 in the deca_regs.h file

 *

 * output parameters

 *

 * no return value

 */

void dwt_setgpiovalue(uint32_t gpioNum, uint32_t value)

{

    uint8_t buf[GPIO_DOUT_LEN];

    uint32_t command = value | gpioNum;

    buf[0] = command & 0xff;

    buf[1] = (command >> 8) & 0xff;

    buf[2] = (command >> 16) & 0xff;

    dwt_writetodevice(GPIO_CTRL_ID, GPIO_DOUT_OFFSET, GPIO_DOUT_LEN, buf);

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_getpartid()

 *

 * @brief This is used to return the read part ID of the device

 *

 * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.

 *

 * input parameters

 *

 * output parameters

 *

 * returns the 32 bit part ID value as programmed in the factory

 */

uint32_t dwt_getpartid(void)

{

    return pdw1000local->partID;

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_getlotid()

 *

 * @brief This is used to return the read lot ID of the device

 *

 * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.

 *

 * input parameters

 *

 * output parameters

 *

 * returns the 32 bit lot ID value as programmed in the factory

 */

uint32_t dwt_getlotid(void)

{

    return pdw1000local->lotID;

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_readdevid()

 *

 * @brief This is used to return the read device type and revision information of the DW1000 device (MP part is 0xDECA0130)

 *

 * input parameters

 *

 * output parameters

 *

 * returns the read value which for DW1000 is 0xDECA0130

 */

uint32_t dwt_readdevid(void)

{

    return dwt_read32bitoffsetreg(DEV_ID_ID,0);

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_configuretxrf()

 *

 * @brief This function provides the API for the configuration of the TX spectrum

 * including the power and pulse generator delay. The input is a pointer to the data structure

 * of type dwt_txconfig_t that holds all the configurable items.

 *

 * input parameters

 * @param config    -   pointer to the txrf configuration structure, which contains the tx rf config data

 *

 * output parameters

 *

 * no return value

 */

void dwt_configuretxrf(dwt_txconfig_t* config)

{

    // Configure RF TX PG_DELAY

    dwt_write8bitoffsetreg(TX_CAL_ID, TC_PGDELAY_OFFSET, config->PGdly);

    // Configure TX power

    dwt_write32bitreg(TX_POWER_ID, config->power);

}

/*! ------------------------------------------------------------------------------------------------------------------

 * @fn dwt_configure()

 *

 * @brief This function provides the main API for the configuration of the

 * DW1000 and this low-level driver.  The input is a pointer to the data structure

 * of type dwt_config_t that holds all the configurable items.

 * The dwt_config_t structure shows which ones are supported

 *

 * input parameters

 * @param config    -   pointer to the configuration structure, which contains the device configuration data.

 *

 * output parameters

 *

 * no return value

 */

void dwt_configure(dwt_config_t *config)

{

    uint8_t nsSfd_result  = 0;

    uint8_t useDWnsSFD = 0;

    uint8_t chan = config->chan ;

    uint32_t regval ;

    uint16_t reg16 = lde_replicaCoeff[config->rxCode];

    uint8_t prfIndex = config->prf - DWT_PRF_16M;

    uint8_t bw = ((chan == 4) || (chan == 7)) ? 1 : 0 ; // Select wide or narrow band

#ifdef DWT_API_ERROR_CHECK

    assert(config->dataRate <= DWT_BR_6M8);

    assert(config->rxPAC <= DWT_PAC64);

    assert((chan >= 1) && (chan <= 7) && (chan != 6));

    assert(((config->prf == DWT_PRF_64M) && (config->txCode >= 9) && (config->txCode <= 24))

           || ((config->prf == DWT_PRF_16M) && (config->txCode >= 1) && (config->txCode <= 8)));

    assert(((config->prf == DWT_PRF_64M) && (config->rxCode >= 9) && (config->rxCode <= 24))

           || ((config->prf == DWT_PRF_16M) && (config->rxCode >= 1) && (config->rxCode <= 8)));

    assert((config->txPreambLength == DWT_PLEN_64) || (config->txPreambLength == DWT_PLEN_128) || (config->txPreambLength == DWT_PLEN_256)

           || (config->txPreambLength == DWT_PLEN_512) || (config->txPreambLength == DWT_PLEN_1024) || (config->txPreambLength == DWT_PLEN_1536)

           || (config->txPreambLength == DWT_PLEN_2048) || (config->txPreambLength == DWT_PLEN_4096));

    assert((config->phrMode == DWT_PHRMODE_STD) || (config->phrMode == DWT_PHRMODE_EXT));

#endif

    // For 110 kbps we need a special setup

    if(DWT_BR_110K == config->dataRate)

    {

        pdw1000local->sysCFGreg |= SYS_CFG_RXM110K ;

        reg16 >>= 3; // lde_replicaCoeff must be divided by 8

    }

    else

    {

        pdw1000local->sysCFGreg &= (~SYS_CFG_RXM110K) ;

    }

    pdw1000local->longFrames = config->phrMode ;

    pdw1000local->sysCFGreg &= ~SYS_CFG_PHR_MODE_11;

    pdw1000local->sysCFGreg |= (SYS_CFG_PHR_MODE_11 & (uint32_t)(config->phrMode << SYS_CFG_PHR_MODE_SHFT));

    dwt_write32bitreg(SYS_CFG_ID,pdw1000local->sysCFGreg) ;

    // Set the lde_replicaCoeff

    dwt_write16bitoffsetreg(LDE_IF_ID, LDE_REPC_OFFSET, reg16) ;

    _dwt_configlde(prfIndex);

    // Configure PLL2/RF PLL block CFG/TUNE (for a given channel)

    dwt_write32bitoffsetreg(FS_CTRL_ID, FS_PLLCFG_OFFSET, fs_pll_cfg[chan_idx[chan]]);

    dwt_write8bitoffsetreg(FS_CTRL_ID, FS_PLLTUNE_OFFSET, fs_pll_tune[chan_idx[chan]]);

    // Configure RF RX blocks (for specified channel/bandwidth)

    dwt_write8bitoffsetreg(RF_CONF_ID, RF_RXCTRLH_OFFSET, rx_config[bw]);

    // Configure RF TX blocks (for specified channel and PRF)