Browse Source

Restored the transceiver

martins
Martins Eglitis 8 months ago
parent
commit
dca9a675d2
1 changed files with 216 additions and 169 deletions
  1. 216
    169
      src/transceiver/transceiver.ino

+ 216
- 169
src/transceiver/transceiver.ino View File

@@ -4,216 +4,263 @@
#include <stdint.h>
#include <string.h>

//arduino libraries
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>

//local libraries
#include "helpers.hpp"
#include "pip.hpp"
#include "gps.hpp"
#include "buttons.hpp"

//global variables
RF24 radio(RADIO_ENABLE, RADIO_SELECT);

SoftwareSerial ssGPS(GPS_TX, GPS_RX);

TinyGPSPlus gps;

int flag = 1;
struct region r;

uint8_t buttonState;
uint8_t buttonLastState = LOW;
unsigned long buttonLastDebounce = 0;

void setup()
{
Serial.begin(SYSTEM_BAUD);

ssGPS.begin(SYSTEM_BAUD);

radio.begin();
const byte addressReading[6] = RADIO_ADDRESS_READING;
const byte addressWriting[6] = RADIO_ADDRESS_WRITING;
radio.openReadingPipe(0, addressReading);
radio.openWritingPipe(addressWriting);
radio.setPALevel(RF24_PA_MIN);
radio.startListening();

pinMode(HMI_BUTTON_PIN, INPUT);
pinMode(HMI_LED, OUTPUT);
}

void loop()
{
if (!flag) {
if (!radio.available()) {
return;
}

//TODO - float is 7 digits (in total!!!) max

//4
int size = 4;

float all[size + 8][2] = {
{57.68748,11.97867},
{57.68754,11.97899},
{57.68719,11.97937},
{57.68714,11.97904},

{57.68754,11.97899},
{57.68755,11.97899},
{57.68755,11.97900},
{57.68757,11.97901},
{57.68753,11.97898},
{57.68753,11.97897},
{57.68752,11.97898},
{57.68750,11.97896},
};

//8
/* int size = 8; */

/* float all[size + 8][2] = { */
/* {57.68844,11.97455}, */
/* {57.68866,11.97521}, */
/* {57.68846,11.97541}, */
/* {57.68831,11.97495}, */
/* {57.68828,11.97498}, */
/* {57.68824,11.97485}, */
/* {57.68825,11.97483}, */
/* {57.68824,11.97478}, */

/* {57.68866,11.97521}, */
/* {57.68867,11.97522}, */
/* {57.68867,11.97520}, */
/* {57.68869,11.97522}, */
/* {57.68865,11.97520}, */
/* {57.68864,11.97521}, */
/* {57.68864,11.97519}, */
/* {57.68862,11.97519}, */
/* }; */

//16
/* int size = 16; */

/* float all[size + 8][2] = { */
/* {57.68796,11.97582}, */
/* {57.68817,11.97669}, */
/* {57.68770,11.97707}, */
/* {57.68772,11.97716}, */
/* {57.68744,11.97741}, */
/* {57.68742,11.97730}, */
/* {57.68727,11.97741}, */
/* {57.68714,11.97682}, */
/* {57.68711,11.97686}, */
/* {57.68707,11.97666}, */
/* {57.68724,11.97652}, */
/* {57.68721,11.97643}, */
/* {57.68731,11.97634}, */
/* {57.68730,11.97627}, */
/* {57.68740,11.97618}, */
/* {57.68742,11.97628}, */

/* {57.68818,11.97668}, */
/* {57.68817,11.97669}, */
/* {57.68818,11.97670}, */
/* {57.68820,11.97671}, */
/* {57.68816,11.97668}, */
/* {57.68815,11.97668}, */
/* {57.68815,11.97666}, */
/* {57.68813,11.97666}, */
/* }; */

/* int size = 32; */

/* float all[size + 8][2] = { */
/* {57.68931,11.97687}, */
/* {57.68949,11.97756}, */
/* {57.68927,11.97777}, */
/* {57.68920,11.97753}, */
/* {57.68913,11.97760}, */
/* {57.68926,11.97809}, */
/* {57.68907,11.97827}, */
/* {57.68909,11.97836}, */
/* {57.68921,11.97826}, */
/* {57.68928,11.97853}, */
/* {57.68862,11.97913}, */
/* {57.68854,11.97886}, */
/* {57.68864,11.97877}, */
/* {57.68862,11.97868}, */
/* {57.68843,11.97885}, */
/* {57.68823,11.97814}, */
/* {57.68818,11.97819}, */
/* {57.68814,11.97800}, */
/* {57.68818,11.97797}, */
/* {57.68815,11.97787}, */
/* {57.68837,11.97768}, */
/* {57.68844,11.97795}, */
/* {57.68857,11.97784}, */
/* {57.68851,11.97759}, */
/* {57.68861,11.97750}, */
/* {57.68860,11.97747}, */
/* {57.68900,11.97710}, */
/* {57.68903,11.97721}, */
/* {57.68911,11.97713}, */
/* {57.68907,11.97697}, */
/* {57.68916,11.97689}, */
/* {57.68919,11.97698}, */
/* {57.68949,11.97756}, */
/* {57.68950,11.97756}, */
/* {57.68950,11.97757}, */
/* {57.68952,11.97758}, */
/* {57.68948,11.97755}, */
/* {57.68947,11.97754}, */
/* {57.68947,11.97756}, */
/* {57.68945,11.97754}, */
/* }; */

float X[size];
float Y[size];
float lat[8];
float lng[8];
struct verticeIncoming vi;

if (DEBUG == 1) {
Serial.println("Polygon");
radio.read(&vi, sizeof(vi));

if (vi.id == 0) {
if (DEBUG == 1) {
Serial.println("Radio: empty data");

Serial.println();
}

return;
}

for (int i = 0; i < size; i++) {
X[i] = all[i][0];
Y[i] = all[i][1];
//TODO - by increasing the size, we loose some packages
gpsDelay(&gps, &ssGPS, GPS_DELAY);

if (gps.satellites.value() == 0) {
if (DEBUG == 1) {
Serial.print(X[i], 5);
Serial.print(" ");
Serial.println(Y[i], 5);
Serial.println("GPS: empty data");

Serial.println();
}

return;
}

float lat = gps.location.lat();
float lng = gps.location.lng();

if (DEBUG == 1) {
Serial.println("Points");
Serial.print("Vertice: ");
Serial.print(vi.id);
Serial.print(" ");
Serial.print(vi.order);
Serial.print(" ");
Serial.print(vi.count);
Serial.print(" ");
Serial.print(vi.x, 5);
Serial.print(" ");
Serial.println(vi.y, 5);

Serial.print("Coordinates: ");
Serial.print(lat, 5);
Serial.print(" ");
Serial.println(lng, 5);

Serial.println();
}

for (int i = 0; i < 8; i++) {
lat[i] = all[i + size][0];
lng[i] = all[i + size][1];
//TODO - we don't do anything without GPS or BLE

if (buttonPushed(&buttonState, &buttonLastState, &buttonLastDebounce)) {
if (DEBUG == 1) {
Serial.print(lat[i], 5);
Serial.print(" ");
Serial.println(lng[i], 5);
Serial.println("Button pushed");

Serial.println();
}
}

if (DEBUG == 1) {
Serial.println("-----------");
struct verticeOutgoing vo;

vo.x = lat;
vo.y = lng;

radio.stopListening();
radio.write(&vo, sizeof(vo));
radio.startListening();
}

for (int i = 0; i < 8; i++) {
unsigned long st = micros();
uint8_t id = vi.id;
uint8_t count = vi.count;
uint8_t order = vi.order;
float x = vi.x;
float y = vi.y;

uint8_t pip = pipWindingNumber(lat[i], lng[i], X, Y, size);
/* uint8_t pip = pipRayCasting(lat[i], lng[i], X, Y, size); */
uint8_t foundGeofence = 0;

//TODO - we can't optimize GEOFENCE_GEOFENCES to size of struct region.
//That is because the geofences in region are fragmented - there might
//be free spots because of reseting a certain geofence.
for (uint8_t i = 0; i < GEOFENCE_GEOFENCES; i++) {
if (r.geofences[i].id == 0) {
continue;
}

unsigned long et = micros();
//check the lifetime of a geofence
if (r.geofences[i].ttl == 0) {
digitalWrite(HMI_LED, LOW);

r.geofences[i] = (struct geofence) {0};

break;
}

/* Serial.print("Time "); */
/* Serial.print(lat[i], 5); */
/* Serial.print(" "); */
/* Serial.print(lng[i], 5); */
/* Serial.print(": "); */
if (r.geofences[i].id == id) {
foundGeofence = 1;

if (pip == 1) {
Serial.print("in,");
uint8_t foundVertice = 0;

//check for repeating vertice order
for (uint8_t j = 0; j < r.geofences[i].size; j++) {
if (r.geofences[i].vertices[j].order == order) {
r.geofences[i].ttl--;

foundVertice = 1;

break;
}
}

if (foundVertice == 0) {
uint8_t size = r.geofences[i].size;

//dynamic
r.geofences[i].size = size + 1;

r.geofences[i].vertices[size].order = order;
r.geofences[i].vertices[size].x = x;
r.geofences[i].vertices[size].y = y;

break;
}
} else {
Serial.print("out,");
//reduce ttl if the geofence is not empty
//and you don't belong to it
r.geofences[i].ttl--;
}
}

if (foundGeofence == 0) {
for (uint8_t i = 0; i < GEOFENCE_GEOFENCES; i++) {
if (r.geofences[i].id == 0) {
r.geofences[i].id = id;
r.geofences[i].count = count;

Serial.println(et - st);
//dynamic
r.geofences[i].size = 1;
r.geofences[i].ttl = GEOFENCE_TTL;

/* if (DEBUG == 1) { */
/* if (pip == 1) { */
/* Serial.print("in "); */
/* } else { */
/* Serial.print("out "); */
/* } */
r.geofences[i].vertices[0].order = order;
r.geofences[i].vertices[0].x = x;
r.geofences[i].vertices[0].y = y;

/* Serial.print(lat[i], 5); */
/* Serial.print(","); */
/* Serial.println(lng[i], 5); */
/* } */
break;
}
}
}

flag = 0;
//TODO - again, see the optimization
for (uint8_t i = 0; i < GEOFENCE_GEOFENCES; i++) {
//we can use a local variable here
struct geofence g = r.geofences[i];

if (g.id == 0) {
continue;
}

if (g.size == g.count) {
float X[g.size];
float Y[g.size];

qsort(g.vertices, g.size, sizeof(struct vertice), compare);

if (DEBUG == 1) {
Serial.println("Vertices: ");
}

for (uint8_t i = 0; i < g.size; i++) {
X[i] = g.vertices[i].x;
Y[i] = g.vertices[i].y;

if (DEBUG == 1) {
Serial.print(i);
Serial.print(" ");
Serial.print(X[i], 5);
Serial.print(" ");
Serial.println(Y[i], 5);
}
}

if (DEBUG == 1) {
Serial.println();
}

uint8_t pip = pipWindingNumber(lat, lng, X, Y, g.size);

if (pip == 1) {
digitalWrite(HMI_LED, HIGH);

if (DEBUG == 1) {
Serial.println("PIP inside");

Serial.println();
}
} else {
digitalWrite(HMI_LED, LOW);

if (DEBUG == 1) {
Serial.println("PIP outside");

Serial.println();
}
}

r.geofences[i] = (struct geofence) {0};

break;
}
}
}

Loading…
Cancel
Save