AMiRo-OS

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

 *  @file    instance_common.c

 *  @brief   DecaWave application level common instance functions

 *

 * @attention

 *

 * Copyright 2015 (c) DecaWave Ltd, Dublin, Ireland.

 *

 * All rights reserved.

 *

 * @author DecaWave

 */

#include <deca_instance.h>

#if defined(AMIROLLD_CFG_USE_DW1000) || defined(__DOXYGEN__)

#include <alld_dw1000.h>

#include <string.h>

#include <math.h>

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

extern const uint16_t rfDelays[2];

extern const uint16_t rfDelaysTREK[2];

extern const tx_struct txSpectrumConfig[8];

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

// Deca Calibration Values

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

#define DWT_PRF_64M_RFDLY   (514.462f)

#define DWT_PRF_16M_RFDLY   (513.9067f)

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

//The table below specifies the default TX spectrum configuration parameters... this has been tuned for DW EVK hardware units

//the table is set for smart power - see below in the instance_config function how this is used when not using smart power

const tx_struct txSpectrumConfig[8] =

{

    //Channel 0 ----- this is just a place holder so the next array element is channel 1

    {

            0x0,   //0

            {

                    0x0, //0

                    0x0 //0

            }

    },

    //Channel 1

    {

            0xc9,   //PG_DELAY

            {

                    0x15355575, //16M prf power

                    0x07274767 //64M prf power

            }

    },

    //Channel 2

    {

            0xc2,   //PG_DELAY

            {

                    0x15355575, //16M prf power

                    0x07274767 //64M prf power

            }

    },

    //Channel 3

    {

            0xc5,   //PG_DELAY

            {

                    0x0f2f4f6f, //16M prf power

                    0x2b4b6b8b //64M prf power

            }

    },

    //Channel 4

    {

            0x95,   //PG_DELAY

            {

                    0x1f1f3f5f, //16M prf power

                    0x3a5a7a9a //64M prf power

            }

    },

    //Channel 5

    {

            0xc0,   //PG_DELAY

            {

                    0x0E082848, //16M prf power

                    0x25456585 //64M prf power

            }

    },

    //Channel 6 ----- this is just a place holder so the next array element is channel 7

    {

            0x0,   //0

            {

                    0x0, //0

                    0x0 //0

            }

    },

    //Channel 7

    {

            0x93,   //PG_DELAY

            {

                    0x32527292, //16M prf power

                    0x5171B1d1 //64M prf power

            }

    }

};

//these are default antenna delays for EVB1000, these can be used if there is no calibration data in the DW1000,

//or instead of the calibration data

const uint16_t rfDelays[2] = {

        (uint16_t) ((DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16

        (uint16_t) ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS)

};

//these are default TREK Tag/Anchor antenna delays

const uint16_t rfDelaysTREK[2] = {

        (uint16_t) ((514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2

        (uint16_t) ((514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5

};

//int instance_starttxtest(int framePeriod)

//{

//    //define some test data for the tx buffer

//    uint8 msg[127] = "The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the l";

//    //NOTE: SPI frequency must be < 3MHz

//    port_set_dw1000_slowrate();  //max SPI before PLLs configured is ~4M

//    // the value here 0x1000 gives a period of 32.82 µs

//    //this is setting 0x1000 as frame period (125MHz clock cycles) (time from Tx en - to next - Tx en)

//    dwt_configcontinuousframemode(framePeriod);

//    dwt_writetxdata(127, (uint8 *)  msg, 0) ;

//    dwt_writetxfctrl(127, 0, 0);

//    //to start the first frame - set TXSTRT

//    dwt_starttx(DWT_START_TX_IMMEDIATE);

//    //measure the power

//    //Spectrum Analyser set:

//    //FREQ to be channel default e.g. 3.9936 GHz for channel 2

//    //SPAN to 1GHz

//    //SWEEP TIME 1s

//    //RBW and VBW 1MHz

//    //measure channel power

//    return DWT_SUCCESS ;

//}

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

//      Data Definitions

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

static instance_data_t instance_data[NUM_INST] ;

static double inst_tdist[MAX_TAG_LIST_SIZE] ;

static double inst_idist[MAX_ANCHOR_LIST_SIZE] ;

static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ;

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

// Functions

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

/* @fn           instance_get_local_structure_ptr

 * @brief function to return the pointer to local instance data structure

 * */

instance_data_t* instance_get_local_structure_ptr(unsigned int x)

{

        if (x >= NUM_INST)

        {

                return NULL;

        }

        return &instance_data[x];

}

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

/* @fn           instance_convert_usec_to_devtimeu

 * @brief function to convert microseconds to device time

 * */

uint64_t instance_convert_usec_to_devtimeu (double microsecu)

{

    uint64_t dt;

    long double dtime;

    dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6 ;

    dt =  (uint64_t) (dtime) ;

    return dt;

}

/* @fn           instance_calculate_rangefromTOF

 * @brief function to calculate and the range from given Time of Flight

 * */

int instance_calculate_rangefromTOF(int idx, uint32_t tofx)

{

                instance_data_t* inst = instance_get_local_structure_ptr(0);

        double distance ;

        double distance_to_correct;

        double tof ;

        int32_t tofi ;

        // check for negative results and accept them making them proper negative integers

        tofi = (int32_t) tofx ; // make it signed

        if (tofi > 0x7FFFFFFF)  // close up TOF may be negative

        {

            tofi -= 0x80000000 ;  //

        }

        // convert device time units to seconds (as floating point)

        tof = tofi * DWT_TIME_UNITS ;

        inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;

#if (CORRECT_RANGE_BIAS == 1)

        //for the 6.81Mb data rate we assume gating gain of 6dB is used,

        //thus a different range bias needs to be applied

        //if(inst->configData.dataRate == DWT_BR_6M8)

        if(inst->smartPowerEn)

        {

                //1.31 for channel 2 and 1.51 for channel 5

                if(inst->configData.chan == 5)

                {

                        distance_to_correct = distance/1.51;

                }

                else //channel 2

                {

                        distance_to_correct = distance/1.31;

                        }

        }

        else

        {

                distance_to_correct = distance;

        }

        distance = distance - dwt_getrangebias(inst->configData.chan, (float) distance_to_correct, inst->configData.prf);

#endif

        if ((distance < 0) || (distance > 20000.000))    // discard any results less than <0 cm or >20 km

            return 0;

        inst_idist[idx] = distance;

        inst->longTermRangeCount++ ;                          // for computing a long term average

    return 1;

}// end of calculateRangeFromTOF

void instance_set_tagdist(int tidx, int aidx)

{

        inst_tdist[tidx] = inst_idist[aidx];

}

double instance_get_tagdist(int idx)

{

        return inst_tdist[idx];

}

void instance_cleardisttable(int idx)

{

        inst_idistraw[idx] = 0;

        inst_idist[idx] = 0;

}

void instance_cleardisttableall(void)

{

        int i;

        for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)

        {

                inst_idistraw[i] = 0xffff;

                inst_idist[i] = 0xffff;

        }

}

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

// Set this instance role as the Tag, Anchor

/*void instance_set_role(int inst_mode)

{

    // assume instance 0, for this

    inst->mode =  inst_mode;                   // set the role

}

*/

int instance_get_role(void)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

    return inst->mode;

}

int instance_newrange(void)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

        int x = inst->newRange;

    inst->newRange = TOF_REPORT_NUL;

    return x;

}

int instance_newrangeancadd(void)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

    return inst->newRangeAncAddress;

}

int instance_newrangetagadd(void)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

    return inst->newRangeTagAddress;

}

int instance_newrangetim(void)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

    return inst->newRangeTime;

}

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

// function to clear counts/averages/range values

//

void instance_clearcounts(void)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

    int i= 0 ;

    //inst->rxTimeouts = 0 ;

    //inst->txMsgCount = 0 ;

    //inst->rxMsgCount = 0 ;

    dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP

    inst->frameSN = 0;

    inst->longTermRangeCount  = 0;

    for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)

        {

            inst->tofArray[i] = INVALID_TOF;

        }

    for(i=0; i<MAX_TAG_LIST_SIZE; i++)

        {

                inst->tof[i] = INVALID_TOF;

        }

} // end instanceclearcounts()

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

// function to initialise instance structures

//

// Returns 0 on success and -1 on error

int instance_init(int inst_mode, DW1000Driver* drv)

{

        instance_data_t* inst = instance_get_local_structure_ptr(0);

    int result;

    inst->mode =  inst_mode;                                // assume listener,

    inst->twrMode = LISTENER;

    inst->testAppState = TA_INIT ;

    inst->instToSleep = FALSE;

    // Reset the IC (might be needed if not getting here from POWER ON)

    // ARM code: Remove soft reset here as using hard reset in the inittestapplication() in the main.c file

    //dwt_softreset();

        //this initialises DW1000 and uses specified configurations from OTP/ROM

    result = dwt_initialise(DWT_LOADUCODE, drv) ;

    //this is platform dependent - only program if DW EVK/EVB

    dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs

    if (DWT_SUCCESS != result)

    {

        return (-1) ;   // device initialise has failed

    }

    instance_clearcounts() ;

    inst->wait4ack = 0;

    inst->instanceTimerEn = 0;

    instance_clearevents();

#if (DISCOVERY == 1)

    dwt_geteui(inst->eui64);

    inst->panID = 0xdada ;

#else

    memset(inst->eui64, 0, ADDR_BYTE_SIZE_L);

    inst->panID = 0xdeca ;

#endif

    inst->tagSleepCorrection_ms = 0;

    dwt_setdblrxbuffmode(0); //disable double RX buffer

    // if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag

    // other errors which need to be checked (as they disable receiver) are

    //dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);

    dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);

    if(inst_mode == ANCHOR)

    {

            dwt_setcallbacks(tx_conf_cb, rx_ok_cb_anch, rx_to_cb_anch, rx_err_cb_anch);

    }

    else

    {

            dwt_setcallbacks(tx_conf_cb, rx_ok_cb_tag, rx_to_cb_tag, rx_err_cb_tag);

    }

    inst->monitor = 0;

    //inst->lateTX = 0;

    //inst->lateRX = 0;

    inst->remainingRespToRx = -1; //initialise

    inst->rxResps = 0;

    dwt_setlnapamode(1, 1); //enable TX, RX state on GPIOs 6 and 5

    inst->delayedTRXTime32h = 0;

#if (READ_EVENT_COUNTERS == 1)

    dwt_configeventcounters(1);

#endif

    return 0 ;

}

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

//

// Return the Device ID register value, enables higher level validation of physical device presence

//

uint32_t instance_readdeviceid(void)

{

    return dwt_readdevid() ;

}

//OTP memory addresses for TREK calibration data

#define TXCFG_ADDRESS  (0x10)

#define ANTDLY_ADDRESS (0x1C)

#define TREK_ANTDLY_1  (0xD)

#define TREK_ANTDLY_2  (0xE)

#define TREK_ANTDLY_3  (0xF)

#define TREK_ANTDLY_4  (0x1D)

extern uint8_t chan_idx[];

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

//

// function to allow application configuration be passed into instance and affect underlying device operation