Skip to content

Commit f0010fd

Browse files
ianmott77ianmott77
authored
Initial upload
This upload includes the necessary code for synchronizing two Arduino's with an LoRa RF_95 module compatible with the RadioHead library (https://www.airspayce.com/mikem/arduino/RadioHead/).
1 parent d2ea23c commit f0010fd

22 files changed

+1126
-0
lines changed

‎TimeSync/Buffer.cpp‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
#include "Buffer.h"
2+
3+
Buffer::Buffer(int data)
4+
{
5+
uint8_t * buffer = BufferConverter::intToBuffer(data);
6+
this->length = 2;
7+
this->type = INT;
8+
this->packetStack = new Stack<Packet *>(1);
9+
this->packetStack->add(new Packet(buffer, this->length));
10+
delete buffer;
11+
}
12+
13+
Buffer::Buffer(unsigned long data)
14+
{
15+
uint8_t * buffer = BufferConverter::longToBuffer(data);
16+
this->length = 4;
17+
this->type = LONG;
18+
this->packetStack = new Stack<Packet*>(1);
19+
this->packetStack->add(new Packet(buffer, this->length));
20+
delete buffer;
21+
}
22+
23+
Buffer::Buffer(char *data)
24+
{
25+
this->length = strlen(data);
26+
this->init((uint8_t *)data);
27+
this->type = STRING;
28+
}
29+
30+
Buffer::Buffer(uint8_t *data, int size)
31+
{
32+
this->length = size;
33+
this->init(data);
34+
this->type = BUFFER;
35+
}
36+
37+
Buffer::Buffer(Buffer * input){
38+
this->length = input->size();
39+
this->packetStack = input->packetStack;
40+
this->type = BUFFER;
41+
}
42+
Buffer::Buffer(Packet * data){
43+
this->length = data->size;
44+
this->init(data->data);
45+
this->type = BUFFER;
46+
}
47+
48+
49+
Buffer::~Buffer()
50+
{
51+
delete this->packetStack;
52+
}
53+
54+
void Buffer::init(uint8_t *data)
55+
{
56+
this->makePacketStack(data);
57+
}
58+
59+
void Buffer::makePacketStack(uint8_t * data)
60+
{
61+
int numPackets = ceil(this->length / MAX_PACKET_SIZE);
62+
this->packetStack = new Stack<Packet *>(numPackets);
63+
int byteCount = 0;
64+
for (int i = 0; i < numPackets; i++)
65+
{
66+
int remaining = this->length - byteCount;
67+
int nextPackSize = (remaining > MAX_PACKET_SIZE) ? MAX_PACKET_SIZE : remaining;
68+
uint8_t buf[nextPackSize];
69+
for (int j = 0; j < nextPackSize; j++)
70+
{
71+
buf[j] = data[byteCount];
72+
byteCount++;
73+
}
74+
this->packetStack->add(new Packet(buf, nextPackSize));
75+
}
76+
}
77+
78+
char * Buffer::toString()
79+
{
80+
char * str = (char*) malloc(this->length+1);
81+
int byteCount = 0;
82+
int numPackets = ceil(this->length / MAX_PACKET_SIZE);
83+
for (int i = 0; i < numPackets; i++)
84+
{
85+
Packet *incoming = this->packetStack->get(i);
86+
int remaining = this->length - byteCount;
87+
int nextPackSize = (remaining > MAX_PACKET_SIZE) ? MAX_PACKET_SIZE : remaining;
88+
for (int j = 0; j < nextPackSize; j++)
89+
{
90+
str[byteCount] = (char) incoming->data[j];
91+
byteCount++;
92+
}
93+
}
94+
str[byteCount] = '\0';
95+
return str;
96+
}
97+
98+
int Buffer::toInt(){
99+
return BufferConverter::bufferToInt(this->packetStack->get(0)->data);
100+
}
101+
102+
unsigned long Buffer::toLong(){
103+
return BufferConverter::bufferToLong(this->packetStack->get(0)->data);
104+
}
105+
106+
int Buffer::size(){
107+
return this->length;
108+
}

‎TimeSync/Buffer.h‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
#ifndef BUFFER_H
2+
#define BUFFER_H
3+
4+
#include "BufferConverter.h"
5+
#include "Packet.h"
6+
#ifndef STACK_H
7+
#include "Stack.h"
8+
#endif
9+
#define INT 1
10+
#define LONG 2
11+
#define STRING 3
12+
#define BUFFER 4
13+
14+
class Buffer
15+
{
16+
private:
17+
void makePacketStack(uint8_t *);
18+
void init(uint8_t *);
19+
int length;
20+
21+
public:
22+
Buffer(int);
23+
Buffer(unsigned long);
24+
Buffer(char *);
25+
Buffer(uint8_t *, int);
26+
Buffer(Packet*);
27+
Buffer(Buffer*);
28+
~Buffer();
29+
char * toString();
30+
int toInt();
31+
unsigned long toLong();
32+
int size();
33+
int type;
34+
Stack<Packet*> * packetStack;
35+
36+
};
37+
38+
#endif
39+

‎TimeSync/BufferConverter.cpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
#include "BufferConverter.h"
2+
3+
uint8_t *BufferConverter::intToBuffer(int data)
4+
{
5+
uint8_t * buf = (uint8_t*) malloc(2);
6+
buf[0] = (data >> 8) & 0xFF;
7+
buf[1] = data & 0xFF;
8+
return buf;
9+
}
10+
11+
int BufferConverter::bufferToInt(uint8_t * data)
12+
{
13+
int val;
14+
val = data[0] << 8;
15+
val |= data[1] & 0xFF;
16+
return val;
17+
}
18+
19+
uint8_t * BufferConverter::longToBuffer(unsigned long data){
20+
uint8_t * buf = (uint8_t*) malloc(4);
21+
for(int i = 0; i < 4; i++){
22+
buf[i] = (data >> (i*8)) & 0xFF;
23+
}
24+
return buf;
25+
}
26+
27+
unsigned long BufferConverter::bufferToLong(uint8_t * data){
28+
union{
29+
uint8_t buf[4];
30+
long val;
31+
} long_union;
32+
33+
for(int i = 0; i < 4; i++){
34+
long_union.buf[i] = data[i];
35+
}
36+
37+
return long_union.val;
38+
}

‎TimeSync/BufferConverter.h‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
#ifndef BUFFERCONVERTER_H
2+
#define BUFFERCONVERTER_H
3+
4+
#include <Arduino.h>
5+
6+
class BufferConverter
7+
{
8+
public:
9+
static uint8_t * intToBuffer(int);
10+
static int bufferToInt(uint8_t *);
11+
static unsigned long bufferToLong(uint8_t *);
12+
static uint8_t * longToBuffer(unsigned long);
13+
};
14+
15+
#endif

‎TimeSync/ClientStageOne.cpp‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
#include "ClientStageOne.h"
2+
3+
ClientStageOne::ClientStageOne(Timing * time, Connection * connection, bool logging)
4+
{
5+
this->time = time;
6+
this->connection = connection;
7+
this->count = 0;
8+
this->logging = logging;
9+
this->id = 1;
10+
this->syncStarted = false;
11+
this->done = false;
12+
}
13+
14+
void ClientStageOne::init(){
15+
Buffer * starter = new Buffer(-1);
16+
if(this->connection->send(starter)){
17+
this->syncStarted = true;
18+
}
19+
delete starter;
20+
}
21+
22+
void ClientStageOne::exec(){
23+
if(this->count < 5){
24+
Buffer * now = new Buffer(this->time->now());
25+
if(this->connection->send(now)){
26+
Buffer * t = this->connection->receive();
27+
unsigned long them = this->time->now();
28+
if(them != 0 && them != '\0'){
29+
this->time->startMils = now->toLong();
30+
this->time->endMils = them;
31+
this->time->adjusted = this->time->endMils - this->time->startMils;
32+
this->time->delays->add(this->time->adjusted);
33+
if(this->logging){
34+
this->log(BufferConverter::bufferToLong(now->packetStack->get(0)->data));
35+
}
36+
delete t;
37+
delete now;
38+
this->count++;
39+
}else{
40+
Serial.print(F("Receiving Failed"));
41+
delete t;
42+
delete now;
43+
}
44+
}else{
45+
Serial.print(F("Sending: "));
46+
Serial.print(now->toLong());
47+
Serial.println(F(" Failed"));
48+
delete now;
49+
}
50+
}else if(this->count == 5){
51+
this->time->delay();
52+
this->done = true;
53+
}
54+
}
55+
56+
void ClientStageOne::log(unsigned long now){
57+
Serial.print(F("now: "));
58+
Serial.println(now);
59+
Serial.print(F("startMils: "));
60+
Serial.println(this->time->startMils);
61+
Serial.print(F("endMils: "));
62+
Serial.println(this->time->endMils);
63+
Serial.print(this->count);
64+
Serial.print(F(") Delay: "));
65+
Serial.println(this->time->adjusted);
66+
}

‎TimeSync/ClientStageOne.h‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#ifndef CLIENT_STAGE_ONE
2+
#define CLIENT_STAGE_ONE
3+
4+
#include "Timing.h"
5+
#include "Operation.h"
6+
#include "Connection.h"
7+
8+
9+
class ClientStageOne : public Operation
10+
{
11+
private:
12+
Connection * connection;
13+
bool logging;
14+
bool syncStarted;
15+
void log(unsigned long now);
16+
public:
17+
ClientStageOne(Timing *, Connection *, bool logging = false);
18+
virtual void init();
19+
virtual void exec();
20+
int count;
21+
bool done;
22+
Timing * time;
23+
};
24+
25+
#endif

‎TimeSync/ClientStageTwo.cpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
#include "ClientStageTwo.h"
2+
3+
ClientStageTwo::ClientStageTwo(Timing * time, Connection * connection, bool logging)
4+
{
5+
this->time = time;
6+
this->connection = connection;
7+
this->logging = logging;
8+
this->count = 0;
9+
this->done = false;
10+
}
11+
12+
void ClientStageTwo::init(){};
13+
14+
void ClientStageTwo::exec(){
15+
this->sync();
16+
}
17+
18+
bool ClientStageTwo::sync(){
19+
Buffer * nowBuf = new Buffer(this->time->now());
20+
if(this->connection->send(nowBuf)){
21+
Buffer * themBuf = this->connection->receive();
22+
unsigned long now = nowBuf->toLong();
23+
unsigned long them = themBuf->toLong();
24+
delete nowBuf;
25+
delete themBuf;
26+
27+
if(this->count == 0){
28+
this->time->dif = them - (now + this->time->avgDelay);
29+
this->time->first = now + this->time->avgDelay + this->time->dif;
30+
}
31+
32+
this->time->adjusted = (this->time->factor == 0.00) ? now + this->time->avgDelay + this->time->dif : now;
33+
this->time->drift = this->time->adjusted - them;
34+
35+
if(this->logging){
36+
this->log(now, them);
37+
}
38+
39+
if(this->time->drift > 5 || this->time->drift < -5 || this->done){
40+
unsigned long tempNow = this->time->mils + this->time->avgDelay + this->time->dif;
41+
this->time->factor = (tempNow - them) / ((tempNow - this->time->first)/1000.000000);
42+
this->done = true;
43+
}
44+
this->count++;
45+
}
46+
}
47+
48+
void ClientStageTwo::log(unsigned long now, unsigned long them){
49+
if(this->count == 0){
50+
Serial.print(F("Me: "));
51+
Serial.println(now);
52+
Serial.print(F("Them: "));
53+
Serial.println(them);
54+
Serial.print(F("Dif: "));
55+
Serial.println(this->time->dif);
56+
Serial.print(F("First: "));
57+
Serial.println(this->time->first);
58+
Serial.print(F("Count"));
59+
Serial.print(F(" ,"));
60+
Serial.print(F("Elapsed Seconds"));
61+
Serial.print(F(" ,"));
62+
Serial.print(F("Me "));
63+
Serial.print(F(" ,"));
64+
Serial.print(F("Them "));
65+
Serial.print(F(" ,"));
66+
Serial.print(F("Adjusted "));
67+
Serial.print(F(" ,"));
68+
Serial.print(F("Drift "));
69+
Serial.print(F(" ,"));
70+
Serial.print(F("Drift Ms/s: "));
71+
Serial.print(F(" "));
72+
Serial.println();
73+
}
74+
Serial.print(this->count);
75+
Serial.print(F(" ,"));
76+
Serial.print(((this->time->mils + this->time->avgDelay + this->time->dif) - this->time->first)/1000.0000, 4);
77+
Serial.print(F(" ,"));
78+
Serial.print(this->time->mils);
79+
Serial.print(F(" ,"));
80+
Serial.print(them);
81+
Serial.print(F(" ,"));
82+
Serial.print(this->time->adjusted);
83+
Serial.print(F(" ,"));
84+
Serial.print(this->time->drift);
85+
Serial.print(F(" ,"));
86+
Serial.print(this->time->factor, 4);
87+
Serial.println();
88+
Serial.flush();
89+
}

0 commit comments

Comments
 (0)