Huseyin Berkay Berabi
ils
Fork of
GA-Test_copy
by 
Alejandro Ungria Hirte
Diff: main_minimal.cpp
- Revision:
- 0:a3b83d366423
- Child:
- 1:346279def7ac
--- /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");
+}
+