File content as of revision 1:346279def7ac:

#include "mbed.h"


#include "SMConfig.h"
#include "DecaWave.h"                  // DW1000 functions      
//#include "LMMN2WR.h"                   // 2-Way-Ranging
#include "PC.h"                        // Serial Port via USB for debugging with Terminal
#include "Watchdog.h"                  // Resets Program if it gets stuck
#include "nodes.h"
 #include <stddef.h>

#define NELEMS(x)  (sizeof(x) / sizeof((x)[0]))

// Function Prototypes
void dwCallbackTx(const dwt_cb_data_t *rxd);
void dwCallbackRx(const dwt_cb_data_t *rxd);
int min (int a, int b);
void configureDW1000(uint8_t dwMode);
void rangeAndDisplayOne(uint8_t addr);
void rangeAndDisplayAll();
void executeOrder(char* command);


// PA_7 MOSI, PA_6 MISO, PA_5 SCLK, PB_6 CS, PB_9 IRQ
SPI         decaWaveSpi(PA_7, PA_6, PA_5);      // Instance of SPI connection to DW1000
DigitalOut  decaWaveCs(PB_6);
InterruptIn decaWaveIrq(PB_9);
DecaWave    decaWave = DecaWave();              // Instance of the DW1000


/* BIT and PINS
BIT 1 = PA_10
BIT 2 = PB_3
BIT 3 = PB_5
BIT 4 = PB_4
BIT 5 = PB_10
BIT 6 = PA_8
BIT 7 = PA_9
BIT 8 = PC_7
*/


BusIn adressInput(PA_9,PA_8,PB_10,PB_4,PB_5,PB_3,PA_10); // first seven 7 bit for ID settings, most left bit = most significant bit
PC          pc(USBTX, USBRX, 921600);           // USB UART Terminal
DigitalIn anchorInput(PC_7); // usage of last bit as deciding bit for: anchor or beacon
Watchdog    wdt = Watchdog();
BeaconNode  beaconNode(decaWave);
AnchorNode  anchorNode(decaWave);
BaseStationNode  baseStationNode(decaWave);
Node        *node;


int main() {
  
  decaWaveSpi.frequency(20000000); // Crank up the SPI frequency from 1Mhz to 3Mhz (maybe more?)

  // Check if Reset was from Watchdog or externally
  if(RCC->CSR&0x20000000)
    pc.printf("\r\n\r\n --- !!!!WATCHDOG RESET!!!! --- \r\n\r\n", RCC->CSR); 
  else if(RCC->CSR&0x4000000){
    pc.printf("\r\n\r\n --- External Reset --- \r\n\r\n", RCC->CSR);
  //wdt.kick();
    }    
    __HAL_RCC_CLEAR_RESET_FLAGS();

  // Set all Switches Pull-Down so that it is zero if Switch is not set   

    adressInput.mode(PullDown);    
  anchorInput.mode(PullDown);
  //modeInput.mode(PullDown);
  wait_ms(50);
  
    baseStationNode.setAddress(adressInput & adressInput.mask());  
    anchorNode.setAddress(adressInput & adressInput.mask());  
    beaconNode.setAddress(adressInput & adressInput.mask());  
    
    if((adressInput & adressInput.mask()) == BASE_STATION_ADDR){
        node = &baseStationNode; 
        pc.printf("This node is the Base Station, Adress: %d \r\n \r\n \r\n", node->getAddress());        
       // wdt.kick(2);            // Set up WatchDog         
   
    }
    else if(anchorInput){
        node = &anchorNode;  
        pc.printf("This node is an Anchor node, Adress: %d \r\n \r\n \r\n", node->getAddress()); 
        wdt.kick(2);            // Set up WatchDog         
    }     
    else{
        node = &beaconNode; 
        pc.printf("This node is a Beacon, Adress: %d \r\n \r\n \r\n", node->getAddress());
        wdt.kick(2);            // Set up WatchDog   
    }

    //pc.printf(" Adress: %d \r\n \r\n \r\n", node->getAddress());
       
    configureDW1000(7); 
 
    // TODO turn on RXMode regularly (every couple seconds)
    
    char command_str[30];
    
    while(1) {
        
        // Choose between what to execte based on whether this device is a:
        // BEACON
        // BASESTATION
        // ANCHOR
        if (!node->isAnchor() && !node->isBaseStation()){
            
            // THE BEACON EXECUTES THIS
            if(beaconNode.getRepetitions() > 0){
                switch(beaconNode.getMode()){
                    case RANGE_ALL: rangeAndDisplayAll(); 
                                    break;
                    case RANGE_ONE: rangeAndDisplayOne(beaconNode.getDestination());
                                    break;
                    default:        break;
                }
                beaconNode.decreaseRepetitions();
            }
            else{
                beaconNode.clearRec();
                wait_ms(8);
            }
            wdt.kick(); 
        } 
        else if (node->isBaseStation()){
             
             // EXECUTE THIS IF A BASE STATION
            pc.readcommand(executeOrder);
                        wait_ms(80);

            wdt.kick();
        }
        else { // All Anchor Action is in the Interrupt functions!
            // EXECUTE THIS IF AN ANCHOR
            wait_ms(10);
            wdt.kick(); 
        }
    }  

    
}


void executeOrder(char* command){

    int repetitions = command[3]*100 + command[4]*10 + command[5] - 5328;
    //uint8_t dest1 = command[7] - 48;
    uint8_t dest2 = command[8] - 48;
    uint8_t dest3 = command[9] - 48;
    uint8_t dest1=0;
  
    
   
    
    if(command[7] == 0 && command[8] == 0)
    {
    dest1 =    command[9] - 48;
    }
    else if (command[7] == 0 && command[8] != 0)
    {
    dest1 =     command[8] * 10 + command[9] - 528;
    }
    else if (command[7] != 0 && command[8] != 0) 
    {
    dest1 = command[7] * 100 + command[8] * 10 + command[9] - 5328;
    }
    
    if (strncmp(command, "reset", 5) == 0){ // This command is implemented in order to be able to reset BaseStation from Matlab. 
        wdt.kick(2);            // Set up WatchDog   
        pc.printf("Base Station is RESETTED  \r\n\r\n");
    }
    
    
    else if (strncmp(command, "all", 3) == 0){
        baseStationNode.sendOrder(0, NOT_USED, RANGE_ALL, repetitions, Node::BASE_ORDER); 
       // pc.printf("Mode: Range all \r\n");
    }
    else if (strncmp(command, "one", 3) == 0){
        
        
        
        if(dest1 != 15)
        {
           
            baseStationNode.sendOrder(0, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        }
        else
        {
            baseStationNode.sendOrder(0, 1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        }
    }
    
    else if (strncmp(command, "bea", 3) == 0){
        if(dest1 < 15)        
            baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
        else
            baseStationNode.sendOrder(0, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
         
    }
    
    else if (strncmp(command, "anc", 3) == 0){
        if(dest1 < 15)   
            baseStationNode.sendOrder(dest1, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); 
        else
            baseStationNode.sendOrder(0, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); 
    }
    
    else if (strncmp(command, "tri", 3) == 0){
        // Switch them in correct modes (should already be the case)
        baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);  
        baseStationNode.sendOrder(dest3, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);  
        
        // Ranging from first node
        baseStationNode.sendOrder(dest1, dest2, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        wait_ms(10*repetitions);
        baseStationNode.sendOrder(dest1, dest3, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        wait_ms(10*repetitions);
        
        // Mode Switches
        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);  
        baseStationNode.sendOrder(dest1, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);
        
        // Rangings
        baseStationNode.sendOrder(dest2, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        wait_ms(10*repetitions);        
        baseStationNode.sendOrder(dest2, dest3, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        wait_ms(10*repetitions);
        
        // Mode Switches
        baseStationNode.sendOrder(dest3, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);  
        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);
        
        // Rangings
        baseStationNode.sendOrder(dest3, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        wait_ms(10*repetitions);        
        baseStationNode.sendOrder(dest3, dest2, RANGE_ONE, repetitions, Node::BASE_ORDER); 
        wait_ms(10*repetitions);        
        
        // Back to original modes
        baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);  
        baseStationNode.sendOrder(dest3, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);
        
    } 
  
}

#pragma Otime // Compiler optimize Runtime at the cost of image size
// Called after Frame was received
void dwCallbackRx(const dwt_cb_data_t *rxd) {
  int64_t rxTimeStamp = 0;  
  dwt_readrxdata(node->getRecFrameRef(), min(node->getRecFrameLength(), rxd->datalength), 0);  // Read Data Frame from Registers 

  dwt_readrxtimestamp((uint8_t*) &rxTimeStamp);  // Read Timestamp when the frame was received exactly
  rxTimeStamp &= MASK_40BIT; //Mask the 40 Bits of the timestamp
  
  node->callbackRX(rxTimeStamp); // Two Way Ranging Function
}


#pragma Otime // Compiler optimize Runtime at the cost of image size
// Called after Frame was transmitted
void dwCallbackTx(const dwt_cb_data_t *txd) {
  int64_t txTimeStamp = 0;
  dwt_readtxtimestamp((uint8_t*) &txTimeStamp);  // Read Timestamp when the frame was transmitted exactly
  txTimeStamp &= MASK_40BIT; // Delete the most significant 8 Bits because the timestamp has only 40 Bits

  node->callbackTX(txTimeStamp); // Two Way Ranging Function
}


int min (int a, int b){
    if(a<=b) return a;
    return b;
}

void configureDW1000(uint8_t dwMode){
  dwt_config_t dwConfig;
  dwt_txconfig_t dwConfigTx;
    
  SMsetconfig(dwMode, &dwConfig, &dwConfigTx);

  decaWave.setup(dwConfig, dwConfigTx, rfDelays[dwConfig.prf - DWT_PRF_16M], dwCallbackTx, dwCallbackRx);

  pc.printf("%s\r\n", DW1000_DEVICE_DRIVER_VER_STRING);
  {
    uint16_t tempvbat = dwt_readtempvbat(1);
    if (tempvbat>0) {
      float tempC = 1.13f * (float) (tempvbat >> 8) - 113.0f;
      float vbatV = 0.0057f * (float) (tempvbat & 0xFF) + 2.3f;

      pc.printf("  Voltage: %f, Temperature: %f\r\n", vbatV, tempC);
    } else {
      pc.printf("ERROR: Cannot read voltage/temperature\r\n");
    }
  }
  pc.printf("  Device Lot ID %lu, Part ID %lu\r\n", dwt_getlotid(), dwt_getpartid());

  uint16_t DWID = (dwt_getpartid() & 0xFFFF);

  pc.printf(
      "  Settings %i: \r\n    Channel %u (%1.3f GHz w/ %1.3f GHz BW), Datarate %s, \r\n    PRF %s, Preamble Code %u, PreambleLength %u symbols, \r\n    PAC Size %u, %s SFD, SDF timeout %ul symbols\r\n",
      dwMode, dwConfig.chan, ChannelFrequency[dwConfig.chan],
    ChannelBandwidth[dwConfig.chan], ChannelBitrate[dwConfig.dataRate], ChannelPRF[dwConfig.prf],
      dwConfig.txCode, ChannelPLEN(dwConfig.txPreambLength), ChannelPAC[dwConfig.rxPAC],
      dwConfig.nsSFD==0?"standard":"non-standard", dwConfig.sfdTO);
  pc.printf("    Power: NORM %2.1f dB, BOOST: 0.125ms %2.1f dB, 0.25ms %2.1f dB, 0.5ms %2.1f dB\r\n",
    SMgain(dwConfigTx.power& 0xFF), SMgain((dwConfigTx.power>>24)& 0xFF), SMgain((dwConfigTx.power>>16)& 0xFF),
    SMgain((dwConfigTx.power>>8)& 0xFF));
  pc.printf("  Frame length: %d us\r\n", decaWave.computeFrameLength_us());
  pc.printf("  Antenna Delay set to: %d \r\n", decaWave.getAntennaDelay());

  decaWave.turnonrx();   // start listening       
}

void rangeAndDisplayOne(uint8_t addr){
    beaconNode.requestRanging(addr); 
    pc.printf("%f(%f) \r\n", beaconNode.getDistance(addr), beaconNode.getSignalStrength(addr));
}



void rangeAndDisplayAll(){
    beaconNode.requestRangingAll(); 
    for(int i = 0; i < ADRESSES_COUNT; i++){
        /*if(beaconNode.getDistance(i) > -10){
            pc.printf("#%d %f(%f), ", i, beaconNode.getDistance(i), beaconNode.getSignalStrength(i));
        }*/
    }
   // pc.printf("o error3 ? \r\n");
}