ils
Fork of GA-Test_copy by
main_minimal.cpp
- Committer:
- bberabi
- Date:
- 2018-01-10
- Revision:
- 2:5adf0b785944
- Parent:
- 1:346279def7ac
File content as of revision 2:5adf0b785944:
#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(10); 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(); // 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"); }