122 lines
3.4 KiB
C++
Executable file
122 lines
3.4 KiB
C++
Executable file
// DecaDuinoTWR_server
|
|
// A simple implementation of the TWR protocol, server side
|
|
// Contributors: Adrien van den Bossche, Réjane Dalcé, Ibrahim Fofana, Robert Try, Thierry Val
|
|
// This sketch is a part of the DecaDuino Project - please refer to the DecaDuino LICENCE file for licensing details
|
|
|
|
#include <SPI.h>
|
|
#include <DecaDuino.h>
|
|
|
|
// Timeout parameters
|
|
#define TIMEOUT_WAIT_ACK_SENT 5 //ms
|
|
#define TIMEOUT_WAIT_DATA_REPLY_SENT 5 //ms
|
|
#define ACK_DATA_REPLY_INTERFRAME 10 //ms
|
|
|
|
// TWR server states state machine enumeration: see state diagram on documentation for more details
|
|
enum { TWR_ENGINE_STATE_INIT, TWR_ENGINE_STATE_WAIT_START, TWR_ENGINE_STATE_MEMORISE_T2,
|
|
TWR_ENGINE_STATE_SEND_ACK, TWR_ENGINE_STATE_WAIT_ACK_SENT, TWR_ENGINE_STATE_MEMORISE_T3,
|
|
TWR_ENGINE_STATE_SEND_DATA_REPLY, TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT };
|
|
|
|
// Message types of the TWR protocol
|
|
#define TWR_MSG_TYPE_UNKNOWN 0
|
|
#define TWR_MSG_TYPE_START 1
|
|
#define TWR_MSG_TYPE_ACK 2
|
|
#define TWR_MSG_TYPE_DATA_REPLY 3
|
|
|
|
uint64_t t2, t3;
|
|
|
|
DecaDuino decaduino;
|
|
uint8_t txData[128];
|
|
uint8_t rxData[128];
|
|
uint16_t rxLen;
|
|
int state;
|
|
uint32_t timeout;
|
|
|
|
|
|
void setup()
|
|
{
|
|
pinMode(13, OUTPUT); // Internal LED (pin 13 on DecaWiNo board)
|
|
Serial.begin(115200); // Init Serial port
|
|
SPI.setSCK(14); // Set SPI clock pin (pin 14 on DecaWiNo board)
|
|
|
|
// Init DecaDuino and blink if initialisation fails
|
|
if ( !decaduino.init() ) {
|
|
Serial.println("decaduino init failed");
|
|
while(1) { digitalWrite(13, HIGH); delay(50); digitalWrite(13, LOW); delay(50); }
|
|
}
|
|
|
|
// Set RX buffer
|
|
decaduino.setRxBuffer(rxData, &rxLen);
|
|
state = TWR_ENGINE_STATE_INIT;
|
|
}
|
|
|
|
|
|
void loop()
|
|
{
|
|
switch (state) {
|
|
|
|
case TWR_ENGINE_STATE_INIT:
|
|
decaduino.plmeRxEnableRequest();
|
|
state = TWR_ENGINE_STATE_WAIT_START;
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_WAIT_START:
|
|
if ( decaduino.rxFrameAvailable() ) {
|
|
if ( rxData[0] == TWR_MSG_TYPE_START ) {
|
|
state = TWR_ENGINE_STATE_MEMORISE_T2;
|
|
} else {
|
|
state = TWR_ENGINE_STATE_INIT;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_MEMORISE_T2:
|
|
t2 = decaduino.getLastRxTimestamp();
|
|
state = TWR_ENGINE_STATE_SEND_ACK;
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_SEND_ACK:
|
|
txData[0] = TWR_MSG_TYPE_ACK;
|
|
decaduino.pdDataRequest(txData, 1);
|
|
timeout = millis() + TIMEOUT_WAIT_ACK_SENT;
|
|
state = TWR_ENGINE_STATE_WAIT_ACK_SENT;
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_WAIT_ACK_SENT:
|
|
if ( millis() > timeout ) {
|
|
state = TWR_ENGINE_STATE_INIT;
|
|
} else {
|
|
if ( decaduino.hasTxSucceeded() ) {
|
|
state = TWR_ENGINE_STATE_MEMORISE_T3;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_MEMORISE_T3:
|
|
t3 = decaduino.getLastTxTimestamp();
|
|
state = TWR_ENGINE_STATE_SEND_DATA_REPLY;
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_SEND_DATA_REPLY:
|
|
delay(ACK_DATA_REPLY_INTERFRAME);
|
|
txData[0] = TWR_MSG_TYPE_DATA_REPLY;
|
|
decaduino.encodeUint40(t2, &txData[1]);
|
|
decaduino.encodeUint40(t3, &txData[6]);
|
|
decaduino.pdDataRequest(txData, 11);
|
|
timeout = millis() + TIMEOUT_WAIT_DATA_REPLY_SENT;
|
|
state = TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT;
|
|
break;
|
|
|
|
case TWR_ENGINE_STATE_WAIT_DATA_REPLY_SENT:
|
|
if ( (millis()>timeout) || (decaduino.hasTxSucceeded()) ) {
|
|
state = TWR_ENGINE_STATE_INIT;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
state = TWR_ENGINE_STATE_INIT;
|
|
break;
|
|
}
|
|
}
|
|
|
|
|