AMiRo-OS

/*

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

Copyright (C) 2016..2020  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>

#include <alld_DW1000_regs.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)