#include "SPI.h" #include "Adafruit_GFX.h" #include "Adafruit_GC9A01A.h" #include #include #include #include // ADJUST FOR SENSOR OFFSET int NORTH_OFFSET = 0; // PIN DEFINITIONS #define TFT_DC 28 #define TFT_CS 27 #define INT_PIN 26 // BNO055 REGISTERS #define BNO055_PAGE_ID_ADDR 0x07 #define BNO055_ACC_AM_THRES_ADDR 0x11 #define BNO055_ACC_INT_SETTINGS_ADDR 0x12 #define BNO055_INT_MSK_ADDR 0x0F #define BNO055_INT_EN_ADDR 0x10 #define BNO055_INT_STA_ADDR 0x37 #define BNO055_SYS_TRIGGER_ADDR 0x3F #define BATTERY_CHECK_INTERVAL 4000 #define SLEEP_TIMEOUT_MS 9000 // 9 seconds #define DIFF 1 #define ARROW_WIDTH 3 const uint8_t circle_lut[360][2] = { {230, 120}, {230, 122}, {230, 124}, {230, 126}, {230, 128}, {230, 130}, {229, 131}, {229, 133}, {229, 135}, {229, 137}, {228, 139}, {228, 141}, {228, 143}, {227, 145}, {227, 147}, {226, 148}, {226, 150}, {225, 152}, {225, 154}, {224, 156}, {223, 158}, {223, 159}, {222, 161}, {221, 163}, {220, 165}, {220, 166}, {219, 168}, {218, 170}, {217, 172}, {216, 173}, {215, 175}, {214, 177}, {213, 178}, {212, 180}, {211, 182}, {210, 183}, {209, 185}, {208, 186}, {207, 188}, {205, 189}, {204, 191}, {203, 192}, {202, 194}, {200, 195}, {199, 196}, {198, 198}, {196, 199}, {195, 200}, {194, 202}, {192, 203}, {191, 204}, {189, 205}, {188, 207}, {186, 208}, {185, 209}, {183, 210}, {182, 211}, {180, 212}, {178, 213}, {177, 214}, {175, 215}, {173, 216}, {172, 217}, {170, 218}, {168, 219}, {166, 220}, {165, 220}, {163, 221}, {161, 222}, {159, 223}, {158, 223}, {156, 224}, {154, 225}, {152, 225}, {150, 226}, {148, 226}, {147, 227}, {145, 227}, {143, 228}, {141, 228}, {139, 228}, {137, 229}, {135, 229}, {133, 229}, {131, 229}, {130, 230}, {128, 230}, {126, 230}, {124, 230}, {122, 230}, {120, 230}, {118, 230}, {116, 230}, {114, 230}, {112, 230}, {110, 230}, {109, 229}, {107, 229}, {105, 229}, {103, 229}, {101, 228}, {99, 228}, {97, 228}, {95, 227}, {93, 227}, {92, 226}, {90, 226}, {88, 225}, {86, 225}, {84, 224}, {82, 223}, {81, 223}, {79, 222}, {77, 221}, {75, 220}, {74, 220}, {72, 219}, {70, 218}, {68, 217}, {67, 216}, {65, 215}, {63, 214}, {62, 213}, {60, 212}, {58, 211}, {57, 210}, {55, 209}, {54, 208}, {52, 207}, {51, 205}, {49, 204}, {48, 203}, {46, 202}, {45, 200}, {44, 199}, {42, 198}, {41, 196}, {40, 195}, {38, 194}, {37, 192}, {36, 191}, {35, 189}, {33, 188}, {32, 186}, {31, 185}, {30, 183}, {29, 182}, {28, 180}, {27, 178}, {26, 177}, {25, 175}, {24, 173}, {23, 172}, {22, 170}, {21, 168}, {20, 166}, {20, 165}, {19, 163}, {18, 161}, {17, 159}, {17, 158}, {16, 156}, {15, 154}, {15, 152}, {14, 150}, {14, 148}, {13, 147}, {13, 145}, {12, 143}, {12, 141}, {12, 139}, {11, 137}, {11, 135}, {11, 133}, {11, 131}, {10, 130}, {10, 128}, {10, 126}, {10, 124}, {10, 122}, {10, 120}, {10, 118}, {10, 116}, {10, 114}, {10, 112}, {10, 110}, {11, 109}, {11, 107}, {11, 105}, {11, 103}, {12, 101}, {12, 99}, {12, 97}, {13, 95}, {13, 93}, {14, 92}, {14, 90}, {15, 88}, {15, 86}, {16, 84}, {17, 82}, {17, 81}, {18, 79}, {19, 77}, {20, 75}, {20, 74}, {21, 72}, {22, 70}, {23, 68}, {24, 67}, {25, 65}, {26, 63}, {27, 62}, {28, 60}, {29, 58}, {30, 57}, {31, 55}, {32, 54}, {33, 52}, {35, 51}, {36, 49}, {37, 48}, {38, 46}, {40, 45}, {41, 44}, {42, 42}, {44, 41}, {45, 40}, {46, 38}, {48, 37}, {49, 36}, {51, 35}, {52, 33}, {54, 32}, {55, 31}, {57, 30}, {58, 29}, {60, 28}, {62, 27}, {63, 26}, {65, 25}, {67, 24}, {68, 23}, {70, 22}, {72, 21}, {74, 20}, {75, 20}, {77, 19}, {79, 18}, {81, 17}, {82, 17}, {84, 16}, {86, 15}, {88, 15}, {90, 14}, {92, 14}, {93, 13}, {95, 13}, {97, 12}, {99, 12}, {101, 12}, {103, 11}, {105, 11}, {107, 11}, {109, 11}, {110, 10}, {112, 10}, {114, 10}, {116, 10}, {118, 10}, {120, 10}, {122, 10}, {124, 10}, {126, 10}, {128, 10}, {130, 10}, {131, 11}, {133, 11}, {135, 11}, {137, 11}, {139, 12}, {141, 12}, {143, 12}, {145, 13}, {147, 13}, {148, 14}, {150, 14}, {152, 15}, {154, 15}, {156, 16}, {158, 17}, {159, 17}, {161, 18}, {163, 19}, {165, 20}, {166, 20}, {168, 21}, {170, 22}, {172, 23}, {173, 24}, {175, 25}, {177, 26}, {178, 27}, {180, 28}, {182, 29}, {183, 30}, {185, 31}, {186, 32}, {188, 33}, {189, 35}, {191, 36}, {192, 37}, {194, 38}, {195, 40}, {196, 41}, {198, 42}, {199, 44}, {200, 45}, {202, 46}, {203, 48}, {204, 49}, {205, 51}, {207, 52}, {208, 54}, {209, 55}, {210, 57}, {211, 58}, {212, 60}, {213, 62}, {214, 63}, {215, 65}, {216, 67}, {217, 68}, {218, 70}, {219, 72}, {220, 74}, {220, 75}, {221, 77}, {222, 79}, {223, 81}, {223, 82}, {224, 84}, {225, 86}, {225, 88}, {226, 90}, {226, 92}, {227, 93}, {227, 95}, {228, 97}, {228, 99}, {228, 101}, {229, 103}, {229, 105}, {229, 107}, {229, 109}, {230, 110}, {230, 112}, {230, 114}, {230, 116}, {230, 118}, }; const char* cardinal_lut[360] = { " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NNE", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "NE ", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", "ENE", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", " E ", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "ESE", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SE ", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", "SSE", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", " S ", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SSW", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "SW ", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", "WSW", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", " W ", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "WNW", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NW ", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", "NNW", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", " N ", }; Adafruit_GC9A01A tft(TFT_CS, TFT_DC); Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire1); GFXcanvas16 canvas(240, 240); float batVal = 0; unsigned long lastBatteryCheck = 0; bool calibDir = false; int calibRadius = 100; int drawArrow = false; unsigned long lastActivityTime = 0; int lastHeading = 0; void gpio_callback() { // Empty interrupt handler for waking } static void writeRegister(uint8_t reg, uint8_t value) { Wire1.beginTransmission(0x28); Wire1.write(reg); Wire1.write(value); Wire1.endTransmission(); } void setup() { Serial.begin(9600); pinMode(19, OUTPUT); // Battery read enable pinMode(29, INPUT); // Battery voltage pin pinMode(INT_PIN, INPUT_PULLDOWN); // Accel interrupt pinMode(5, OUTPUT); // LCD power attachInterrupt(digitalPinToInterrupt(INT_PIN), gpio_callback, RISING); digitalWrite(5, HIGH); // Turn on LCD digitalWrite(19, HIGH); // Turn on battery read enable tft.begin(80000000); tft.setRotation(2); if(!bno.begin()) { while(1); } // Issue with sensor enableMotionInterrupt(); clearBNOInterrupt(); tft.fillScreen(GC9A01A_BLACK); lastActivityTime = millis(); } void loop(void) { if (millis() - lastActivityTime > SLEEP_TIMEOUT_MS) { if (attemptHardwareSleep()) { exitHardwareSleep(); } else { lastActivityTime = millis(); } } sensors_event_t orientationData; bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER); int heading = (int)orientationData.orientation.x % 360; int diff = abs(heading - lastHeading); if (diff > 180) diff = 360 - diff; if (diff > DIFF) { lastActivityTime = millis(); lastHeading = heading; } canvas.fillScreen(GC9A01A_BLACK); // CALIBRATION uint8_t s, g, a, m = 0; bno.getCalibration(&s, &g, &a, &m); if (m < 3) { canvas.setCursor(90, 105); canvas.setTextColor(GC9A01A_WHITE); canvas.setTextSize(2); canvas.println("Please"); canvas.setCursor(70, 125); canvas.println("Calibrate"); canvas.drawCircle(120, 120, 119, GC9A01A_RED); canvas.drawCircle(120, 120, calibRadius, GC9A01A_YELLOW); if(calibDir) calibRadius--; else calibRadius++; if(calibRadius > 119) calibDir = true; if(calibRadius < 100) calibDir = false; drawArrow = false; } else { canvas.drawCircle(120, 120, 119, GC9A01A_GREEN); drawArrow = true; } // Compass thingies heading = heading + NORTH_OFFSET; int idx = (270 - heading + 360) % 360; int south = (idx + 180) % 360; if(drawArrow) { drawNeedleToCanvas(idx, GC9A01A_RED); drawNeedleToCanvas(south, GC9A01A_WHITE); canvas.setCursor(95, 82); canvas.print(cardinal_lut[heading]); canvas.fillTriangle(120, 60, 115, 75, 125, 75, GC9A01A_WHITE); if(heading > 99) { // three digits canvas.setCursor(95, 108); } else if( heading > 9) { // two digits canvas.setCursor(105, 108); } else { // one digit canvas.setCursor(115, 108); } canvas.setTextColor(GC9A01A_WHITE); canvas.setTextSize(3); canvas.print(heading); canvas.print((char)247); // deg symbol canvas.setCursor(80, 140); if (millis() - lastBatteryCheck > BATTERY_CHECK_INTERVAL) { batVal = (readBatteryVoltage() * 100) / 100; lastBatteryCheck = millis(); } canvas.print(batVal); canvas.print("V"); } // alignment /* canvas.drawLine(0, 120, 240, 120, GC9A01A_WHITE); canvas.drawLine(120, 0, 120, 240, GC9A01A_WHITE); */ tft.drawRGBBitmap(0, 0, canvas.getBuffer(), 240, 240); } /////////////////////////////////////// /// Functions /// /////////////////////////////////////// void drawNeedleToCanvas(int index, uint16_t color) { if(index >= 360) index = 0; int tipX = circle_lut[index][0]; int tipY = circle_lut[index][1]; int leftIdx = (index - ARROW_WIDTH + 360) % 360; int rightIdx = (index + ARROW_WIDTH + 360) % 360; int leftX = 120 + ((circle_lut[leftIdx][0] - 120) * 3) / 4; int leftY = 120 + ((circle_lut[leftIdx][1] - 120) * 3) / 4; int rightX = 120 + ((circle_lut[rightIdx][0] - 120) * 3) / 4; int rightY = 120 + ((circle_lut[rightIdx][1] - 120) * 3) / 4; canvas.fillTriangle(tipX, tipY, leftX, leftY, rightX, rightY, color); } static float readBatteryVoltage() { // The XIAO RP2350 sends the battery voltage to Pin 29 int raw = analogRead(29); float voltage = raw * 6.5 / 1024.0; Serial.println(voltage); return (float)(voltage); } /////////////////////////////////////// /// Sleep Helpers /// /////////////////////////////////////// bool attemptHardwareSleep() { Serial.println("Attempting Sleep"); // force INT low clearBNOInterrupt(); delay(100); // ensure low if (digitalRead(INT_PIN) == HIGH) { Serial.println("Pin stuck HIGH (Moving?)"); return false; } // Sleep the display tft.sendCommand(0x10); // Loop trap while(digitalRead(INT_PIN) == LOW) { digitalWrite(5,LOW); __wfi(); } return true; } void exitHardwareSleep() { Serial.println("Wake!"); digitalWrite(5,HIGH); // turn on lcd delay(120); tft.sendCommand(0x11); // wake delay(120); clearBNOInterrupt(); lastActivityTime = millis(); } /////////////////////////////////////// /// Operation Helpers /// /////////////////////////////////////// static void clearBNOInterrupt() { Wire1.beginTransmission(0x28); Wire1.write(BNO055_INT_STA_ADDR); Wire1.endTransmission(); Wire1.requestFrom(0x28, 1); if (Wire1.available()) Wire1.read(); // try to clear INT with read // Write 0x40 (Bit 6) to SYS_TRIGGER (0x3F) to force reset INT Wire1.beginTransmission(0x28); Wire1.write(BNO055_SYS_TRIGGER_ADDR); Wire1.write(0x40); Wire1.endTransmission(); } static void enableMotionInterrupt() { bno.setMode(OPERATION_MODE_CONFIG); delay(25); writeRegister(BNO055_PAGE_ID_ADDR, 1); // 0x14 = 80mg writeRegister(BNO055_ACC_AM_THRES_ADDR, 0x14); // 0x1C = (Bits 4,3,2 enabled for X,Y,Z) | (Bits 1,0 = 00 for minimal duration) writeRegister(BNO055_ACC_INT_SETTINGS_ADDR, 0x1C); // any motion to wake writeRegister(BNO055_INT_MSK_ADDR, 0x40); writeRegister(BNO055_INT_EN_ADDR, 0x40); // return for the library writeRegister(BNO055_PAGE_ID_ADDR, 0); bno.setMode(OPERATION_MODE_NDOF); delay(25); }