rbxii3/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2026-01-15 22:37:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

bt to mesh code is written

Browse Source
This commit is contained in:
geeksville
2020-02-01 16:14:34 -08:00
parent 490331f3cd
commit de03dc88f4
6 changed files with 185 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
src/MeshBluetoothService.cpp Normal file
View File

@@ -0,0 +1,211 @@
#include "BluetoothUtil.h"
#include "MeshBluetoothService.h"
#include <esp_gatt_defs.h>
#include <BLE2902.h>
#include <Arduino.h>
#include <assert.h>
#include <pb_encode.h>
#include <pb_decode.h>
#include "mesh.pb.h"
#include "MeshRadio.h"
/*
receivedPacketQueue - this is a queue of messages we've received from the mesh, which we are keeping to deliver to the phone.
It is implemented with a FreeRTos queue (wrapped with a little RTQueue class) of pointers to MeshPacket protobufs (which were alloced with new).
After a packet ptr is removed from the queue and processed it should be deleted. (eventually we should move sent packets into a 'sentToPhone' queue
of packets we can delete just as soon as we are sure the phone has acked those packets - when the phone writes to FromNum)
mesh - an instance of Mesh class. Which manages the interface to the mesh radio library, reception of packets from other nodes, arbitrating to select
a node number and keeping the current nodedb.
typedef in32_t NodeNum;
class NodeInfo {
position;
last_seen
user
};
class NodeDB {
NodeNum provisionalNodeNum; // if we are trying to find a node num this is our current attempt
NodeNum ourNodeNum; // -1 if not yet found
HashMap<NodeNum, NodeInfo> nodes;
public:
/// don't do mesh based algoritm for node id assignment (initially) - instead just store in flash - possibly even in the initial alpha release do this hack
/// if returns false, that means our node should send a DenyNodeNum response. If true, we think the number is okay for use
// bool handleWantNodeNum(NodeNum n);
void handleDenyNodeNum(NodeNum FIXME read mesh proto docs, perhaps picking a random node num is not a great idea
and instead we should use a special 'im unconfigured node number' and include our desired node number in the wantnum message. the
unconfigured node num would only be used while initially joining the mesh so low odds of conflicting (especially if we randomly select
from a small number of nodenums which can be used temporarily for this operation). figure out what the lower level
mesh sw does if it does conflict? would it be better for people who are replying with denynode num to just broadcast their denial?)
};
*/
/// A temporary buffer used for sending packets, sized to hold the biggest buffer we might need
static uint8_t outbuf[MeshPacket_size];
/**
* Top level app for this service. keeps the mesh, the radio config and the queue of received packets.
*
*/
class MeshService
{
public:
/// Given a ToRadio buffer parse it and properly handle it (setup radio, owner or send packet into the mesh)
void handleToRadio(std::string s)
{
static ToRadio r; // new ToRadio(); FIXME dynamically allocate
pb_istream_t stream = pb_istream_from_buffer((const uint8_t *)s.c_str(), s.length());
if (!pb_decode(&stream, ToRadio_fields, &r))
{
Serial.printf("Error: can't decode ToRadio %s\n", PB_GET_ERROR(&stream));
}
else
{
switch (r.which_variant)
{
case ToRadio_packet_tag:
sendToMesh(r.variant.packet);
break;
default:
Serial.println("Error: unexpected ToRadio variant");
break;
}
}
}
private:
/// Send a packet into the mesh
void sendToMesh(const MeshPacket &p)
{
assert(p.has_payload);
pb_ostream_t stream = pb_ostream_from_buffer(outbuf, sizeof(outbuf));
if (!pb_encode(&stream, MeshPacket_fields, &p))
{
Serial.printf("Error: can't encode MeshPacket %s\n", PB_GET_ERROR(&stream));
}
else
{
radio.sendTo(p.to, outbuf, stream.bytes_written);
}
}
};
MeshService service;
static BLECharacteristic meshFromRadioCharacteristic("8ba2bcc2-ee02-4a55-a531-c525c5e454d5", BLECharacteristic::PROPERTY_READ);
static BLECharacteristic meshToRadioCharacteristic("f75c76d2-129e-4dad-a1dd-7866124401e7", BLECharacteristic::PROPERTY_WRITE);
static BLECharacteristic meshFromNumCharacteristic("ed9da18c-a800-4f66-a670-aa7547e34453", BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
class BluetoothMeshCallbacks : public BLECharacteristicCallbacks
{
void onRead(BLECharacteristic *c)
{
Serial.println("Got on read");
if (c == &meshFromRadioCharacteristic)
{
// Someone is going to read our value as soon as this callback returns. So fill it with the next message in the queue
// or make empty if the queue is empty
// c->setValue(byteptr, len);
}
}
void onWrite(BLECharacteristic *c)
{
// dumpCharacteristic(pCharacteristic);
Serial.println("Got on write");
if (c == &meshToRadioCharacteristic)
{
service.handleToRadio(c->getValue());
}
else
{
assert(0); // Not yet implemented
}
}
};
static BluetoothMeshCallbacks btMeshCb;
/*
MeshBluetoothService UUID 6ba1b218-15a8-461f-9fa8-5dcae273eafd
FIXME - notify vs indication for fromradio output. Using notify for now, not sure if that is best
FIXME - in the esp32 mesh managment code, occasionally mirror the current net db to flash, so that if we reboot we still have a good guess of users who are out there.
FIXME - make sure this protocol is guaranteed robust and won't drop packets
"According to the BLE specification the notification length can be max ATT_MTU - 3. The 3 bytes subtracted is the 3-byte header(OP-code (operation, 1 byte) and the attribute handle (2 bytes)).
In BLE 4.1 the ATT_MTU is 23 bytes (20 bytes for payload), but in BLE 4.2 the ATT_MTU can be negotiated up to 247 bytes."
MAXPACKET is 256? look into what the lora lib uses. FIXME
Characteristics:
UUID
properties
description
8ba2bcc2-ee02-4a55-a531-c525c5e454d5
read
fromradio - contains a newly received packet destined towards the phone (up to MAXPACKET bytes? per packet).
After reading the esp32 will put the next packet in this mailbox. If the FIFO is empty it will put an empty packet in this
mailbox.
f75c76d2-129e-4dad-a1dd-7866124401e7
write
toradio - write ToRadio protobufs to this charstic to send them (up to MAXPACKET len)
ed9da18c-a800-4f66-a670-aa7547e34453
read|notify|write
fromnum - the current packet # in the message waiting inside fromradio, if the phone sees this notify it should read messages
until it catches up with this number.
The phone can write to this register to go backwards up to FIXME packets, to handle the rare case of a fromradio packet was dropped after the esp32
callback was called, but before it arrives at the phone. If the phone writes to this register the esp32 will discard older packets and put the next packet >= fromnum in fromradio.
When the esp32 advances fromnum, it will delay doing the notify by 100ms, in the hopes that the notify will never actally need to be sent if the phone is already pulling from fromradio.
Note: that if the phone ever sees this number decrease, it means the esp32 has rebooted.
Re: queue management
Not all messages are kept in the fromradio queue (filtered based on SubPacket):
* only the most recent Position and User messages for a particular node are kept
* all Data SubPackets are kept
* No WantNodeNum / DenyNodeNum messages are kept
A variable keepAllPackets, if set to true will suppress this behavior and instead keep everything for forwarding to the phone (for debugging)
*/
BLEService *createMeshBluetoothService(BLEServer *server)
{
// Create the BLE Service
BLEService *service = server->createService("6ba1b218-15a8-461f-9fa8-5dcae273eafd");
addWithDesc(service, &meshFromRadioCharacteristic, "fromRadio");
addWithDesc(service, &meshToRadioCharacteristic, "toRadio");
addWithDesc(service, &meshFromNumCharacteristic, "fromNum");
meshFromRadioCharacteristic.setCallbacks(&btMeshCb);
meshToRadioCharacteristic.setCallbacks(&btMeshCb);
meshFromNumCharacteristic.setCallbacks(&btMeshCb);
meshFromNumCharacteristic.addDescriptor(new BLE2902()); // Needed so clients can request notification
return service;
}

6
src/MeshBluetoothService.h Normal file
View File

@@ -0,0 +1,6 @@
#pragma once
#include <Arduino.h>
BLEService *createMeshBluetoothService(BLEServer* server);

1
src/MeshRadio.cpp
View File

@@ -84,7 +84,6 @@ void mesh_init() {
}
}
int16_t packetnum = 0; // packet counter, we increment per xmission
void mesh_loop()

317
src/main.ino
View File

@@ -26,6 +26,7 @@
#include <TinyGPS++.h>
#include <Wire.h>
#include "BluetoothUtil.h"
#include "MeshBluetoothService.h"
#include "MeshRadio.h"
#ifdef T_BEAM_V10
@@ -50,47 +51,49 @@ esp_sleep_source_t wakeCause; // the reason we booted this time
void doDeepSleep(uint64_t msecToWake)
{
Serial.printf("Entering deep sleep for %llu seconds\n", msecToWake / 1000);
Serial.printf("Entering deep sleep for %llu seconds\n", msecToWake / 1000);
// not using wifi yet, but once we are this is needed to shutoff the radio hw
// esp_wifi_stop();
// not using wifi yet, but once we are this is needed to shutoff the radio hw
// esp_wifi_stop();
screen_off(); // datasheet says this will draw only 10ua
// FIXME, shutdown radio headinterups before powering off device
#ifdef T_BEAM_V10
if(axp192_found) {
// turn on after initial testing with real hardware
axp.setPowerOutPut(AXP192_LDO2, AXP202_OFF); // LORA radio
axp.setPowerOutPut(AXP192_LDO3, AXP202_OFF); // GPS main power
}
#endif
screen_off(); // datasheet says this will draw only 10ua
// FIXME - use an external 10k pulldown so we can leave the RTC peripherals powered off
// until then we need the following lines
esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_ON);
// FIXME, shutdown radio headinterups before powering off device
#ifdef BUTTON_PIN
// Only GPIOs which are have RTC functionality can be used in this bit map: 0,2,4,12-15,25-27,32-39.
uint64_t gpioMask = (1ULL << BUTTON_PIN);
// FIXME change polarity so we can wake on ANY_HIGH instead - that would allow us to use all three buttons (instead of just the first)
gpio_pullup_en((gpio_num_t) BUTTON_PIN);
esp_sleep_enable_ext1_wakeup(gpioMask, ESP_EXT1_WAKEUP_ALL_LOW);
#ifdef T_BEAM_V10
if (axp192_found)
{
// turn on after initial testing with real hardware
axp.setPowerOutPut(AXP192_LDO2, AXP202_OFF); // LORA radio
axp.setPowerOutPut(AXP192_LDO3, AXP202_OFF); // GPS main power
}
#endif
esp_sleep_enable_timer_wakeup(msecToWake * 1000ULL); // call expects usecs
esp_deep_sleep_start(); // TBD mA sleep current (battery)
// FIXME - use an external 10k pulldown so we can leave the RTC peripherals powered off
// until then we need the following lines
esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_ON);
#ifdef BUTTON_PIN
// Only GPIOs which are have RTC functionality can be used in this bit map: 0,2,4,12-15,25-27,32-39.
uint64_t gpioMask = (1ULL << BUTTON_PIN);
// FIXME change polarity so we can wake on ANY_HIGH instead - that would allow us to use all three buttons (instead of just the first)
gpio_pullup_en((gpio_num_t)BUTTON_PIN);
esp_sleep_enable_ext1_wakeup(gpioMask, ESP_EXT1_WAKEUP_ALL_LOW);
#endif
esp_sleep_enable_timer_wakeup(msecToWake * 1000ULL); // call expects usecs
esp_deep_sleep_start(); // TBD mA sleep current (battery)
}
void sleep() {
void sleep()
{
#if SLEEP_BETWEEN_MESSAGES
// If the user has a screen, tell them we are about to sleep
if(ssd1306_found) {
if (ssd1306_found)
{
// Show the going to sleep message on the screen
char buffer[20];
snprintf(buffer, sizeof(buffer), "Sleeping in %3.1fs\n", (MESSAGE_TO_SLEEP_DELAY / 1000.0));
@@ -101,7 +104,7 @@ void sleep() {
// Turn off screen
screen_off();
}
}
// We sleep for the interval between messages minus the current millis
// this way we distribute the messages evenly every SEND_INTERVAL millis
@@ -111,43 +114,48 @@ void sleep() {
#endif
}
void scanI2Cdevice(void)
{
byte err, addr;
int nDevices = 0;
for (addr = 1; addr < 127; addr++) {
Wire.beginTransmission(addr);
err = Wire.endTransmission();
if (err == 0) {
Serial.print("I2C device found at address 0x");
if (addr < 16)
Serial.print("0");
Serial.print(addr, HEX);
Serial.println(" !");
nDevices++;
byte err, addr;
int nDevices = 0;
for (addr = 1; addr < 127; addr++)
{
Wire.beginTransmission(addr);
err = Wire.endTransmission();
if (err == 0)
{
Serial.print("I2C device found at address 0x");
if (addr < 16)
Serial.print("0");
Serial.print(addr, HEX);
Serial.println(" !");
nDevices++;
if (addr == SSD1306_ADDRESS) {
ssd1306_found = true;
Serial.println("ssd1306 display found");
}
#ifdef T_BEAM_V10
if (addr == AXP192_SLAVE_ADDRESS) {
axp192_found = true;
Serial.println("axp192 PMU found");
}
#endif
} else if (err == 4) {
Serial.print("Unknow error at address 0x");
if (addr < 16)
Serial.print("0");
Serial.println(addr, HEX);
}
if (addr == SSD1306_ADDRESS)
{
ssd1306_found = true;
Serial.println("ssd1306 display found");
}
#ifdef T_BEAM_V10
if (addr == AXP192_SLAVE_ADDRESS)
{
axp192_found = true;
Serial.println("axp192 PMU found");
}
#endif
}
if (nDevices == 0)
Serial.println("No I2C devices found\n");
else
Serial.println("done\n");
else if (err == 4)
{
Serial.print("Unknow error at address 0x");
if (addr < 16)
Serial.print("0");
Serial.println(addr, HEX);
}
}
if (nDevices == 0)
Serial.println("No I2C devices found\n");
else
Serial.println("done\n");
}
/**
@@ -161,61 +169,70 @@ void scanI2Cdevice(void)
LDO2 200mA -> LORA
LDO3 200mA -> GPS
*/
void axp192Init() {
#ifdef T_BEAM_V10
if (axp192_found) {
if (!axp.begin(Wire, AXP192_SLAVE_ADDRESS)) {
Serial.println("AXP192 Begin PASS");
} else {
Serial.println("AXP192 Begin FAIL");
}
// axp.setChgLEDMode(LED_BLINK_4HZ);
Serial.printf("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
Serial.println("----------------------------------------");
axp.setPowerOutPut(AXP192_LDO2, AXP202_ON); // LORA radio
axp.setPowerOutPut(AXP192_LDO3, AXP202_ON); // GPS main power
axp.setPowerOutPut(AXP192_DCDC2, AXP202_ON);
axp.setPowerOutPut(AXP192_EXTEN, AXP202_ON);
axp.setPowerOutPut(AXP192_DCDC1, AXP202_ON);
axp.setDCDC1Voltage(3300); // for the OLED power
Serial.printf("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
pinMode(PMU_IRQ, INPUT_PULLUP);
attachInterrupt(PMU_IRQ, [] {
pmu_irq = true;
}, FALLING);
axp.adc1Enable(AXP202_BATT_CUR_ADC1, 1);
axp.enableIRQ(AXP202_VBUS_REMOVED_IRQ | AXP202_VBUS_CONNECT_IRQ | AXP202_BATT_REMOVED_IRQ | AXP202_BATT_CONNECT_IRQ, 1);
axp.clearIRQ();
if (axp.isChargeing()) {
baChStatus = "Charging";
}
} else {
Serial.println("AXP192 not found");
void axp192Init()
{
#ifdef T_BEAM_V10
if (axp192_found)
{
if (!axp.begin(Wire, AXP192_SLAVE_ADDRESS))
{
Serial.println("AXP192 Begin PASS");
}
#endif
else
{
Serial.println("AXP192 Begin FAIL");
}
// axp.setChgLEDMode(LED_BLINK_4HZ);
Serial.printf("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
Serial.println("----------------------------------------");
axp.setPowerOutPut(AXP192_LDO2, AXP202_ON); // LORA radio
axp.setPowerOutPut(AXP192_LDO3, AXP202_ON); // GPS main power
axp.setPowerOutPut(AXP192_DCDC2, AXP202_ON);
axp.setPowerOutPut(AXP192_EXTEN, AXP202_ON);
axp.setPowerOutPut(AXP192_DCDC1, AXP202_ON);
axp.setDCDC1Voltage(3300); // for the OLED power
Serial.printf("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
Serial.printf("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
Serial.printf("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
pinMode(PMU_IRQ, INPUT_PULLUP);
attachInterrupt(PMU_IRQ, [] {
pmu_irq = true;
},
FALLING);
axp.adc1Enable(AXP202_BATT_CUR_ADC1, 1);
axp.enableIRQ(AXP202_VBUS_REMOVED_IRQ | AXP202_VBUS_CONNECT_IRQ | AXP202_BATT_REMOVED_IRQ | AXP202_BATT_CONNECT_IRQ, 1);
axp.clearIRQ();
if (axp.isChargeing())
{
baChStatus = "Charging";
}
}
else
{
Serial.println("AXP192 not found");
}
#endif
}
// Perform power on init that we do on each wake from deep sleep
void initDeepSleep() {
bootCount++;
wakeCause = esp_sleep_get_wakeup_cause();
/*
void initDeepSleep()
{
bootCount++;
wakeCause = esp_sleep_get_wakeup_cause();
/*
Not using yet because we are using wake on all buttons being low
wakeButtons = esp_sleep_get_ext1_wakeup_status(); // If one of these buttons is set it was the reason we woke
@@ -223,14 +240,15 @@ void initDeepSleep() {
wakeButtons = ((uint64_t)1) << buttons.gpios[0];
*/
Serial.printf("booted, wake cause %d (boot count %d)\n", wakeCause, bootCount);
Serial.printf("booted, wake cause %d (boot count %d)\n", wakeCause, bootCount);
}
void setup() {
// Debug
#ifdef DEBUG_PORT
void setup()
{
// Debug
#ifdef DEBUG_PORT
DEBUG_PORT.begin(SERIAL_BAUD);
#endif
#endif
initDeepSleep();
// delay(1000); FIXME - remove
@@ -238,12 +256,12 @@ void setup() {
#ifdef VEXT_ENABLE
pinMode(VEXT_ENABLE, OUTPUT);
digitalWrite(VEXT_ENABLE, 0); // turn on the display power
#endif
#endif
#ifdef RESET_OLED
pinMode(RESET_OLED, OUTPUT);
digitalWrite(RESET_OLED, 1);
#endif
#endif
Wire.begin(I2C_SDA, I2C_SCL);
scanI2Cdevice();
@@ -264,10 +282,10 @@ void setup() {
DEBUG_MSG(APP_NAME " " APP_VERSION "\n");
// Don't init display if we don't have one or we are waking headless due to a timer event
if(wakeCause == ESP_SLEEP_WAKEUP_TIMER)
if (wakeCause == ESP_SLEEP_WAKEUP_TIMER)
ssd1306_found = false; // forget we even have the hardware
if(ssd1306_found)
if (ssd1306_found)
screen_setup();
// Init GPS
@@ -275,24 +293,28 @@ void setup() {
// Show logo on first boot after removing battery
//if (bootCount == 0) {
screen_print(APP_NAME " " APP_VERSION, 0, 0);
screen_show_logo();
screen_update();
delay(LOGO_DELAY);
screen_print(APP_NAME " " APP_VERSION, 0, 0);
screen_show_logo();
screen_update();
delay(LOGO_DELAY);
//}
initBLE("KHBT Test"); // FIXME, use a real name based on the macaddr
mesh_init();
BLEServer *serve = initBLE("KHBT Test"); // FIXME, use a real name based on the macaddr
BLEService *bts = createMeshBluetoothService(serve);
bts->start();
serve->getAdvertising()->addServiceUUID(bts->getUUID());
}
void loop() {
void loop()
{
gps_loop();
screen_loop();
mesh_loop();
loopBLE();
if(packetSent) {
if (packetSent)
{
packetSent = false;
sleep();
}
@@ -301,16 +323,21 @@ void loop() {
// if user presses button for more than 3 secs, discard our network prefs and reboot (FIXME, use a debounce lib instead of this boilerplate)
static bool wasPressed = false;
static uint32_t minPressMs; // what tick should we call this press long enough
if(!digitalRead(BUTTON_PIN)) {
if(!wasPressed) { // just started a new press
if (!digitalRead(BUTTON_PIN))
{
if (!wasPressed)
{ // just started a new press
Serial.println("pressing");
wasPressed = true;
minPressMs = millis() + 3000;
}
} else if(wasPressed) {
}
}
else if (wasPressed)
{
// we just did a release
wasPressed = false;
if(millis() > minPressMs) {
if (millis() > minPressMs)
{
// held long enough
screen_print("Erasing prefs");
delay(5000); // Give some time to read the screen
@@ -322,18 +349,24 @@ void loop() {
// Send every SEND_INTERVAL millis
static uint32_t last = 0;
static bool first = true;
if (0 == last || millis() - last > SEND_INTERVAL) {
if (false) {
if (0 == last || millis() - last > SEND_INTERVAL)
{
if (false)
{
last = millis();
first = false;
Serial.println("TRANSMITTED");
} else {
if (first) {
}
else
{
if (first)
{
screen_print("Waiting GPS lock\n");
first = false;
}
#ifdef GPS_WAIT_FOR_LOCK
if (millis() > GPS_WAIT_FOR_LOCK) {
if (millis() > GPS_WAIT_FOR_LOCK)
{
sleep();
}
#endif