File content as of revision 58:8d4a354816b1:

#include "main.h"
// MUST USE MBED 5.10.4   2fd0c5cfbd

/*
PRE DEPLOYMENT CHECKS!!
• Ensure APP DATA address is correct
*/

//------------------------------------------------------------------------------
//FUNCTION PROTOTYPES - NEED TO ADD ALL OF THE MISSING ONES
//------------------------------------------------------------------------------ 
//should really add these - will just add as and when needed for now
void bleShutdown();

//------------------------------------------------------------------------------
// VARS
//------------------------------------------------------------------------------
char             RET_pf_identifier[6];
time_t           RET_RTCunixtime                                    = 0;
bool             RET_debug                                          = true;
time_t           RET_debug_offat                                    = 0;
char             GLOBAL_debug_buffer[DEBUG_BUFFERSIZE];

//SENSORS
float            RET_voltage                                        = 0.0;
float            RET_temperature                                    = 0.0;
float            RET_humidity                                       = 0.0;
bool             RET_motionState                                    = false;
int8_t           RET_accX                                           = 0;
int8_t           RET_accY                                           = 0;
int8_t           RET_accZ                                           = 0;
time_t           RET_motionLastDetected                             = 0;
time_t           RET_InMotionSeconds                                = 0;
time_t           RET_NoMotionSeconds                                = 0;
bool             RET_impactState                                    = false;

//STATE
bool             RET_busy                                           = false;
bool             RET_asleep                                         = false;
bool             RET_coldBoot                                       = true;

//BUTTON
bool             RET_buttonPressed                                  = false;
uint8_t          RET_buttonPressCount                               = 0;
time_t           RET_buttonPressTime                                = 0;
time_t           RET_buttonReleaseTime                              = 0;
time_t           RET_buttonReleaseTime_prev                         = 0;
time_t           RET_buttonHoldTime                                 = 0;

//EVENTS
bool             RET_haveEventsToRun                                = false;

bool             EVENT_buttonPress                                  = false;

bool             EVENT_battery                                      = false;
time_t           EVENT_battery_interval                             = DEFAULT_INTERVAL_BATTERY;
time_t           EVENT_battery_time                                 = 0;

bool             EVENT_motionClear                                  = false;
time_t           EVENT_motionClear_interval                         = DEFAULT_INTERVAL_MOTIONCLEAR;
time_t           EVENT_motionClear_time                             = 0;

bool             EVENT_buttonClear                                  = false;
time_t           EVENT_buttonClear_interval                         = DEFAULT_INTERVAL_BUTTONCLEAR;
time_t           EVENT_buttonClear_time                             = 0;

bool             EVENT_temperature                                  = false;
time_t           EVENT_temperature_interval                         = DEFAULT_INTERVAL_TEMPERATURE;
time_t           EVENT_temperature_time                             = 0;

bool             EVENT_humidity                                     = false;
time_t           EVENT_humidity_interval                            = DEFAULT_INTERVAL_HUMIDITY;
time_t           EVENT_humidity_time                                = 0;

bool             EVENT_settingsScan                                 = false;
time_t           EVENT_settingsScan_interval                        = DEFAULT_INTERVAL_SETTINGSSCAN;
time_t           EVENT_settingsScan_time                            = 0;

bool             EVENT_uwbScan                                      = false;
time_t           EVENT_uwbScan_interval                             = 60;
time_t           EVENT_uwbScan_time                                 = 0;

bool             EVENT_resetACCInt                                  = false;
time_t           EVENT_resetACCInt_interval                         = DEFAULT_INTERVAL_RESETACCINT;
time_t           EVENT_resetACCInt_time                             = 0;


//BLE
uint16_t         RET_setting_beacon_interval_ms                     = DEFAULT_BEACON_INTERVAL_MS;
uint16_t         RET_setting_beacon_interval_ms_active              = DEFAULT_BEACON_INTERVAL_MS;
bool             RET_bleBroadcasting                                = false;
uint8_t          RET_bleAdv_flags                                   = 0;
bool             RET_UWBdetector_present                            = true; //BEN should be false
bool             RET_BLEpacketUpdate                                = false;
bool             RET_settingsReceived                               = false;
const uint8_t    bleAdv_motion_flag                                 = 0x01; //motion detected
const uint8_t    bleAdv_impact_flag                                 = 0x02; //impact detected
const uint8_t    bleAdv_button_pressed_flag                         = 0x08; //button press detected
const uint8_t    bleAdv_gap_connected_flag                          = 0x40; //device is connected
#pragma pack(1)
struct bleData_t {
    uint16_t applicationSpecificId;
    uint8_t firmware;
    uint8_t flags;
    uint16_t voltage;
    uint8_t buttonpresses;
    int16_t temperature;
    uint8_t humidity;
    uint8_t lux;
    int8_t accel_x;
    int8_t accel_y;
    int8_t accel_z;
};
#pragma pack()
    
//------------------------------------------------------------------------------
//GPIO
//------------------------------------------------------------------------------ 
InterruptIn button(PN_IN_BUTTON);
InterruptIn accInt1(PN_ACC_INT1);
//InterruptIn DW_irq(DW1000_IRQ);

//------------------------------------------------------------------------------
//PERIPHERALS
//------------------------------------------------------------------------------ 
BLE myble;
WatchdogTimer watchdog;
LowPowerTicker RTCticker;
LowPowerTimer LPtimer;

I2C i2c(PN_I2C_SDA,PN_I2C_SCL);
SPI spi(PN_SPI_MOSI,PN_SPI_MISO,PN_SPI_CLK);
Lis2dh12 acc(&i2c, 0x32);

//------------------------------------------------------------------------------
//SEMAPHORES
//------------------------------------------------------------------------------ 
Semaphore mainthread;

//------------------------------------------------------------------------------
// LOW LEVEL FUNCS
//------------------------------------------------------------------------------
void watchdogKick() {
    watchdog.kick();
}
void set_8bit_flag(uint8_t& flags, uint8_t flag) {
    flags |= flag;
}
bool get_8bit_flag(uint8_t& flags, uint8_t flag) {
    bool result = (flags & flag);
    return result;
}
void clr_8bit_flag(uint8_t& flags, uint8_t flag) {
    flags &= ~flag;
}
void nrf_configureForSleep(){
    //Disable SPI pins to reduce power
    //nrf_gpio_cfg_input(PN_SPI_MOSI, NRF_GPIO_PIN_NOPULL);  //Don't need this one
    nrf_gpio_cfg_input(PN_SPI_MISO, NRF_GPIO_PIN_NOPULL);
    
    //TWI0
    NRF_TWI0->ENABLE = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos;
    *(volatile uint32_t *)0x40003FFC = 0;
    *(volatile uint32_t *)0x40003FFC;
    *(volatile uint32_t *)0x40003FFC = 1;
    //TWI1
    NRF_TWI1->ENABLE=TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos;
    *(volatile uint32_t *)0x40004FFC = 0;
    *(volatile uint32_t *)0x40004FFC;
    *(volatile uint32_t *)0x40004FFC = 1;
}

void recordFirmwareAsValid() {
    bleShutdown();
    read_app_data_from_flash(&app_data);
    bool write_app_data_to_flash_execute = false;
    if(get_flag(&app_data, app_execution_flag) == true) {
        clr_flag(&app_data, app_execution_flag);
        clr_flag(&app_data, first_run_flag);
        write_app_data_to_flash_execute = true;
    }
    if(app_data.current_firmware_version != FW_VERSION) {
        app_data.current_firmware_version = FW_VERSION;
        app_data.target_firmware_version = FW_VERSION;
        write_app_data_to_flash_execute = true;
    }
    if (write_app_data_to_flash_execute) {
        write_app_data_to_flash(&app_data);
        //read back to check
        ThisThread::sleep_for(200); //need this delay
        read_app_data_from_flash(&app_data);
        if (app_data.current_firmware_version == FW_VERSION) {
            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "OK");debug_exe();
        } else {
            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "FAIL - IS BLE SHUTDOWN?");debug_exe();
        }
    }
}
void updateBatteryV() {
    NRF52_SAADC batteryIn;
    batteryIn.addChannel(9); // vdd for battery
    batteryIn.calibrate();
    //get initial reading, first reading is always low
    batteryIn.updateData(); batteryIn.getData()[0];
    float voltage_accumulator = 0.0;
    uint8_t readings = 0;
    for (uint8_t i = 1; i <= 50; i++) { // need to get it 2 times to get accurate data, first one is always low for some reason
        batteryIn.updateData();
        voltage_accumulator += (batteryIn.getData()[0])*(1.0/1024.0)*3.60;
        readings ++;
    }
    RET_voltage = (voltage_accumulator / (float)readings);
}



//------------------------------------------------------------------------------
// INTERUPTS
//------------------------------------------------------------------------------ 
void buttonPress() {
    RET_buttonPressTime = LPtimer.read_ms();
}
void buttonRelease() {
    //debounce catch
    RET_buttonReleaseTime = LPtimer.read_ms();
    if ((RET_buttonReleaseTime - RET_buttonReleaseTime_prev) > 100) { 
        RET_buttonPressCount ++;
        RET_buttonHoldTime = (RET_buttonReleaseTime - RET_buttonPressTime);
        //temporarily enable debugging
        RET_debug = true;
        RET_debug_offat = (RET_RTCunixtime + 3600); //debug on for 1hr
        EVENT_buttonPress = true;
        mainthread.release();
    }
    RET_buttonReleaseTime_prev = RET_buttonReleaseTime;
}

void accInt1ISR() {
    //led2 = !led2;
    RET_motionLastDetected = RET_RTCunixtime;
}

//------------------------------------------------------------------------------
// RTC TICKER
//------------------------------------------------------------------------------
bool RET_flash_busy = false;
void RTCtick() {
    //YOU MUST NOT CALL ANY OTHER FUNCTIONS OR DEBUG FROM INSIDE HERE!!! OR IT LOCKS UP THE DEVICE, just change vars & comparisons etc
    RET_RTCunixtime += 1;
    
    //CHECK FOR EVENTS
    if (RET_haveEventsToRun == false) {
        if (EVENT_motionClear_time > 0 && RET_RTCunixtime >= EVENT_motionClear_time) {
            RET_haveEventsToRun = true; EVENT_motionClear = true;
        }
        if (EVENT_buttonClear_time > 0 && RET_RTCunixtime >= EVENT_buttonClear_time) {
            RET_haveEventsToRun = true; EVENT_buttonClear = true;
        }
        if (EVENT_battery_time > 0 && RET_RTCunixtime >= EVENT_battery_time) {
            RET_haveEventsToRun = true; EVENT_battery = true;
        }
        if (EVENT_temperature_time > 0 && RET_RTCunixtime >= EVENT_temperature_time) {
            RET_haveEventsToRun = true; EVENT_temperature = true;
        }
        if (EVENT_humidity_time > 0 && RET_RTCunixtime >= EVENT_humidity_time) {
            RET_haveEventsToRun = true; EVENT_humidity = true;
        }
        if (EVENT_settingsScan_time > 0 && RET_RTCunixtime >= EVENT_settingsScan_time) {
            RET_haveEventsToRun = true; EVENT_settingsScan = true;
        }
        if (EVENT_uwbScan_time > 0 && RET_RTCunixtime >= EVENT_uwbScan_time) {
            RET_haveEventsToRun = true; EVENT_uwbScan = true;
        }
        if (EVENT_resetACCInt_time > 0 && RET_RTCunixtime >= EVENT_resetACCInt_time) {
            RET_haveEventsToRun = true; EVENT_resetACCInt = true;
        }
    }
    
    //CHECK FOR MOTION OPT 
    //Had motion in last 5 seconds?
    if (RET_motionLastDetected > 0 && (RET_RTCunixtime-RET_motionLastDetected) < 5) {
        EVENT_motionClear_time = (RET_RTCunixtime + EVENT_motionClear_interval);
        if (RET_motionState == 0) {
            RET_motionState = 1;
            RET_BLEpacketUpdate = true;
            RET_haveEventsToRun = true;
        }
    }
    led1 = !RET_motionState;
    
    //WAKE UP MAIN LOOP AND RUN ANY EVENTS
    if (RET_haveEventsToRun && RET_asleep) {
        mainthread.release();
    }
}


//------------------------------------------------------------------------------
// BLE FUNCS
//------------------------------------------------------------------------------
void bleBuildData(bleData_t &bleData)
{
    bleData.applicationSpecificId = BLE_SERVICEID_UWBBEACON;
    bleData.firmware = FW_VERSION;
    //set flags
    if (RET_motionState) {
        set_8bit_flag(RET_bleAdv_flags, bleAdv_motion_flag);
    } else {
        clr_8bit_flag(RET_bleAdv_flags, bleAdv_motion_flag);
    }
    
    if (RET_impactState) {
        set_8bit_flag(RET_bleAdv_flags, bleAdv_impact_flag);
    } else {
        clr_8bit_flag(RET_bleAdv_flags, bleAdv_impact_flag);
    }
    
    if(RET_buttonPressed) {
        set_8bit_flag(RET_bleAdv_flags, bleAdv_button_pressed_flag);
    } else {
        clr_8bit_flag(RET_bleAdv_flags, bleAdv_button_pressed_flag);
    }
    
    bleData.flags = RET_bleAdv_flags;
    bleData.voltage = RET_voltage;
    bleData.buttonpresses = RET_buttonPressCount;
    bleData.temperature = RET_temperature;
    bleData.humidity = 0xFF;
    bleData.lux = 0xFF;
    bleData.accel_x = 0xFF;
    bleData.accel_y = 0xFF;
    bleData.accel_z = 0xFF;
}
void bleUpdateAndAdvertise(void) 
{
    bleData_t bleData;
    bleBuildData(bleData);
    myble.gap().stopAdvertising();
    myble.gap().setTxPower(DEFAULT_BEACON_POWER); // set tx power,valid values are -40, -20, -16, -12, -8, -4, 0, 4
    myble.gap().accumulateAdvertisingPayloadTxPower(DEFAULT_BEACON_POWER); // (accepted values are -40, -30, -20, -16, -12, -8, -4, 0, and 4 dBm)
    myble.gap().setAdvertisingInterval(RET_setting_beacon_interval_ms_active);
    myble.gap().updateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA, (uint8_t *)&bleData, sizeof(bleData_t));
    //myble.gap().updateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&bleData, sizeof(bleData_t));
    myble.gap().startAdvertising();
    RET_bleBroadcasting = true;
    RET_BLEpacketUpdate = false;
}
void bleSetupAdvertising(BLE::InitializationCompleteCallbackContext *params)
{
    if (RET_pf_identifier[0] != 0x00 && RET_pf_identifier[1] != 0x00 && RET_pf_identifier[2] != 0x00) {
        bleData_t bleData;
        bleBuildData(bleData);
        char localname[6];
        memcpy(localname,RET_pf_identifier,6);
        myble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE );
        myble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)localname, sizeof(localname));
        myble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA, (uint8_t *)&bleData, sizeof(bleData_t));
        ////myble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&bleData, sizeof(bleData_t));
        myble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);
    }
}
void bleStopAdvertising(void)
{
    myble.gap().stopAdvertising();
    RET_bleBroadcasting = false;
    if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "BLE Adv Stop");debug_exe();}
}
void bleAdvertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
    struct AdvertisingData_t {
        uint8_t                        length;
        GapAdvertisingData::DataType_t dataType;
        uint8_t                        data[0];
    } PACKED;
    struct bleData_t {
        uint16_t applicationSpecificId;
    } PACKED;
    
    //Search for the manufacturer specific data with matching application-ID
    AdvertisingData_t *pAdvData;
    for (size_t index = 0; index < params->advertisingDataLen; index += (pAdvData->length + 1)) {
        pAdvData = (AdvertisingData_t *)&params->advertisingData[index];
        const bleData_t *pbleData = (const bleData_t *)pAdvData->data;
        
        //BLE TYPE SWITCH
        switch(pbleData->applicationSpecificId) {
            case BLE_SERVICEID_PFDETECTOR2 :
            {
                char ble_advdata[100];
                snprintf(ble_advdata, sizeof(ble_advdata), "%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
                params->advertisingData[0],
                params->advertisingData[1],
                params->advertisingData[2],
                params->advertisingData[3],
                params->advertisingData[4],
                params->advertisingData[5],
                params->advertisingData[6],
                params->advertisingData[7],
                params->advertisingData[8],
                params->advertisingData[9],
                params->advertisingData[10],
                params->advertisingData[11],
                params->advertisingData[12],
                params->advertisingData[13],
                params->advertisingData[14],
                params->advertisingData[15],
                params->advertisingData[16],
                params->advertisingData[17],
                params->advertisingData[18],
                params->advertisingData[19],
                params->advertisingData[20]);
                //if(RET_debug){debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), " Mac: [%02x%02x%02x%02x%02x%02x], rssi: %d, AType: %u, Id: %s",params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],params->rssi, pAdvData->dataType, ble_advdata);debug_exe();}
                //if(RET_debug){debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Detector @rssi: %d",params->rssi);debug_exe();}
                if (params->rssi >= -75) {
                    
                }
                RET_settingsReceived = true;
                break;
            }
        }
    }
}
void bleConfigureScan(void)
{
    myble.gap().setScanParams(200,200); //scan interval //scan window
    myble.gap().startScan(bleAdvertisementCallback);
}
void bleStopScan(void)
{
    myble.gap().stopScan();
}
void bleShutdown(void)
{
    bleStopScan();
    bleStopAdvertising();
    myble.shutdown();
}

//------------------------------------------------------------------------------
// FIRST RUN
//------------------------------------------------------------------------------ 
void firstRun() {
    //Set Serial Number
    RET_pf_identifier[0] = 'U';
    RET_pf_identifier[1] = 'W';
    RET_pf_identifier[2] = 'B';
    RET_pf_identifier[3] = '0';
    RET_pf_identifier[4] = '1';
    RET_pf_identifier[5] = 0x00;
    
    //GET INITITAL SENSORS
    updateBatteryV();
    //updateTemperature
    //updateHumidity

    //SET EVENT TIMES
    EVENT_battery_time = (RET_RTCunixtime + EVENT_battery_interval);
    EVENT_temperature_time = (RET_RTCunixtime + EVENT_temperature_interval);
    EVENT_humidity_time = (RET_RTCunixtime + EVENT_humidity_interval);
    EVENT_settingsScan_time = (RET_RTCunixtime + EVENT_settingsScan_interval);
    EVENT_uwbScan_time = (RET_RTCunixtime + EVENT_uwbScan_interval);
}

//------------------------------------------------------------------------------
// BLE SETTINGS SCAN
//------------------------------------------------------------------------------ 
void settingsScan() {
    //LED1on(0);
    bleStopAdvertising();
    LowPowerTimer bleScan_t;
    bleScan_t.start();
    RET_settingsReceived = false;
    uint32_t bleScan_startmillis = bleScan_t.read_ms(); uint32_t bleScan_runtime = 0;
    if (!myble.hasInitialized()) {
        myble.init();
        while (!myble.hasInitialized() && bleScan_runtime < 1000) { 
            bleScan_runtime = (bleScan_t.read_ms() - bleScan_startmillis);
        }
    }
    if (myble.hasInitialized()) {
        bleConfigureScan();
        bleScan_startmillis = bleScan_t.read_ms();
        while (bleScan_runtime < DEFAULT_SETTINGSSCANTIME_MS && RET_settingsReceived == false) {
            bleScan_runtime = (bleScan_t.read_ms() - bleScan_startmillis);
            myble.waitForEvent(); // low power wait for event
        }
    }
    bleScan_t.stop();
    myble.gap().stopScan();
    //LED1off();
}

//------------------------------------------------------------------------------
// UWB SCAN
//------------------------------------------------------------------------------ 
struct __attribute__((packed, aligned(1))) DistancesFrame {
        uint8_t source;
        uint8_t destination;
        uint8_t type;
        float dist[4];
    };
    
DW1000 dw(PN_SPI_MOSI, PN_SPI_MISO, PN_SPI_CLK, DW1000_CS, DW1000_IRQ, DW1000_RST);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
//DW1000 dw(&spi,DW_irq,DW1000_CS,DW1000_RST);
MM2WayRanging node(dw);

void uwbScan() {
    
}

//------------------------------------------------------------------------------
// ACC
//------------------------------------------------------------------------------ 
void acc_configForMotionInt() {
    acc.setMode(LOW_POWER);
    acc.enableAxes(X_axis);
    acc.enableAxes(Y_axis);
    acc.enableAxes(Z_axis);    
    acc.setODR(ODR_1Hz);
    acc.setScale(_8g);
    acc.int1Setup(0b01000000);       // IntActivity 1 driven to INT1 pad
    //acc.setCTRL_REG2(0b11001011);  //High pass filter active
    acc.setCTRL_REG2(0b00000000);    //High pass filter off
    acc.int1Latch(0b00000000);       //Dont latch
    acc.int1Threshold(7);
    acc.int1Duration(0x00);  
    acc.int1Config(0b01111111);     // INT2_CFG Enable XHigh, YHigh and ZHigh, triggers INT1 on any motion
    acc.clearIntFlag(); //reset int1 
}

void acc_updateOrientation() {
    //Divide by 16 to get range of +- 16 on each axis
    RET_accX = (acc.readXAxis() / 16);
    RET_accY = (acc.readYAxis() / 16);
    RET_accZ = (acc.readZAxis() / 16);
}

//------------------------------------------------------------------------------
// STATE ENGINE
//------------------------------------------------------------------------------ 
void mainStateEngine() {
    if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "MainStateEngine");debug_exe();}
    
    if (EVENT_buttonPress) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonPress");debug_exe();}
        EVENT_buttonClear_time = (RET_RTCunixtime + EVENT_buttonClear_interval);
        RET_setting_beacon_interval_ms_active = DEFAULT_BEACON_INTERVAL_FAST_MS;
        RET_BLEpacketUpdate = true;
        EVENT_buttonPress = false;
    }
    
    if (EVENT_motionClear) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_MotionClear");debug_exe();}
        EVENT_motionClear_time = 0;
        EVENT_motionClear = false;
        RET_motionState = false;
        RET_impactState = false;
        acc_updateOrientation(); //update orientation data
        RET_BLEpacketUpdate = true;
    }
    
    if (EVENT_buttonClear) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonClear");debug_exe();}
        EVENT_buttonClear_time = 0;
        EVENT_buttonClear = false;
        RET_buttonPressed = false;
        RET_buttonPressCount = 0;
        RET_setting_beacon_interval_ms_active = RET_setting_beacon_interval_ms; //reset back to normal broadcast rate
        RET_BLEpacketUpdate = true;
    }
    
    if (EVENT_battery) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Battery");debug_exe();}
        EVENT_battery_time = (RET_RTCunixtime + EVENT_battery_interval);
        EVENT_battery = false;
        updateBatteryV();
        RET_BLEpacketUpdate = true;
    }
    
    if (EVENT_temperature) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Temperature");debug_exe();}
        EVENT_temperature_time = (RET_RTCunixtime + EVENT_temperature_interval);
        EVENT_temperature = false;
        //get temperature TODO - only update ble if different to last
        RET_BLEpacketUpdate = true;
    }
    
    if (EVENT_humidity) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Humidity");debug_exe();}
        EVENT_humidity_time = (RET_RTCunixtime + EVENT_humidity_interval);
        EVENT_humidity = false;
        //get humidity TODO - only update ble if different to last
        RET_BLEpacketUpdate = true;
    }
    
    if (EVENT_settingsScan) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_SettingsScan");debug_exe();}
        EVENT_settingsScan_time = (RET_RTCunixtime + EVENT_settingsScan_interval);
        EVENT_settingsScan = false;
        settingsScan();
    }
    
    if (EVENT_uwbScan) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_uwbScan");debug_exe();}
        EVENT_uwbScan_time = (RET_RTCunixtime + EVENT_uwbScan_interval);
        EVENT_uwbScan = false;
    }
    
    /*if (EVENT_resetACCInt) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_resetACCInt1");debug_exe();}
        EVENT_resetACCInt_time = 0;
        EVENT_resetACCInt = false;
        acc.clearIntFlag();
    }*/
        
    //BLE START OR UPDATE
    if (RET_bleBroadcasting == false) {
        myble.init(bleSetupAdvertising);
        if (myble.hasInitialized()) {
            if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "BLE Start");debug_exe();}
            bleUpdateAndAdvertise();
        }
    }
    if (RET_BLEpacketUpdate == true) {
        if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "BLE Update only");debug_exe();}
        bleUpdateAndAdvertise(); //this function also sets RET_BLEpacketUpdate = false
    }
    
    //END
    RET_haveEventsToRun = false;
}



//------------------------------------------------------------------------------
// MAIN
//------------------------------------------------------------------------------ 
int main() {
    //INIT
    LED1off();
    ThisThread::sleep_for(2000); //Initial pause, this is needed for softdevice to init, dont remove!! If we dont have this we get crashes
    watchdog.configure(400.0); //6.6 mins //Do not set to less than 4500ms or can cause issues with softdevice
    RTCticker.attach(&RTCtick, 1.0);
    LPtimer.start();
    button.fall(&buttonPress);
    button.rise(&buttonRelease);
    accInt1.rise(&accInt1ISR);
    //read_app_data_from_flash(&app_data);
    acc_configForMotionInt();
    firstRun();
    debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"Ready\n");debug_exe();
    
    //MAIN LOOP
    while(true) {
        RET_asleep = false;
        //LED2on(30);
        
        
        while(1) {
            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"UWB SCAN START\n");debug_exe();
            uwbScan();
            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"UWB SCAN END\n");debug_exe();
            nrf_configureForSleep();
            ThisThread::sleep_for(1000);
        }
        
        
        while(1) {
            acc_updateOrientation();
            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"X:%d    Y:%d   D:%d\n",RET_accX,RET_accY,RET_accZ);debug_exe();
            ThisThread::sleep_for(20);
        }
        
        //STATE ENGINE
        mainStateEngine();
        
        //LOG FIRST RUN - BOOTLOADER COMMS
        if (RET_coldBoot) {
            //recordFirmwareAsValid();
            RET_coldBoot = 0;
        }
        
        //DEBUGGING OFF TIMER
        if(RET_debug){
            debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"TIME:%d\n",RET_RTCunixtime);debug_exe();
            if (RET_RTCunixtime > RET_debug_offat) {
                debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer),"DEBUGGING OFF\n");debug_exe();
                RET_debug = false;
            }
        }
                
        //PRE-SLEEP ACTIONS
        acc_configForMotionInt();
        watchdogKick();
        //SLEEP
        nrf_configureForSleep();
        NRFuart_uninit();
        RET_asleep = true;
        //LED1off();
        mainthread.wait(DEFAULT_SLEEP_FRAME);
    }
}