AMiRo-OS

#define NUM_CH_SUPPORTED 8  //supported channels are '0', 1, 2, 3, 4, 5, '6', 7

#define PCODES 25           //supported preamble codes

extern const uint8_t chan_idx[NUM_CH_SUPPORTED];

/**

  const apalControlGpio_t* gpio_reset;  /**< @brief The GPIO indicating reset sig*/

//  const apalGpio_t* gpio_exti;    /**< @brief The GPIO indicating external interrupt */

//  const apalGpio_t* gpio_reset;  /**< @brief The GPIO indicating reset sig*/

} DW1000Driver;

    uint8_t nsSFD ;          //!< Boolean should we use non-standard SFD for better performance

    uint8_t dataRate ;       //!< Data Rate {DWT_BR_110K, DWT_BR_850K or DWT_BR_6M8}

    uint8_t phrMode ;        //!< PHR mode {0x0 - standard DWT_PHRMODE_STD, 0x3 - extended frames DWT_PHRMODE_EXT}

    uint16_t sfdTO ;         //!< SFD timeout value (in symbols)    

} dwt_config_t ;

 *

 * returns DWT_SUCCESS for success, or DWT_ERROR for error

 */

int dwt_initialise(uint16_t config, DW1000Driver* drv) ;

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

#define RX_RESPONSE1_TURNAROUND_6M81 (300) //takes about 100 us for response to come back

#define RX_RESPONSE1_TURNAROUND_110K (300) //takes about 100 us for response to come back

  //Tag will range to 3 or 4 anchors

  //Each ranging exchange will consist of minimum of 3 messages (Poll, Response, Final)

  //and a maximum of 6 messages (Poll, Response x 4, Final)

    uint8_t fcs[2] ;                              	//  125-126  we allow space for the CRC as it is logically part of the message. However ScenSor TX calculates and adds these bytes.

  } srd_msg_dssl ;

  {

    uint8_t channelNumber ;       // valid range is 1 to 11

    uint8_t preambleCode ;        // 00 = use NS code, 1 to 24 selects code

  //in turn and the event queued up before the instance processed the TX event.

#define MAX_EVENT_NUMBER (6)

  {

    uint8_t  type;			// event type - if 0 there is no event in the queue

    uint8_t  type_save;		// holds the event type - does not clear (used to show what event has been processed)

    uint8_t gotit;			//stores the instance function which processed the event (used for debug)

  }event_data_t ;

  // TX power and PG delay configuration structure

    uint8_t PGdelay;

    //TX POWER

    uint32_t txPwr[2]; //

  }tx_struct;

  {

    INST_MODE mode;				//instance mode (tag or anchor)

    int newRange;			//flag set when there is a new range to report TOF_REPORT_A2A or TOF_REPORT_T2A

    int newRangeAncAddress; //last 4 bytes of anchor address - used for printing/range output display

    int newRangeTagAddress; //last 4 bytes of tag address - used for printing/range output display

    uint8_t gatewayAnchor ; //set to TRUE = 1 if anchor address == GATEWAY_ANCHOR_ADDR

  void instance_close(void);

  // Call init, then call config, then call run. call close when finished

  // initialise the instance (application) structures and DW1000 device

  // configure the instance and DW1000 device

  void instance_config(instanceConfig_t *config, sfConfig_t *sfconfig, DW1000Driver* drv) ;

  // configure TX/RX callback functions that are called from DW1000 ISR

  void instance_rxcallback(const dwt_cb_data_t *rxd);

  void instance_txcallback(const dwt_cb_data_t *txd);

  void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd);

  void instance_rxerrorcallback(const dwt_cb_data_t *rxd);

  int instancenewrangetagadd(void);

  int instancenewrangepolltim(void);

  int instancenewrange(void);

  uint64_t convertmicrosectodevicetimeu (double microsecu);

  double convertdevicetimetosec(int32_t dt);

  void instance_notify_DW1000_inIDLE(int idle);

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

  uint32_t dwt_getotptxpower(uint8_t prf, uint8_t chan);

  uint16_t dwt_readantennadelay(uint8_t prf);

  // configure TX power

  void instanceconfigtxpower(uint32_t txpower);

  void instancesettxpower(void);

#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>

};

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

}; // end range25cm64PRFwb

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

 * Function: dwt_getrangebias()

 *

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

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

    } // end else

    mOffset *= 0.01 ;                                                   // convert to metres

    return (mOffset) ;

}

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

//

// Internal functions for controlling and configuring the device

// Read non-volatile memory

uint32_t _dwt_otpread(uint32_t address);

// Program the non-volatile memory

// Upload the device configuration into always on memory

void _dwt_aonarrayupload(void);

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

    pdw1000local->cbRxTo = NULL;

    pdw1000local->cbRxErr = NULL;

    // 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

    pdw1000local->longFrames = config->phrMode ;

    pdw1000local->sysCFGreg &= ~SYS_CFG_PHR_MODE_11;

    dwt_write32bitreg(SYS_CFG_ID,pdw1000local->sysCFGreg) ;

    // Set the lde_replicaCoeff

        nsSfd_result = 3 ;

        useDWnsSFD = 1 ;

    }

    dwt_write32bitreg(CHAN_CTRL_ID,regval) ;

    // Set up TX Preamble Size, PRF and Data Rate

    dwt_write32bitreg(TX_FCTRL_ID, pdw1000local->txFCTRL);

    // The SFD transmit pattern is initialised by the DW1000 upon a user TX request, but (due to an IC issue) it is not done for an auto-ACK TX. The

    // Write the frame length to the TX frame control register

    // pdw1000local->txFCTRL has kept configured bit rate information

    dwt_write32bitreg(TX_FCTRL_ID, reg32);

} // end dwt_writetxfctrl()

    // Write message header selecting WRITE operation and addresses as appropriate (this is one to three bytes long)

    if (index == 0) // For index of 0, no sub-index is required

    {

    }

    else

    {

#ifdef DWT_API_ERROR_CHECK

        assert((index <= 0x7FFF) && ((index + length) <= 0x7FFF)); // Index and sub-addressable area are limited to 15-bits.

#endif

        if (index <= 127) // For non-zero index < 127, just a single sub-index byte is required

        {

    }

    // Write it to the SPI

} // end dwt_writetodevice()

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

    }

    // Do the read from the SPI

} // end dwt_readfromdevice()

    int     j ;

    uint8_t   buffer[4] ;

    for (j = 3 ; j >= 0 ; j --)

    {

    uint16_t  regval = 0 ;

    uint8_t   buffer[2] ;

    return regval ;

} // end dwt_read16bitoffsetreg()

{

    uint8_t regval;

    return regval ;

}

 */

void dwt_write8bitoffsetreg(int regFileID, int regOffset, uint8_t regval)

{

}

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

    buffer[0] = regval & 0xFF;

    buffer[1] = regval >> 8 ;

} // end dwt_write16bitoffsetreg()

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

        regval >>= 8 ;

    }

} // end dwt_write32bitoffsetreg()

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

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

    {

    }

    _dwt_enableclocks(ENABLE_ALL_SEQ); // Restore system clock to PLL

    uint32_t ret_data;

    // Write the address

    // Perform OTP Read - Manual read mode has to be set

    dwt_write8bitoffsetreg(OTP_IF_ID, OTP_CTRL, OTP_CTRL_OTPREAD | OTP_CTRL_OTPRDEN);

 *

 * returns DWT_SUCCESS for success, or DWT_ERROR for error

 */

{

    uint8_t rd_buf[4];

    uint8_t wr_buf[4];

 *

 * returns DWT_SUCCESS for success, or DWT_ERROR for error

 */

{

    uint8_t rd_buf[1];

    uint8_t wr_buf[4];

    if (enable)

    {

        /* Configure ON/OFF times and enable PLL2 on/off sequencing by SNIFF mode. */

        dwt_write16bitoffsetreg(RX_SNIFF_ID, RX_SNIFF_OFFSET, sniff_reg);

        pmsc_reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET);

        pmsc_reg |= PMSC_CTRL0_PLL2_SEQ_EN;

    wr_buf[0] = 0x00; // Clear SAR enable

    dwt_writetodevice(TX_CAL_ID, TC_SARL_SAR_C,1,wr_buf);

}

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

        curr_bw = curr_bw | bit_field;

        // Write bw setting to PG_DELAY register

        // Set cal direction and time

        dwt_write8bitoffsetreg(TX_CAL_ID, TC_PGCCTRL_OFFSET, TC_PGCCTRL_DIR_CONV | TC_PGCCTRL_TMEAS_MASK);

            }

        }

        new_regval |= ((uint32_t) ((new_da_attn << 5) | new_mixer_gain)) << (i * 8);

    }

    dwt_write16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET, old_pmsc_ctrl1);

    dwt_write32bitreg(RF_CONF_ID, old_rf_conf_txpow_mask);

    return average_count;

}

    0xDECA0130                               // DW1000 - MP

/****************************************************************************************************************************************************

 *

 */

#pragma GCC optimize ("O3")

static int writetospi(uint16_t headerLength,

} // end writetospi()

/*! @brief sleep or idle the thread in millisecond */

void deca_sleep(unsigned int time_ms)

{

}

/*! @brief sleep or idle the thread in millisecond */

void Sleep(unsigned int time_ms)

{

}

  return chVTGetSystemTimeX();

}

/*! @brief Disable the interrupt handler */

void port_DisableEXT_IRQ(void){   

  nvicDisableVector(DW1000_EXTI_IRQn);

}

/*! @brief Enable the interrupt handler */

//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] = {

};

//these are default TREK Tag/Anchor antenna delays

const uint16_t rfDelaysTREK[2] = {

};

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

//      Data Definitions

static double inst_idist[MAX_ANCHOR_LIST_SIZE] ;

static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ;

instance_data_t instance_data[NUM_INST] ;

{

    uint32_t      deviceID ;

    uint8_t       chan;               // added chan here - used in the reading of acc

  uint64_t dt;

  long double dtime;

  dt =  (uint64_t) (dtime) ;

  double distance ;

  double distance_to_correct;

  double tof ;

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

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

  }

  // convert to seconds (as floating point)

  inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;

#if (CORRECT_RANGE_BIAS == 1)

// Set this instance role as the Tag, Anchor or Listener

void instancesetrole(int inst_mode){

  // assume instance 0, for this

}

int instancegetrole(void){

}

int instancenewrange(void){

  return instance_data[0].newRangeTagAddress;

}

  return instance_data[0].newRangeTime;

}

  //dwt_softreset();

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

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

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

  dw_event.uTimeStamp = portGetTickCnt();

  instance_data[instance].monitor = 0;

}

  case 2:

  default:

    if(instance_data[instance].responseTO > 0) {  //can get here as result of error frame so need to check

      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;

      type_pend = DWT_SIG_RX_PENDING ;

    }

  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;

  //set the delayed rx on time (the final message will be sent after this delay)

  instance_data[instance].tagSleepCorrection = 0;

  instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;

  //we have our range - update the own mask entry...

  if(instance_data[instance].tof[tof_idx] != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array

    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = instance_data[instance].tof[tof_idx];

  }

  else	{  //reset response mask

    instance_data[instance].rxResponseMask = 0;	//reset the mask of received responses when rx poll

  }

  //set the delayed rx on time (the final message will be sent after this delay)

  //if this is gateway anchor - calculate the slot period correction

  if(instance_data[instance].gatewayAnchor)  {

  //if Listener then just report the received frame to the instance (application)

  if(rxd_event == DWT_SIG_RX_OKAY) { //Process good/known frame types

    if(instance_data[instance].mode != LISTENER)      {

      if(instance_data[instance].mode == TAG) //if tag got a good frame - this is probably a response, but could also be some other non-ranging frame

          instance_data[instance].rxMsgCount++;

          return;

        }

            + ((sourceAddress&0x7) << 4));

        instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;

          instance_data[instance].rxMsgCount++;

          return;

        }

//        __attribute__ ((fallthrough));

        //if anchor fall into case below and process the frame

}

#pragma GCC optimize ("O3")

void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd){

}

#pragma GCC optimize ("O3")

void instance_rxerrorcallback(const dwt_cb_data_t *rxd) {

  dw_event_g.type = instance_data[instance].dwevent[indexOut].type ;

  //instance_data[instance].dwevent[indexOut].eventtimeclr = portGetTickCnt();

    message = 0;

  }

  if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) { //we are in RX and need to timeout (Tag needs to send another poll if no Rx frame)

    if(instance_data[instance].mode == TAG) { //Tag (is either in RX or sleeping)

      int32_t nextPeriod ;

  instance_data[0].txPower = txpower ;

  instance_data[0].txPowerChanged = 1;

}

void instancesettxpower(void){

#endif

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

 * @fn dwt_getotptxpower()

#include <v1/deca_instance_v1.h>

#include <string.h>

#include "module.h"

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

//

// function to construct the message/frame header bytes

//

  }

  //begin delayed TX of frame

    result = 1; //late/error

    inst->lateTX++;

  }

  return result;                                              // state changes

}

int instance_calcranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask){

  int i;

  int newRange = TOF_REPORT_NUL;

      sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV);

      if(inst->configData.txPreambLength == DWT_PLEN_64)  //if using 64 length preamble then use the corresponding OPSet

        sleep_mode |= DWT_LOADOPSET;

        dwt_seteui(inst->eui64);

      }

#else

#endif

      instancesetantennadelays(); //this will update the antenna delay if it has changed

          else {  //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses)

            tagCalculatedFinalTxTime =  (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS;

          }

          // Calculate Time Final message will be sent and write this field of Final message

          // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by

          // zeroing its low 9 bits, and then having the TX antenna delay added

        message = 0;

        //fall into the next case (turn on the RX)

      }

    }

    break ; // end case TA_TX_WAIT_CONF

      // end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.

      if(message == 0) break;

    }

    break;

      //printf("TA_RX_WAIT_DATA %d", message) ;

      switch (message){

        //int srclen = 0;

        //int fctrladdr_len;

        uint8_t tof_idx  = 0;

        inst->stopTimer = 0; //clear the flag, as we have received a message

                }

              }

            }

            if(fcode == RTLS_DEMO_MSG_ANCH_RESP) { //tag to anchor mode

              if(currentRangeNum == inst->rangeNum) { //these are the previous ranges...

            uint64_t tagFinalRxTime  = 0;

            uint64_t tagPollTxTime  = 0;

            uint64_t anchorRespRxTime  = 0;

            double RaRbxDaDb = 0;

            double RbyDb = 0;

              RayDa = ((double)Ra + (double)Da);

            }

            //tag to anchor ranging

            if(RTLS_DEMO_MSG_TAG_FINAL == fcode) {

              inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8);

              //time-of-flight

              //calculate all tag - anchor ranges... and report

              inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);

              inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report

              inst->rxResponseMask = 0;

              //we have our range - update the own mask entry...

                setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array

              }

              inst->newRangeTime = dw_event->uTimeStamp ;

            }

            else { //anchor to anchor ranging

              inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8);

              //time-of-flight

            }

            //reset the response count

  float msgdatalen = 0;

  float preamblelen = 0;

  int sfdlen = 0;

  //Set the RX timeouts based on the longest expected message - the Final message

  //Poll = 13, Response = 20, Final = 44 bytes

  //msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;

  msgdatalen = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;

  msgdatalen = msgdatalen*8 + x*48;

  //add some margin so we don't timeout too soon

  margin = 0; //(TAG_FINAL_MSG_LEN - TAG_POLL_MSG_LEN);

  //assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M

  if(instance_data[instance].configData.dataRate == DWT_BR_110K) {

    preamblelen = (sfdlen + preamblelen) * 1.01763f;

  }

  //this is the delay used for the delayed transmit (when sending the response, and final messages)

  instance_data[instance].pollTx2FinalTxDelay = convertmicrosectodevicetimeu (delayus);

  //andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...

  //this delay depends on how quickly the tag can receive and process the message from previous anchor

  //(and also the frame length of course)

  if(instance_data[instance].configData.dataRate == DWT_BR_110K) {

    //set the frame wait timeout time - total time the frame takes in symbols

    instance_data[instance].fwtoTimeAnc_sy = respframe_sy; //add some margin so we don't timeout too soon

    instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_110K);

    instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_110K ;

  }

    instance_data[instance].fwtoTimeAnc_sy =  respframe_sy;

    instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_6M81);

    instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_6M81 ;

  }

}

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