AMiRo-OS

#define AMIROLLD_CFG_DW1000              1

Subproject commit 7df78c60db679f32c180cc23776e5e449fc7fab5

#ifndef AMIROOS_UT_ALLD_DW1000_V1_H

#define AMIROOS_UT_ALLD_DW1000_V1_H

#include <amiroos.h>

#if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0)) || defined(__DOXYGEN__)

#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0) */

#endif /* AMIROOS_UT_ALLD_DW1000_V1_H */

/*

AMiRo-OS is an operating system designed 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 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 General Public License for more details.

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

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

*/

#ifndef _AMIROOS_UT_DW1000_LLD_H_

#define _AMIROOS_UT_DW1000_LLD_H_

#include <aos_unittest.h>

#include <amiro-lld.h>

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

#include <alld_DW1000.h>

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

/* CONSTANTS                                                                  */

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

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

/* SETTINGS                                                                   */

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

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

/* CHECKS                                                                     */

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

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

/* DATA STRUCTURES AND TYPES                                                  */

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

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

/* MACROS                                                                     */

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

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

/* EXTERN DECLARATIONS                                                        */

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

#if defined(__cplusplus)

extern "C" {

#endif /* defined(__cplusplus) */

  aos_utresult_t utAlldDw1000Func(BaseSequentialStream* stream, aos_unittest_t* ut);

  extern uint8_t s1switch;

  extern int instance_anchaddr;

  extern int dr_mode;

  extern int chan, tagaddr, ancaddr;

  extern int instance_mode;

  int UWB_Init(DW1000Driver* drv);

  int32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv);

  int decarangingmode(uint8_t s1switch);

  void addressconfigure(uint8_t s1switch, uint8_t mode);

  void set_SPI_chip_select(void);

  void clear_SPI_chip_select(void);

  void reset_DW1000(void);

  void setHighSpeed_SPI(bool speedValue, DW1000Driver* drv);

  apalGpioState_t port_CheckEXT_IRQ(void) ;

  void process_deca_irq(void);

#if defined(__cplusplus)

}

#endif /* defined(__cplusplus) */

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

/* INLINE FUNCTIONS                                                           */

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

#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0) */

#endif /* _AMIROOS_UT_DW1000_LLD_H_ */

#include <ut_alld_DW1000_v1.h>

#if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0)) || defined(__DOXYGEN__)

    aosDbgCheck(ut->data != NULL);

    aos_utresult_t result = {0, 0};

    chprintf(stream, "init DW1000...\n");

    dwt_initialise(DWT_LOADUCODE, (DW1000Driver*) ut->data);

    chprintf(stream, "device ID should be: 0xDECA0130\nget device ID...\n");

    uint32_t actual_deviceId = dwt_readdevid();

    chprintf(stream, "actual device ID is: 0x%x\n", actual_deviceId);

    aosThdMSleep(1);

    if (actual_deviceId == DWT_DEVICE_ID){

      aosUtPassed(stream, &result);

    } else {

      aosUtFailed(stream, &result);

    }

    /*chprintf(stream, "write-read test...\n");

    uint32_t testvalue = 0x0A;

    uint16_t panid = 0x00;

    dwt_readfromdevice(PANADR_ID, PANADR_PAN_ID_OFFSET, 2, (uint8_t*) &panid);

    chprintf(stream, "value PANADR register before write: %x\n", panid);

    chprintf(stream, "write 0x%x to PANADR register...\n", testvalue);

    dwt_setpanid(testvalue);

    dwt_readfromdevice(PANADR_ID, PANADR_PAN_ID_OFFSET, 2, (uint8_t*) &panid);

    chprintf(stream, "PANADR register is now: 0x%x\n", panid);

    if (panid == testvalue){

      aosUtPassed(stream, &result);

    } else {

      aosUtFailed(stream, &result);

    }

*/

    dwt_setleds(0x03);

    // RUN DECA-DEMO

    instanceConfig_t chConfig;

    chConfig.channelNumber = 2;            // channel

    chConfig.preambleCode = 4;             // preambleCode

    chConfig.pulseRepFreq = DWT_PRF_16M;   // prf

    chConfig.dataRate = DWT_BR_6M8;        // datarate

    chConfig.preambleLen = DWT_PLEN_128;   // preambleLength

    chConfig.pacSize = DWT_PAC8;           // pacSize

    chConfig.nsSFD = 0;                    // non-standard SFD

    chConfig.sfdTO = (129 + 8 - 8);        // SFD timeout

    sfConfig_t sfConfig;

    sfConfig.slotDuration_ms = (10);        //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time

                                            //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time

    sfConfig.numSlots = (10);               //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),

    sfConfig.sfPeriod_ms = (10*10);         //in ms => 100 ms frame means 10 Hz location rate

    sfConfig.tagPeriod_ms = (10*10);        //tag period in ms (sleep time + ranging time)

    sfConfig.pollTxToFinalTxDly_us = (2500); //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)

    //TODO Disable EXTI IRQ

    //port_DisableEXT_IRQ();

    // inittestapplication

    // dwt_softreset(); // already done in instance_init()

    // Set this instance mode (tag/anchor)

    (void) instance_init(TAG, (DW1000Driver*) ut->data);

    //int err = instance_init(TAG, (DW1000Driver*) ut->data);

    (void) instance_readdeviceid();

    //uint32_t deca_dev_id = instance_readdeviceid();

    // TAG ID 0

    instance_set_16bit_address(0);

    // simulate DECA config Mode 2 (DIP 1100000)

    instance_config(&chConfig, &sfConfig) ;

    //TODO Enable EXTI IRQ

    //port_EnableEXT_IRQ();

    // Start Ranging

    chprintf(stream, "start ranging...\n");

    while(1) {

        //int n = 0;

        instance_data_t* inst = instance_get_local_structure_ptr(0);

        int monitor_local = inst->monitor ;

        int txdiff = (chVTGetSystemTimeX() - inst->timeofTx);

        tag_run();

        //if delayed TX scheduled but did not happen after expected time then it has failed... (has to be < slot period)

        //if anchor just go into RX and wait for next message from tags/anchors

        //if tag handle as a timeout

        if( (monitor_local == 1) && ( txdiff > inst->slotDuration_ms) )  {

            inst->wait4ack = 0;

            tag_process_rx_timeout(inst);

            inst->monitor = 0;

        }

    }

    (void) instance_newrange();

    //int rx = instance_newrange();

    return result;

#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0) */

/*

AMiRo-OS is an operating system designed 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 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 General Public License for more details.

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

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

*/

#include <amiroos.h>

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

#include <aos_debug.h>

#include <chprintf.h>

#include <aos_thread.h>

#include <math.h>

#include <module.h>

#include <alld_DW1000.h>

#include <v1/deca_instance_v1.h>

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

/* LOCAL DEFINITIONS                                                          */

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

//#define UNIT_TEST_SNIPPETS_DW1000   // switch between unit test and demo apps

#define SWS1_SHF_MODE 0x02  //short frame mode (6.81M)

#define SWS1_CH5_MODE 0x04  //channel 5 mode

#define SWS1_ANC_MODE 0x08  //anchor mode

#define SWS1_A1A_MODE 0x10  //anchor/tag address A1

#define SWS1_A2A_MODE 0x20  //anchor/tag address A2

#define SWS1_A3A_MODE 0x40  //anchor/tag address A3

#define S1_SWITCH_ON  (1)

#define S1_SWITCH_OFF (0)

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

/* EXPORTED VARIABLES                                                         */

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

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

/* LOCAL TYPES                                                                */

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

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

/* LOCAL VARIABLES                                                            */

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

uint8_t s1switch = 0;

int instance_anchaddr = 0;

int dr_mode = 0;

int chan, tagaddr, ancaddr;

int instance_mode = ANCHOR;

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

/* LOCAL FUNCTIONS                                                            */

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

/*! @brief Change the SPI speed configuration on the fly */

void setHighSpeed_SPI(bool speedValue, DW1000Driver* drv){

  spiStop(drv->spid);

  if (speedValue == FALSE){

    spiStart(drv->spid, &moduleHalSpiUwbLsConfig);  // low speed spi configuration

  }

  else{

    spiStart(drv->spid, &moduleHalSpiUwbHsConfig); // high speed spi configuration

  }

}

/*! @brief entry point to the IRQn event in DW1000 module

 *

 * */

void process_deca_irq(void){

  do{

    dwt_isr();

    //while IRS line active (ARM can only do edge sensitive interrupts)

  }while(port_CheckEXT_IRQ() == 1);

}

/*! @brief Check the current value of GPIO pin and return the value */

apalGpioState_t port_CheckEXT_IRQ(void) {

  apalGpioState_t  val;

  apalGpioRead(moduleGpioDw1000Irqn.gpio, &val);

  return val;

}

/*! @brief Manually set the chip select pin of the SPI */

void set_SPI_chip_select(void){

  apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_HIGH);

}

/*! @brief Manually reset the chip select pin of the SPI */

void clear_SPI_chip_select(void){

  apalGpioWrite(moduleGpioSpiChipSelect.gpio, APAL_GPIO_LOW);

}

/*! @brief Manually reset the DW1000 module  */

void reset_DW1000(void){

  // Set the pin as output

  palSetPadMode(moduleGpioDw1000Reset.gpio->port, moduleGpioDw1000Reset.gpio->pad, APAL_GPIO_DIRECTION_OUTPUT);

  //drive the RSTn pin low

  apalGpioWrite(moduleGpioDw1000Reset.gpio, APAL_GPIO_LOW);

  //put the pin back to tri-state ... as input

//  palSetPadMode(moduleGpioDw1000Reset.gpio->port, moduleGpioDw1000Reset.gpio->pad, APAL_GPIO_DIRECTION_INPUT); // TODO:

  aosThdMSleep(2);

}

/*! @brief Configure instance tag/anchor/etc... addresses */

void addressconfigure(uint8_t s1switch, uint8_t mode){

  uint16_t instAddress ;

  instance_anchaddr = (((s1switch & SWS1_A1A_MODE) << 2) + (s1switch & SWS1_A2A_MODE) + ((s1switch & SWS1_A3A_MODE) >> 2)) >> 4;

  if(mode == ANCHOR) {

    if(instance_anchaddr > 3) {

      instAddress = GATEWAY_ANCHOR_ADDR | 0x4 ; //listener

    }

    else {

      instAddress = GATEWAY_ANCHOR_ADDR | (uint16_t)instance_anchaddr;

    }

  }

  else{

    instAddress = (uint16_t)instance_anchaddr;

  }

  instancesetaddresses(instAddress);

}

/*! @brief returns the use case / operational mode */

int decarangingmode(uint8_t s1switch){

  int mode = 0;

  if(s1switch & SWS1_SHF_MODE)  {

    mode = 1;

  }

  if(s1switch & SWS1_CH5_MODE) {

    mode = mode + 2;

  }

  return mode;

}

/*! @brief Check connection setting and initialize DW1000 module */

int32_t inittestapplication(uint8_t s1switch, DW1000Driver* drv){

  uint32_t devID ;

  int result;

  setHighSpeed_SPI(FALSE, drv);          //low speed spi max. ~4M

  devID = instancereaddeviceid() ;

  if(DWT_DEVICE_ID != devID) {

    clear_SPI_chip_select();

    Sleep(1);

    set_SPI_chip_select();

    Sleep(7);

    devID = instancereaddeviceid() ;

    if(DWT_DEVICE_ID != devID){

      return(-1) ;

    }    

    dwt_softreset();

  }

    reset_DW1000();  //reset the DW1000 by driving the RSTn line low

  if((s1switch & SWS1_ANC_MODE) == 0){

    instance_mode = TAG;

  }

  else{

    instance_mode = ANCHOR;

  }

  result = instance_init(drv) ;

  if (0 > result){

    return(-1) ;

  }

  setHighSpeed_SPI(TRUE, drv);       // high speed spi max. ~ 20M

  devID = instancereaddeviceid() ;

  if (DWT_DEVICE_ID != devID){

    return(-1) ;

  }

  addressconfigure(s1switch, (uint8_t)instance_mode) ;

  if((instance_mode == ANCHOR) && (instance_anchaddr > 0x3)){

    instance_mode = LISTENER;

  }

  instancesetrole(instance_mode) ;       // Set this instance role

  dr_mode = decarangingmode(s1switch);

  chan = chConfig[dr_mode].channelNumber ;

  instance_config(&chConfig[dr_mode], &sfConfig[dr_mode], drv) ;

  return (int32_t)devID;

}

/*! @brief Main Entry point to Initialization of UWB DW1000 configuration  */

#pragma GCC optimize ("O3")

int UWB_Init(DW1000Driver* drv){

  /*! Software defined Configurartion for TAG, ANC, and other settings as needed */

  s1switch = S1_SWITCH_OFF << 1  // (on = 6.8 Mbps, off = 110 kbps)

           | S1_SWITCH_OFF << 2  // (on = CH5, off = CH2)

           | S1_SWITCH_OFF << 3  // (on = Anchor, off = TAG)

           | S1_SWITCH_OFF << 4  // (configure Tag or anchor ID no.)

           | S1_SWITCH_OFF << 5  // (configure Tag or anchor ID no.)

           | S1_SWITCH_OFF << 6  // (configure Tag or anchor ID no.)

           | S1_SWITCH_OFF << 7; // Not use in this demo

  port_DisableEXT_IRQ();           //disable ScenSor IRQ until we configure the device

  if(inittestapplication(s1switch, drv) == -1) {

    return (-1); //error

  }

  aosThdMSleep(5);

  port_EnableEXT_IRQ();  //enable DW1000 IRQ before starting

  return 0;

}

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

/* EXPORTED FUNCTIONS                                                         */

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

aos_utresult_t utAlldDw1000Func(BaseSequentialStream* stream, aos_unittest_t* ut) {

  aosDbgCheck(ut->data != NULL);

  aos_utresult_t result = {0, 0};

  chprintf(stream, "init DW1000...\n");

  dwt_initialise(DWT_LOADUCODE, (DW1000Driver*) ut->data);

  aosThdMSleep(5);

/*! Unit Test snippets for DW1000.

 * @Note: Event IRQ for DW1000 should be tested separately

 */

#if defined(UNIT_TEST_SNIPPETS_DW1000)

  uint32_t actual_deviceId;

  port_DisableEXT_IRQ();

  setHighSpeed_SPI(false, (DW1000Driver*) ut->data);

  chprintf(stream, "expected device ID (LS SPI): 0xDECA0130 \n");

  aosThdMSleep(5);

  actual_deviceId = instancereaddeviceid();

  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);

  aosThdMSleep(5);

  //if the read of device ID fails, the DW1000 could be asleep

  if(DWT_DEVICE_ID != actual_deviceId){

    clear_SPI_chip_select();

    aosThdMSleep(1);

    set_SPI_chip_select();

    aosThdMSleep(7);

    actual_deviceId = instancereaddeviceid() ;

    if(DWT_DEVICE_ID != actual_deviceId){

      chprintf(stream, "SPI is not working or Unsupported Device ID\n");

      chprintf(stream, "actual device ID is: 0x%x\n", actual_deviceId);

      chprintf(stream, "expected device ID: 0xDECA0130 \n");

      aosThdMSleep(5);

    }

    //clear the sleep bit - so that after the hard reset below the DW does not go into sleep

    dwt_softreset();

  }

  /*! UT1: Low speed SPI result */

  if (actual_deviceId == DWT_DEVICE_ID){

    aosUtPassed(stream, &result);

  } else {

    aosUtFailed(stream, &result);

  }

  reset_DW1000();

  chprintf(stream, "initialise instance for DW1000 \n");

  aosThdSleep(1);

  int x_init = instance_init((DW1000Driver*) ut->data) ;

  if (0 != x_init){

    chprintf(stream, "init error with return value: %d \n", x_init);

    aosThdSleep(1);

  }

  else {

    chprintf(stream, "init success with return value: %d \n", x_init);

    aosThdSleep(1);

  }

  /*! UT2: Initialization  result*/

  if (x_init == 0){

    aosUtPassed(stream, &result);

  } else {

    aosUtFailed(stream, &result);

  }

  setHighSpeed_SPI(true, (DW1000Driver*) ut->data);

  chprintf(stream, "expected device ID (HS SPI): 0xDECA0130\n");

  actual_deviceId = instancereaddeviceid();

  chprintf(stream, "actual read ID: 0x%x\n", actual_deviceId);

  aosThdMSleep(1);

  /*! UT3: High speed SPI result*/

  if (actual_deviceId == DWT_DEVICE_ID){

    aosUtPassed(stream, &result);

  } else {

    aosUtFailed(stream, &result);

  }

  port_EnableEXT_IRQ();

  reset_DW1000();

  chprintf(stream, "initialise the configuration for UWB application \n");

  aosThdSleep(1);

  int uwb_init = UWB_Init((DW1000Driver*) ut->data);

  if (0 != uwb_init){

    chprintf(stream, "UWB config error with return value: %d \n", uwb_init);

    aosThdSleep(1);

  }

  else {

    chprintf(stream, "UWB config success with return value: %d \n", uwb_init);

    aosThdSleep(1);

  }

  /*! UT4: UWB configuration result

   * If all the four unit tests are passed, the module is ready to run.

   * Note that the interrupt IRQn should be tested separately.

   */

  if (uwb_init == 0){

    aosUtPassed(stream, &result);

  } else {

    aosUtFailed(stream, &result);

  }

  /************** End of UNIT_TEST_SNIPPETS_DW1000*****************/

#else /* defined(UNIT_TEST_SNIPPETS_DW1000) */

  /*! RUN THE STATE MACHINE DEMO APP (RTLS) */

  chprintf(stream, "initialise the State Machine \n");

  aosThdSleep(2);

  /* Initialize UWB system with user defined configuration  */

  int uwb_init = UWB_Init((DW1000Driver*) ut->data);

  if (0 != uwb_init){

    chprintf(stream, "error in UWB config with return value: %d \n", uwb_init);

  }

  else {

    chprintf(stream, "succeed the init of UWB config \n");

  }

  aosThdSleep(1);

  chprintf(stream, "running the RTLS demo application ... \n");

  aosThdSleep(1);

  /*! Run the localization system demo app as a thread */

  while(1){

    instance_run();

//    aosThdUSleep(10);

  }

#endif  /* defined(UNIT_TEST_SNIPPETS_DW1000) */

  return result;

}

#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */