Pathfindr
init
Dependencies: aconno_I2C Lis2dh12 WatchdogTimer
Diff: main.cpp
- Revision:
- 58:8d4a354816b1
- Parent:
- 57:066dfbe8b4b9
diff -r 066dfbe8b4b9 -r 8d4a354816b1 main.cpp
--- a/main.cpp Thu Feb 13 00:57:06 2020 +0000
+++ b/main.cpp Mon Feb 17 23:24:52 2020 +0000
@@ -1,4 +1,4 @@
- #include "main.h"
+#include "main.h"
// MUST USE MBED 5.10.4 2fd0c5cfbd
/*
@@ -26,12 +26,16 @@
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;
-uint8_t RET_state = STATE_SETUP;
-uint8_t RET_state_prev = RET_state;
bool RET_asleep = false;
bool RET_coldBoot = true;
@@ -73,9 +77,14 @@
time_t EVENT_settingsScan_time = 0;
bool EVENT_uwbScan = false;
-time_t EVENT_uwbScan_interval = 60;
+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;
@@ -108,6 +117,8 @@
//GPIO
//------------------------------------------------------------------------------
InterruptIn button(PN_IN_BUTTON);
+InterruptIn accInt1(PN_ACC_INT1);
+//InterruptIn DW_irq(DW1000_IRQ);
//------------------------------------------------------------------------------
//PERIPHERALS
@@ -117,6 +128,10 @@
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
//------------------------------------------------------------------------------
@@ -200,7 +215,7 @@
//------------------------------------------------------------------------------
-// USER BUTTON HANDLING
+// INTERUPTS
//------------------------------------------------------------------------------
void buttonPress() {
RET_buttonPressTime = LPtimer.read_ms();
@@ -220,7 +235,10 @@
RET_buttonReleaseTime_prev = RET_buttonReleaseTime;
}
-
+void accInt1ISR() {
+ //led2 = !led2;
+ RET_motionLastDetected = RET_RTCunixtime;
+}
//------------------------------------------------------------------------------
// RTC TICKER
@@ -253,9 +271,25 @@
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;
+ }
}
- if (RET_haveEventsToRun) {
+ //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();
}
}
@@ -430,7 +464,7 @@
// BLE SETTINGS SCAN
//------------------------------------------------------------------------------
void settingsScan() {
- LED1on(0);
+ //LED1on(0);
bleStopAdvertising();
LowPowerTimer bleScan_t;
bleScan_t.start();
@@ -452,17 +486,62 @@
}
bleScan_t.stop();
myble.gap().stopScan();
- LED1off();
+ //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(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "MainStateEngine");debug_exe();}
+ if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "MainStateEngine");debug_exe();}
if (EVENT_buttonPress) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonPress");debug_exe();}
+ 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;
@@ -470,60 +549,68 @@
}
if (EVENT_motionClear) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_MotionClear");debug_exe();}
+ 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;
- EVENT_motionClear = false;
}
if (EVENT_buttonClear) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_ButtonClear");debug_exe();}
+ 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;
- EVENT_buttonClear = false;
}
if (EVENT_battery) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Battery");debug_exe();}
+ 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;
- EVENT_battery = false;
}
if (EVENT_temperature) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Temperature");debug_exe();}
+ 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);
- RET_BLEpacketUpdate = true;
EVENT_temperature = false;
//get temperature TODO - only update ble if different to last
+ RET_BLEpacketUpdate = true;
}
if (EVENT_humidity) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_Humidity");debug_exe();}
+ 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);
- RET_BLEpacketUpdate = true;
EVENT_humidity = false;
//get humidity TODO - only update ble if different to last
+ RET_BLEpacketUpdate = true;
}
if (EVENT_settingsScan) {
- if(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_SettingsScan");debug_exe();}
+ 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(debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "EVENT_uwbScan");debug_exe();}
+ 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) {
@@ -535,13 +622,15 @@
}
if (RET_BLEpacketUpdate == true) {
if(RET_debug) {debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "BLE Update only");debug_exe();}
- bleUpdateAndAdvertise();
- }
+ bleUpdateAndAdvertise(); //this function also sets RET_BLEpacketUpdate = false
+ }
//END
RET_haveEventsToRun = false;
}
+
+
//------------------------------------------------------------------------------
// MAIN
//------------------------------------------------------------------------------
@@ -554,13 +643,32 @@
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;
- LED1on(20);
+ //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();
@@ -573,6 +681,7 @@
//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;
@@ -580,13 +689,13 @@
}
//PRE-SLEEP ACTIONS
- //if (GLOBAL_needToConfigureLis3dh) { lis3dh_configureForSleep(RET_setting_motion_g,RET_setting_impact_g); }
+ acc_configForMotionInt();
watchdogKick();
//SLEEP
nrf_configureForSleep();
NRFuart_uninit();
RET_asleep = true;
- LED1off();
+ //LED1off();
mainthread.wait(DEFAULT_SLEEP_FRAME);
}
}
\ No newline at end of file