ils
Fork of GA-Test_copy by
Diff: main_minimal.cpp
- Revision:
- 0:a3b83d366423
- Child:
- 1:346279def7ac
diff -r 000000000000 -r a3b83d366423 main_minimal.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main_minimal.cpp Wed Dec 06 21:35:45 2017 +0000 @@ -0,0 +1,354 @@ +#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()){ + + //pc.readcommand(executeOrder); + + // EXECUTE THIS IF A BASE STATION + pc.readcommand(executeOrder); + wait_ms(10); + wdt.kick(); + /* + while (1) + { + if (pc.readable()) + { + pc.scanf( "%s" , command_str ); + break; + } + } + */ + //wait_ms(3); + //pc.printf("Command Received: %s", command_str); + //wait_ms(3); + //executeOrder(command_str); + //wait_ms(3); + } + 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); + + } + else + { + pc.printf("ERROR ------ This is an invalid command\n"); + } +} + +#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("o error2 ? %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"); +} +