Adafruit-ST7735-Library
Adafruit-ST7735-Library copied to clipboard
Published
20 hours ago •
adafruit
adafruit
Flicker issue is not smooth when blinking
I hope you fix the library to overcome flicker like this which is clearly not smooth.
https://github.com/user-attachments/assets/d29f143c-47bc-4c2c-843e-31013ca48fa7
#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <Adafruit_BMP280.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_INA219.h>
#include <SPI.h>
#include <Wire.h>
#include <LittleFS.h>
#include <TimeLib.h>
#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <DNSServer.h>
#include <ArduinoJson.h>
#include <Fonts/Swansea5pt7b.h>
#include <Fonts/Swansea6pt7b.h>
#include <Fonts/Open_24_Display_St17pt7b.h>
const char* ssid = "Time";
const char* password = "12345678";
#define TFT_MOSI 13
#define TFT_SCLK 14
#define TFT_DC 0
#define downButton 12
const int BATT_IMG_WIDTH = 22;
const int BATT_IMG_HEIGHT = 10;
const int ICON_WIDTH = 80;
const int ICON_HEIGHT = 80;
const int SMALL_ICON_WIDTH = 32;
const int SMALL_ICON_HEIGHT = 32;
const char* ICON_TEMP = "/temp_icon.bmp";
const char* ICON_PRESSURE = "/pressure_icon.bmp";
const char* ICON_ALTITUDE = "/altitude_icon.bmp";
const char* ICON_ANGLE = "/angle_icon.bmp";
const char* ICON_STEP = "/step_icon.bmp";
const char* ICON_SETTINGS = "/settings_icon.bmp";
const byte DNS_PORT = 53;
DNSServer dnsServer;
IPAddress apIP(172, 217, 28, 1);
ESP8266WebServer server(80);
Adafruit_ST7735 tft = Adafruit_ST7735(-1, TFT_DC, TFT_MOSI, TFT_SCLK, -1);
Adafruit_BMP280 bmp;
Adafruit_MPU6050 mpu;
Adafruit_INA219 ina219;
bool bmpEnabled = false;
bool mpuEnabled = false;
const float BATTERY_MAX = 4.2;
const float BATTERY_MIN = 3.0;
float temperature = 0, pressure = 0, altitude = 0;
float lastPressure = 0;
String tempC = "0.0 *C", tempF = "0.0 *F", pressureStr = "0.0 hPa", altitudeStr = "0.0 m";
String accX = "X: 0*", accY = "Y: 0*", accZ = "Z: 0*";
String batteryStatus = "100%";
String prevBatteryStatus = "";
String lastVoltageStr = "";
String prevTempC, prevTempF, prevPressureStr, prevAltitudeStr;
String prevAccX = "", prevAccY = "", prevAccZ = "";
String timeStr = "00:00";
String dateStr = "00/00/0000";
String prevTimeStr = "";
String prevDateStr = "";
String settingsText = "SETTING";
String prevSettingsText = "";
uint16_t backgroundColor = 0;
const uint16_t w = 80;
const uint16_t h = 160;
const int CONTENT_MARGIN = 9; // Margin untuk semua konten (left, right, top, bottom)
const int BORDER_WIDTH = 80; // Lebar border utama
const int BORDER_HEIGHT = 160; // Tinggi border utama
// Konstanta turunan untuk posisi border
const int BORDER_START_X = (tft.width() - BORDER_WIDTH) / 2;
const int BORDER_START_Y = (tft.height() - BORDER_HEIGHT) / 2;
unsigned long stepCount = 0;
String prevStepCount = "";
bool isFirstRun = true;
bool autoReturnEnabled = true;
bool bmpAvailable = false;
bool mpuAvailable = false;
bool ina219Available = false;
bool isInverted = true;
bool wifiInitialized = false;
volatile bool buttonPressed = false;
bool iconsDrawnTimeMenu = false;
int currentMenu = 1;
unsigned long lastButtonPress = 0;
const unsigned long AUTO_RETURN_TIMEOUT = 20000;
unsigned long buttonPressStartTime = 0;
const unsigned long LONG_PRESS_DURATION = 800;
const unsigned long DEBOUNCE_TIME = 0;
bool isButtonDown = false;
volatile bool longPressExecuted = false;
unsigned long sensorMillis = 0;
unsigned long stepMillis = 0;
unsigned long tempMillis = 0;
unsigned long pressureMillis = 0;
unsigned long altitudeMillis = 0;
const long sensorInterval = 50;
const long stepInterval = 1000;
const long tempInterval = 4000;
const long pressureInterval = 4000;
const long altitudeInterval = 4000;
const int WINDOW_SIZE = 10;
float accelWindow[WINDOW_SIZE];
int windowIndex = 0;
unsigned long lastStepTime = 0;
const unsigned long MIN_STEP_INTERVAL = 150;
const float VARIANCE_THRESHOLD = 0.5;
const float STEP_MAGNITUDE_THRESHOLD = 1.5;
const float PEAK_THRESHOLD = 2.0;
class KalmanFilter {
private:
float Q; // Process noise variance
float R; // Measurement noise variance
float P; // Estimation error variance
float X; // State estimate
float K; // Kalman gain
bool initialized;
public:
KalmanFilter(float processNoise = 0.001, float measurementNoise = 0.1) :
Q(processNoise),
R(measurementNoise),
P(1.0),
X(0),
K(0),
initialized(false) {}
float update(float measurement) {
if (!initialized) {
X = measurement;
initialized = true;
return X;
}
P = P + Q;
K = P / (P + R);
X = X + K * (measurement - X);
P = (1 - K) * P;
return X;
}
void reset() {
initialized = false;
P = 1.0;
X = 0;
K = 0;
}
};
// Then modify the KalmanFilter class declaration and other code before these functions
KalmanFilter tempKalman(0.001, 0.1);
KalmanFilter pressureKalman(0.01, 1.0);
KalmanFilter altitudeKalman(0.01, 2.0);
KalmanFilter accelXKalman(0.01, 0.5);
KalmanFilter accelYKalman(0.01, 0.5);
KalmanFilter accelZKalman(0.01, 0.5);
KalmanFilter gyroXKalman(0.01, 0.1);
KalmanFilter gyroYKalman(0.01, 0.1);
KalmanFilter gyroZKalman(0.01, 0.1);
struct FilteredAngles {
float roll; // X-axis rotation
float pitch; // Y-axis rotation
float yaw; // Z-axis rotation
};
float gyroAngleX = 0;
float gyroAngleY = 0;
float gyroAngleZ = 0;
unsigned long lastGyroRead = 0;
struct FilteredMPUData {
float x, y, z;
float roll, pitch, yaw; // Added these fields for angular data
};
float readFilteredTemperature() {
if (!bmpAvailable || !bmpEnabled) return 0.0;
float rawTemp = bmp.readTemperature();
return tempKalman.update(rawTemp);
}
float readFilteredPressure() {
if (!bmpAvailable || !bmpEnabled) return 0.0;
float rawPressure = bmp.readPressure() / 100.0F;
return pressureKalman.update(rawPressure);
}
float readFilteredAltitude() {
if (!bmpAvailable || !bmpEnabled) return 0.0;
float rawAltitude = bmp.readAltitude(1013.25);
return altitudeKalman.update(rawAltitude);
}
FilteredMPUData readFilteredAcceleration() {
FilteredMPUData filtered = {0, 0, 0, 0, 0, 0};
if (!mpuAvailable || !mpuEnabled) return filtered;
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
// Calculate time elapsed since last reading
unsigned long now = micros();
float dt = (now - lastGyroRead) / 1000000.0;
lastGyroRead = now;
// Filter accelerometer data
filtered.x = accelXKalman.update(a.acceleration.x);
filtered.y = accelYKalman.update(a.acceleration.y);
filtered.z = accelZKalman.update(a.acceleration.z);
// Filter gyroscope data
float gyroX = gyroXKalman.update(g.gyro.x);
float gyroY = gyroYKalman.update(g.gyro.y);
float gyroZ = gyroZKalman.update(g.gyro.z);
// Calculate pitch and roll from accelerometer
float pitch = atan2(-filtered.x, sqrt(filtered.y * filtered.y + filtered.z * filtered.z));
float roll = atan2(filtered.y, filtered.z);
// Convert to degrees
filtered.pitch = pitch * 180.0 / M_PI;
filtered.roll = roll * 180.0 / M_PI;
/*
// Calculate yaw using gyroscope with drift compensation
static float yawAngle = 0.0;
const float GYRO_THRESHOLD = 0.02; // Threshold untuk menghilangkan noise gyro
if (abs(gyroZ) > GYRO_THRESHOLD) {
yawAngle += gyroZ * dt;
// Normalize yaw angle to -180 to +180 degrees
while (yawAngle > 180) yawAngle -= 360;
while (yawAngle < -180) yawAngle += 360;
}
filtered.yaw = yawAngle;
*/
return filtered;
}
float calculateFilteredAngle(float ax, float ay, float az) {
static float filteredX = 0;
static float filteredY = 0;
static float filteredZ = 0;
// Complementary filter coefficient
const float ALPHA = 0.96;
// Calculate raw angles
float roll = atan2(ay, az) * 180.0 / M_PI;
float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;
float yaw = atan2(ax, ay) * 180.0 / M_PI;
// Apply complementary filter
filteredX = ALPHA * filteredX + (1.0 - ALPHA) * roll;
filteredY = ALPHA * filteredY + (1.0 - ALPHA) * pitch;
filteredZ = ALPHA * filteredZ + (1.0 - ALPHA) * yaw;
return filteredX; // Return filtered roll by default
}
void resetKalmanFilters() {
tempKalman.reset();
pressureKalman.reset();
altitudeKalman.reset();
accelXKalman.reset();
accelYKalman.reset();
accelZKalman.reset();
}
void enableBMP280() {
if (!bmpEnabled && bmpAvailable) {
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
Adafruit_BMP280::SAMPLING_X2,
Adafruit_BMP280::SAMPLING_X16,
Adafruit_BMP280::FILTER_X4,
Adafruit_BMP280::STANDBY_MS_4000);
bmpEnabled = true;
Serial.println("BMP280 enabled");
}
}
void disableBMP280() {
if (bmpEnabled && bmpAvailable) {
bmp.setSampling(Adafruit_BMP280::MODE_SLEEP,
Adafruit_BMP280::SAMPLING_NONE,
Adafruit_BMP280::SAMPLING_NONE,
Adafruit_BMP280::FILTER_OFF,
Adafruit_BMP280::STANDBY_MS_4000);
bmpEnabled = false;
Serial.println("BMP280 disabled");
}
}
void enableMPU6050() {
if (!mpuEnabled && mpuAvailable) {
mpu.enableSleep(false);
mpuEnabled = true;
Serial.println("MPU6050 enabled");
}
}
void disableMPU6050() {
if (mpuEnabled && mpuAvailable) {
mpu.enableSleep(true);
mpuEnabled = false;
Serial.println("MPU6050 disabled");
}
}
void handleRoot() {
File file = LittleFS.open("/index.html", "r");
if (!file) {
server.send(404, "text/plain", "File not found");
return;
}
server.streamFile(file, "text/html");
file.close();
}
void handleSetTime() {
if (server.hasArg("plain")) {
String json = server.arg("plain");
StaticJsonDocument<200> doc;
DeserializationError error = deserializeJson(doc, json);
if (error) {
server.send(400, "text/plain", "Invalid JSON");
return;
}
int year = doc["year"];
int month = doc["month"];
int day = doc["day"];
int hour = doc["hour"];
int minute = doc["minute"];
int second = doc["second"];
setTime(hour, minute, second, day, month, year);
server.send(200, "text/plain", "Time set successfully");
} else {
server.send(400, "text/plain", "No data received");
}
}
void setupWebServer() {
WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));
dnsServer.start(DNS_PORT, "*", apIP);
server.onNotFound([]() {
server.sendHeader("Location", String("http://") + apIP.toString(), true);
server.send(302, "text/plain", "");
});
server.on("/", HTTP_GET, handleRoot);
server.on("/settime", HTTP_POST, handleSetTime);
server.begin();
Serial.println("HTTP server started");
}
uint16_t safeColor565(int r, int g, int b) {
r = constrain(r, 0, 255);
g = constrain(g, 0, 255);
b = constrain(b, 0, 255);
uint8_t r5 = round(r * 31.0 / 255.0);
uint8_t g6 = round(g * 63.0 / 255.0);
uint8_t b5 = round(b * 31.0 / 255.0);
return (r5 << 11) | (g6 << 5) | b5;
}
void setBackgroundColor(int r, int g, int b) {
backgroundColor = safeColor565(r, g, b);
}
void clearScreen() {
tft.fillScreen(backgroundColor);
drawMargins();
}
void drawMargins() {
// Gambar margin kiri dan kanan dengan warna hitam
tft.fillRect(0, 0, CONTENT_MARGIN, tft.height(), safeColor565(0, 0, 0));
tft.fillRect(tft.width() - CONTENT_MARGIN, 0, CONTENT_MARGIN, tft.height(), safeColor565(0, 0, 0));
}
void drawBorder() {
drawMargins();
int borderStartX = (tft.width() - BORDER_WIDTH) / 2;
int borderStartY = (tft.height() - BORDER_HEIGHT) / 2;
tft.drawFastHLine(borderStartX, borderStartY, BORDER_WIDTH, safeColor565(0, 0, 0));
tft.drawFastHLine(borderStartX, borderStartY + BORDER_HEIGHT, BORDER_WIDTH, safeColor565(0, 0, 0));
tft.drawFastVLine(borderStartX, borderStartY, BORDER_HEIGHT, safeColor565(0, 0, 0));
tft.drawFastVLine(borderStartX + BORDER_WIDTH, borderStartY, BORDER_HEIGHT, safeColor565(0, 0, 0));
}
void clearBorderArea() {
// Hitung posisi battery icon
const int battX = BORDER_START_X + BORDER_WIDTH - BATT_IMG_WIDTH - CONTENT_MARGIN;
const int battY = BORDER_START_Y + CONTENT_MARGIN;
// Clear area diatas battery (jika ada)
tft.fillRect(BORDER_START_X + 1, BORDER_START_Y + 1,
BORDER_WIDTH - 2, battY - BORDER_START_Y - 1,
backgroundColor);
// Clear area kiri battery
tft.fillRect(BORDER_START_X + 1, battY,
battX - BORDER_START_X - 1, BATT_IMG_HEIGHT,
backgroundColor);
// Clear area kanan battery
tft.fillRect(battX + BATT_IMG_WIDTH + 1, battY,
BORDER_START_X + BORDER_WIDTH - (battX + BATT_IMG_WIDTH) - 1,
BATT_IMG_HEIGHT,
backgroundColor);
// Clear area dibawah battery
tft.fillRect(BORDER_START_X + 1, battY + BATT_IMG_HEIGHT,
BORDER_WIDTH - 2,
(BORDER_START_Y + BORDER_HEIGHT - 1) - (battY + BATT_IMG_HEIGHT),
backgroundColor);
}
int adjustContentY(int y) {
return BORDER_START_Y + y; // Sekarang relatif terhadap border
}
uint32_t read32(File &f) {
uint32_t result;
((uint8_t *)&result)[0] = f.read();
((uint8_t *)&result)[1] = f.read();
((uint8_t *)&result)[2] = f.read();
((uint8_t *)&result)[3] = f.read();
return result;
}
void drawBMP(const char *filename, int16_t x, int16_t y, int16_t targetWidth, int16_t targetHeight) {
File bmpFile = LittleFS.open(filename, "r");
if (!bmpFile) {
Serial.println("Failed to open BMP file");
return;
}
// Read BMP header
bmpFile.seek(0x0A);
uint32_t imageOffset = read32(bmpFile);
bmpFile.seek(0x12);
uint32_t imageWidth = read32(bmpFile);
uint32_t imageHeight = read32(bmpFile);
// Calculate scaling while preserving aspect ratio
float scaleX = (float)targetWidth / imageWidth;
float scaleY = (float)targetHeight / imageHeight;
float scale = min(scaleX, scaleY); // Use the smaller scaling factor
int16_t actualWidth = round(imageWidth * scale);
int16_t actualHeight = round(imageHeight * scale);
// Center the image within the target area
x += (targetWidth - actualWidth) / 2;
y += (targetHeight - actualHeight) / 2;
// Check if this is a small icon (32x32 or smaller)
bool isSmallIcon = (targetWidth <= 32 && targetHeight <= 32);
if (isSmallIcon) {
// Pre-calculate scaling ratios
float scaleX = (float)imageWidth / actualWidth;
float scaleY = (float)imageHeight / actualHeight;
// Create temporary buffer for a single line of pixels
uint8_t* lineBuffer = new uint8_t[imageWidth * 4];
for (int16_t targetY = 0; targetY < actualHeight; targetY++) {
// Calculate source Y position
float srcY = (actualHeight - 1 - targetY) * scaleY;
int srcYInt = (int)srcY;
// Read the full line of source pixels
bmpFile.seek(imageOffset + (srcYInt * imageWidth * 4));
bmpFile.read(lineBuffer, imageWidth * 4);
for (int16_t targetX = 0; targetX < actualWidth; targetX++) {
// Calculate source X position
float srcX = targetX * scaleX;
int srcXInt = (int)srcX;
// Get color values from buffer
uint8_t b = lineBuffer[srcXInt * 4];
uint8_t g = lineBuffer[srcXInt * 4 + 1];
uint8_t r = lineBuffer[srcXInt * 4 + 2];
uint8_t a = lineBuffer[srcXInt * 4 + 3];
if (a > 127) { // Only draw if pixel is visible
// For small icons, enhance contrast and sharpness
r = enhancePixel(r);
g = enhancePixel(g);
b = enhancePixel(b);
uint16_t color = safeColor565(r, g, b);
// Check boundaries before drawing
if (x + targetX >= BORDER_START_X &&
x + targetX < BORDER_START_X + BORDER_WIDTH &&
y + targetY >= BORDER_START_Y &&
y + targetY < BORDER_START_Y + BORDER_HEIGHT) {
tft.drawPixel(x + targetX, y + targetY, color);
// Apply edge smoothing
if (targetX > 0 && targetY > 0 &&
targetX < actualWidth-1 && targetY < actualHeight-1) {
if (isEdge(lineBuffer, srcXInt, imageWidth)) {
smoothEdge(tft, x + targetX, y + targetY, color);
}
}
}
}
}
}
delete[] lineBuffer;
} else {
// Original scaling method for larger images
for (int16_t row = 0; row < actualHeight; row++) {
int16_t sourceRow = imageHeight - 1 - (int16_t)((float)row * imageHeight / actualHeight);
bmpFile.seek(imageOffset + (sourceRow * imageWidth * 4));
for (int16_t col = 0; col < actualWidth; col++) {
int16_t sourceCol = (int16_t)((float)col * imageWidth / actualWidth);
bmpFile.seek(imageOffset + (sourceRow * imageWidth * 4) + (sourceCol * 4));
uint8_t b = bmpFile.read();
uint8_t g = bmpFile.read();
uint8_t r = bmpFile.read();
uint8_t a = bmpFile.read();
if (a > 127) {
// Check boundaries before drawing
if (x + col >= BORDER_START_X &&
x + col < BORDER_START_X + BORDER_WIDTH &&
y + row >= BORDER_START_Y &&
y + row < BORDER_START_Y + BORDER_HEIGHT) {
tft.drawPixel(x + col, y + row, safeColor565(r, g, b));
}
}
}
}
}
bmpFile.close();
}
uint8_t enhancePixel(uint8_t value) {
// Increase contrast for small icons
const float contrast = 1.2; // Contrast enhancement factor
const int brightness = 10; // Brightness adjustment
// Apply contrast
float adjusted = ((value / 255.0f - 0.5f) * contrast + 0.5f) * 255.0f;
// Apply brightness
adjusted += brightness;
// Ensure value stays in valid range
return (uint8_t)constrain(adjusted, 0, 255);
}
// Helper function to detect edges in the image
bool isEdge(uint8_t* buffer, int pos, int width) {
// Check if current pixel is significantly different from neighbors
uint8_t current = buffer[pos * 4 + 3]; // Alpha channel
uint8_t left = (pos > 0) ? buffer[(pos-1) * 4 + 3] : 0;
uint8_t right = (pos < width-1) ? buffer[(pos+1) * 4 + 3] : 0;
return (abs(current - left) > 127 || abs(current - right) > 127);
}
// Helper function to smooth edges
void smoothEdge(Adafruit_ST7735& tft, int16_t x, int16_t y, uint16_t color) {
// Get RGB components
uint8_t r = (color >> 11) << 3;
uint8_t g = ((color >> 5) & 0x3F) << 2;
uint8_t b = (color & 0x1F) << 3;
// Create slightly darker version for edge smoothing
uint16_t edgeColor = safeColor565(
r * 0.8,
g * 0.8,
b * 0.8
);
// Apply edge smoothing only if needed
if ((x + y) % 2 == 0) {
tft.drawPixel(x, y, edgeColor);
}
}
void drawMenuIcon(const char* iconPath, int y) {
int xCenter = BORDER_START_X + (BORDER_WIDTH - ICON_WIDTH) / 2;
y = BORDER_START_Y + y; // Hapus referensi ke MARGIN_TOP
if (LittleFS.exists(iconPath)) {
drawBMP(iconPath, xCenter, y, ICON_WIDTH, ICON_HEIGHT);
} else {
Serial.printf("Icon not found: %s\n", iconPath);
}
}
float getBatteryVoltage() {
if (!ina219Available) return 0.0;
float busVoltage = ina219.getBusVoltage_V();
float shuntVoltage = ina219.getShuntVoltage_mV() / 1000.0;
float voltage = busVoltage + shuntVoltage;
return (voltage < 2.0 || voltage > 5.0) ? 0.0 : voltage;
}
int getBatteryPercentage(float voltage) {
float percentage = ((voltage - BATTERY_MIN) / (BATTERY_MAX - BATTERY_MIN)) * 100;
return round(constrain(percentage, 0, 100));
}
void updateBatteryStatus() {
float voltage = getBatteryVoltage();
String currentVoltageStr = String(voltage, 2);
if (currentVoltageStr != lastVoltageStr) {
lastVoltageStr = currentVoltageStr;
int percentage = getBatteryPercentage(voltage);
if (percentage <= 3) {
batteryStatus = "0%";
} else if (percentage <= 20) {
batteryStatus = "20%";
} else if (percentage <= 50) {
batteryStatus = "50%";
} else {
batteryStatus = "100%";
}
if (batteryStatus != prevBatteryStatus) {
displayBatteryStatus();
}
}
}
void displayBatteryStatus() {
// Adjust battery position relative to border
const int battX = BORDER_START_X + BORDER_WIDTH - BATT_IMG_WIDTH - CONTENT_MARGIN;
const int battY = BORDER_START_Y + CONTENT_MARGIN;
if (batteryStatus != prevBatteryStatus) {
tft.fillRect(battX, battY, BATT_IMG_WIDTH, BATT_IMG_HEIGHT, backgroundColor);
const char* batteryImage = batteryStatus == "100%" ? "/battery_100.bmp" :
batteryStatus == "50%" ? "/battery_50.bmp" :
batteryStatus == "20%" ? "/battery_20.bmp" : "/battery_0.bmp";
drawBMP(batteryImage, battX, battY, BATT_IMG_WIDTH, BATT_IMG_HEIGHT);
prevBatteryStatus = batteryStatus;
}
}
void drawText(const String& text, int x, int y, uint16_t textColor, int paddingX = 5) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(text, x, y, &x1, &y1, &w, &h);
int actualX = x;
tft.fillRect(actualX - paddingX, y1, w + (paddingX * 2), h, backgroundColor);
tft.setTextColor(textColor);
tft.setCursor(actualX, y);
tft.print(text);
}
void drawCenteredText(const String& text, int y, uint16_t textColor, int paddingX = 5, int offsetX = 0) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(text, 0, 0, &x1, &y1, &w, &h);
int availableWidth = BORDER_WIDTH - (CONTENT_MARGIN * 2);
int x = BORDER_START_X + CONTENT_MARGIN + (availableWidth - w) / 2 + offsetX;
y = BORDER_START_Y + y; // Hapus referensi ke MARGIN_TOP
drawText(text, x, y, textColor, paddingX);
}
void drawDottedLine(int y, int length, int dotSpacing, uint16_t color) {
int borderStartX = (tft.width() - 80) / 2;
int xStart = borderStartX + (80 - length) / 2;
int xEnd = xStart + length;
y = adjustContentY(y);
for (int x = xStart; x <= xEnd; x += dotSpacing) {
tft.drawPixel(x, y, color);
}
}
void drawSmallIcon(const char* iconPath, int x, int y) {
// Debug info
Serial.printf("Drawing icon at x:%d y:%d with size %dx%d\n",
x, y, SMALL_ICON_WIDTH, SMALL_ICON_HEIGHT);
// Constrain position within border bounds
x = constrain(x, BORDER_START_X + CONTENT_MARGIN,
BORDER_START_X + BORDER_WIDTH - SMALL_ICON_WIDTH - CONTENT_MARGIN);
y = constrain(y, BORDER_START_Y + CONTENT_MARGIN,
BORDER_START_Y + BORDER_HEIGHT - SMALL_ICON_HEIGHT - CONTENT_MARGIN);
if (LittleFS.exists(iconPath)) {
drawBMP(iconPath, x, y, SMALL_ICON_WIDTH, SMALL_ICON_HEIGHT);
} else {
Serial.printf("Small icon not found: %s\n", iconPath);
}
}
void clearTextArea(int x, int y, const String& text) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(text, x, y, &x1, &y1, &w, &h);
tft.fillRect(x, y1, w + 2, h + 2, backgroundColor);
}
void updateTimeAndDate() {
String hourStr = (hour() < 10 ? "0" : "") + String(hour());
String minStr = (minute() < 10 ? "0" : "") + String(minute());
timeStr = hourStr + ":" + minStr;
String dayStr = (day() < 10 ? "0" : "") + String(day());
String monthStr = (month() < 10 ? "0" : "") + String(month());
String yearStr = String(year());
dateStr = dayStr + " / " + monthStr + " / " + yearStr;
}
void displayTimeAndDate() {
// Adjust base position relative to border
int yBase = BORDER_START_Y + 70;
int yStep = 10;
// Display time with direct centering
tft.setFont(&Open_24_Display_St17pt7b);
if (timeStr != prevTimeStr) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(timeStr, 0, 0, &x1, &y1, &w, &h);
// Calculate center position for time
int x = BORDER_START_X + (BORDER_WIDTH - w) / 2;
int y = yBase + yStep + 0;
// Clear previous time area
tft.fillRect(x + x1, y + y1, w, h, backgroundColor);
// Draw centered time
tft.setTextColor(safeColor565(255, 255, 255));
tft.setCursor(x, y);
tft.print(timeStr);
prevTimeStr = timeStr;
}
// Display date with direct centering
tft.setFont(&Swansea5pt7b);
if (dateStr != prevDateStr) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(dateStr, 0, 0, &x1, &y1, &w, &h);
// Calculate center position for date
int x = BORDER_START_X + (BORDER_WIDTH - w) / 2;
int y = yBase + yStep + 15;
// Clear previous date area
tft.fillRect(x + x1, y + y1, w, h, backgroundColor);
// Draw centered date
tft.setTextColor(safeColor565(221, 125, 64));
tft.setCursor(x, y);
tft.print(dateStr);
prevDateStr = dateStr;
}
if (currentMenu == 1) {
tft.setFont(&Swansea6pt7b);
// Calculate proper icon positions
int iconOffset = SMALL_ICON_HEIGHT / 2;
int tempIconY = yBase + yStep + 30;
int altIconY = yBase + yStep + 70;
if (!iconsDrawnTimeMenu) {
// Draw icons with adjusted positions and centering
drawSmallIcon(ICON_TEMP,
BORDER_START_X + CONTENT_MARGIN,
tempIconY - iconOffset);
drawSmallIcon(ICON_ALTITUDE,
BORDER_START_X + CONTENT_MARGIN,
altIconY - iconOffset);
iconsDrawnTimeMenu = true;
}
// Update temperature value
if (millis() - tempMillis >= tempInterval) {
tempMillis = millis();
if (bmpAvailable && bmpEnabled) {
temperature = readFilteredTemperature();
tempC = String(temperature, 1) + " *C";
}
}
if (tempC != prevTempC) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(tempC, 0, 0, &x1, &y1, &w, &h);
int textX = BORDER_START_X + BORDER_WIDTH - w - CONTENT_MARGIN;
int textY = yBase + yStep + 38;
tft.fillRect(textX, textY - h + y1, w, h, backgroundColor);
tft.setTextColor(safeColor565(255, 255, 255));
tft.setCursor(textX, textY);
tft.print(tempC);
prevTempC = tempC;
}
// Update altitude value
if (millis() - altitudeMillis >= altitudeInterval) {
altitudeMillis = millis();
if (bmpAvailable && bmpEnabled) {
altitude = bmp.readAltitude(1013.25);
altitudeStr = String(altitude, 1) + " m";
}
}
if (altitudeStr != prevAltitudeStr) {
int16_t x1, y1;
uint16_t w, h;
tft.getTextBounds(altitudeStr, 0, 0, &x1, &y1, &w, &h);
int textX = BORDER_START_X + BORDER_WIDTH - w - CONTENT_MARGIN;
int textY = yBase + yStep + 63;
tft.fillRect(textX, textY - h + y1, w, h, backgroundColor);
tft.setTextColor(safeColor565(24, 218, 61));
tft.setCursor(textX, textY);
tft.print(altitudeStr);
prevAltitudeStr = altitudeStr;
}
}
}
float calculateAngle(float ax, float ay, float az) {
// Ensure we don't divide by zero
if (ax == 0 && ay == 0 && az == 0) return 0;
// Calculate angles using arctan2 for better quadrant handling
// For X (Roll) - rotation around Y axis
float roll = atan2(ay, az) * 180.0 / M_PI;
// For Y (Pitch) - rotation around X axis
float pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;
// For Z (Yaw) - can't be accurately determined with just accelerometer
// Would need magnetometer for true heading
float yaw = atan2(ax, ay) * 180.0 / M_PI;
return roll; // Return roll by default, modify based on which angle you're calculating
}
float calculateMovingAverage() {
float sum = 0;
for (int i = 0; i < WINDOW_SIZE; i++) {
sum += accelWindow[i];
}
return sum / WINDOW_SIZE;
}
float calculateMovingVariance(float average) {
float variance = 0;
for (int i = 0; i < WINDOW_SIZE; i++) {
variance += pow(accelWindow[i] - average, 2);
}
return variance / WINDOW_SIZE;
}
void IRAM_ATTR ISR_downButton() {
static unsigned long lastInterruptTime = 0;
unsigned long interruptTime = millis();
if (digitalRead(downButton) == LOW) {
if (interruptTime - lastInterruptTime > DEBOUNCE_TIME) {
buttonPressStartTime = interruptTime;
isButtonDown = true;
longPressExecuted = false;
}
} else {
if (isButtonDown) {
unsigned long pressDuration = interruptTime - buttonPressStartTime;
if (pressDuration >= LONG_PRESS_DURATION && currentMenu == 6) {
stepCount = 0;
prevStepCount = "";
longPressExecuted = true;
} else if (pressDuration < LONG_PRESS_DURATION && !longPressExecuted) {
buttonPressed = true;
}
isButtonDown = false;
}
}
lastInterruptTime = interruptTime;
}
void setup() {
Serial.begin(115200);
Serial.println("Booting...");
ina219Available = ina219.begin();
if (!ina219Available) Serial.println("INA219 not found! Battery monitoring disabled");
if (!LittleFS.begin()) {
Serial.println("LittleFS initialization failed!");
return;
}
Dir dir = LittleFS.openDir("/");
Serial.println("Files in LittleFS:");
while (dir.next()) {
Serial.printf(" - %s (%d bytes)\n", dir.fileName().c_str(), dir.fileSize());
}
pinMode(downButton, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(downButton), ISR_downButton, CHANGE);
tft.initR(INITR_GREENTAB);
tft.setSPISpeed(40000000);
delay(10);
tft.invertDisplay(isInverted);
setBackgroundColor(0, 0, 0);
clearScreen();
bmpAvailable = bmp.begin(0x76);
if (!bmpAvailable) Serial.println("BMP280 not found! Values set to 0");
mpuAvailable = mpu.begin();
if (!mpuAvailable) Serial.println("MPU6050 not found! Values set to 0");
if (bmpAvailable) {
disableBMP280();
}
if (mpuAvailable) {
disableMPU6050();
}
setTime(0, 0, 0, 1, 1, 2000);
lastButtonPress = millis();
drawBorder();
updateBatteryStatus();
displayBatteryStatus();
}
void loop() {
updateBatteryStatus();
if (wifiInitialized) {
dnsServer.processNextRequest();
server.handleClient();
}
if (autoReturnEnabled && (millis() - lastButtonPress >= AUTO_RETURN_TIMEOUT)) {
currentMenu = 1;
clearBorderArea();
prevTimeStr = "";
prevDateStr = "";
prevTempC = "";
prevTempF = "";
prevPressureStr = "";
prevAltitudeStr = "";
prevAccX = "";
prevAccY = "";
prevAccZ = "";
prevSettingsText = "";
drawBorder();
lastButtonPress = millis();
disableMPU6050();
enableBMP280(); // Menu 1 needs BMP280 for temperature display
}
if (buttonPressed) {
buttonPressed = false;
lastButtonPress = millis();
resetKalmanFilters();
int nextMenu;
if (currentMenu == 1) {
nextMenu = 2; // Dari homescreen ke menu 2
} else {
nextMenu = (currentMenu % 7) + 1;
if (nextMenu == 1) { // Skip menu 1 dalam rotasi
nextMenu = 2;
}
}
if (nextMenu == 6 && currentMenu != 6) {
stepCount = 0;
prevStepCount = "";
isFirstRun = true;
}
disableBMP280();
disableMPU6050();
switch (nextMenu) {
case 1:
enableBMP280();
break;
case 2:
enableBMP280();
break;
case 3:
enableBMP280();
break;
case 4:
enableBMP280();
break;
case 5:
enableMPU6050();
break;
case 6:
enableMPU6050();
break;
}
currentMenu = nextMenu;
clearBorderArea();
prevTimeStr = "";
prevDateStr = "";
prevTempC = "";
prevTempF = "";
prevPressureStr = "";
prevAltitudeStr = "";
prevAccX = "";
prevAccY = "";
prevAccZ = "";
prevSettingsText = "";
iconsDrawnTimeMenu = false;
if (currentMenu == 2) {
tempMillis = 0;
}
if (currentMenu == 1) {
tempMillis = 0;
altitudeMillis = 0;
}
if (currentMenu != 6 && wifiInitialized) {
WiFi.softAPdisconnect(true);
server.close();
wifiInitialized = false;
Serial.println("WiFi AP stopped");
}
drawBorder();
}
updateTimeAndDate();
switch (currentMenu) {
case 1:
autoReturnEnabled = false;
displayTimeAndDate();
break;
case 2: {
autoReturnEnabled = true;
tft.setFont(&Swansea6pt7b);
drawMenuIcon(ICON_TEMP, CONTENT_MARGIN + 20);
if (millis() - tempMillis >= tempInterval) {
tempMillis = millis();
if (bmpAvailable && bmpEnabled) {
temperature = readFilteredTemperature();
tempC = String(temperature, 1) + " *C";
tempF = String((temperature * 9/5) + 32, 1) + " *F";
}
}
int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
int yStep = 30;
if (tempC != prevTempC) {
drawCenteredText(tempC, yBase + yStep + 0, safeColor565(255, 255, 255));
prevTempC = tempC;
}
drawDottedLine(yBase + yStep + 7, 55, 4, safeColor565(255, 255, 255));
if (tempF != prevTempF) {
drawCenteredText(tempF, yBase + yStep + 22, safeColor565(221, 125, 64));
prevTempF = tempF;
}
}
break;
case 3: {
autoReturnEnabled = true;
tft.setFont(&Swansea6pt7b);
drawMenuIcon(ICON_PRESSURE, CONTENT_MARGIN + 20);
if (millis() - pressureMillis >= pressureInterval) {
pressureMillis = millis();
if (bmpAvailable && bmpEnabled) {
pressure = readFilteredPressure();
lastPressure = pressure;
pressureStr = String(pressure, 1) + " hPa";
}
}
int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
int yStep = 30;
if (pressureStr != prevPressureStr) {
drawCenteredText(pressureStr, yBase + yStep + 0, safeColor565(255, 100, 100));
prevPressureStr = pressureStr;
}
}
break;
case 4: {
autoReturnEnabled = true;
tft.setFont(&Swansea6pt7b);
drawMenuIcon(ICON_ALTITUDE, CONTENT_MARGIN + 20);
if (millis() - altitudeMillis >= altitudeInterval) {
altitudeMillis = millis();
if (bmpAvailable && bmpEnabled) {
altitude = readFilteredAltitude();
altitudeStr = String(altitude, 1) + " m";
}
}
int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
int yStep = 30;
if (altitudeStr != prevAltitudeStr) {
drawCenteredText(altitudeStr, yBase + yStep + 0, safeColor565(24, 218, 61));
prevAltitudeStr = altitudeStr;
}
}
break;
case 5: {
autoReturnEnabled = true;
tft.setFont(&Swansea6pt7b);
drawMenuIcon(ICON_ANGLE, CONTENT_MARGIN + 20);
if (millis() - sensorMillis >= sensorInterval) {
sensorMillis = millis();
if (mpuAvailable && mpuEnabled) {
FilteredMPUData filtered = readFilteredAcceleration();
// Update display strings with angular data
accX = "X: " + String(filtered.roll, 1) + "*"; // Left-Right tilt
accY = "Y: " + String(filtered.pitch, 1) + "*"; // Forward-Backward tilt
/*
accZ = "Z: " + String(filtered.yaw, 1) + "*"; // Rotation
*/
}
}
int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
int yStep = 30;
if (accX != prevAccX) {
drawCenteredText(accX, yBase + yStep + 0, safeColor565(221, 67, 196));
prevAccX = accX;
}
if (accY != prevAccY) {
drawCenteredText(accY, yBase + yStep + 15, safeColor565(101, 111, 188));
prevAccY = accY;
}
/*
if (accZ != prevAccZ) {
drawCenteredText(accZ, yBase + yStep + 30, safeColor565(249, 218, 188));
prevAccZ = accZ;
}
*/
}
break;
case 6: {
autoReturnEnabled = false;
static const unsigned long MAX_STEPS = 99999999;
static KalmanFilter accelMagKalman(0.01, 0.1); // Process noise, measurement noise
static bool isKalmanInitialized = false;
if (isButtonDown && (millis() - buttonPressStartTime) >= LONG_PRESS_DURATION && currentMenu == 6) {
stepCount = 0;
prevStepCount = "";
isButtonDown = false;
buttonPressed = false;
isKalmanInitialized = false;
accelMagKalman.reset();
}
if (isFirstRun) {
for (int i = 0; i < WINDOW_SIZE; i++) {
accelWindow[i] = 0;
}
isFirstRun = false;
isKalmanInitialized = false;
accelMagKalman.reset();
}
tft.setFont(&Swansea6pt7b);
drawMenuIcon(ICON_STEP, CONTENT_MARGIN + 20);
if (mpuAvailable && mpuEnabled) {
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
// Calculate acceleration magnitude with gravity compensation
float rawMagnitude = sqrt(
a.acceleration.x * a.acceleration.x +
a.acceleration.y * a.acceleration.y +
(a.acceleration.z - 9.81) * (a.acceleration.z - 9.81)
);
// Apply Kalman filter to smooth the magnitude
float filteredMagnitude = accelMagKalman.update(rawMagnitude);
// Noise threshold with hysteresis
const float NOISE_THRESHOLD_HIGH = 0.9;
const float NOISE_THRESHOLD_LOW = 0.7;
static bool isAboveNoise = false;
if (filteredMagnitude > NOISE_THRESHOLD_HIGH) {
isAboveNoise = true;
} else if (filteredMagnitude < NOISE_THRESHOLD_LOW) {
isAboveNoise = false;
}
if (isAboveNoise) {
accelWindow[windowIndex] = filteredMagnitude;
} else {
accelWindow[windowIndex] = 0;
}
windowIndex = (windowIndex + 1) % WINDOW_SIZE;
float movingAvg = calculateMovingAverage();
float movingVar = calculateMovingVariance(movingAvg);
static bool isPeak = false;
static float peakValue = 0;
static float valleyValue = 0;
// Peak detection with enhanced conditions
if (filteredMagnitude > movingAvg && !isPeak &&
movingVar > VARIANCE_THRESHOLD &&
(millis() - lastStepTime) > MIN_STEP_INTERVAL &&
isAboveNoise) {
isPeak = true;
peakValue = filteredMagnitude;
}
else if (filteredMagnitude < movingAvg && isPeak) {
valleyValue = filteredMagnitude;
float stepMagnitude = peakValue - valleyValue;
// Enhanced step validation
if (stepMagnitude > STEP_MAGNITUDE_THRESHOLD &&
peakValue > PEAK_THRESHOLD) {
// Additional validation using variance
if (movingVar > VARIANCE_THRESHOLD * 1.2) {
stepCount++;
if (stepCount >= MAX_STEPS) {
stepCount = 0;
}
lastStepTime = millis();
}
}
isPeak = false;
}
}
String currentStepCount = String(stepCount);
int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
int yStep = 30;
if (currentStepCount != prevStepCount) {
drawCenteredText(currentStepCount, yBase + yStep + 0, safeColor565(255, 255, 255));
prevStepCount = currentStepCount;
}
}
break;
case 7: {
autoReturnEnabled = false;
if (!wifiInitialized) {
WiFi.mode(WIFI_AP);
WiFi.softAP(ssid, password);
setupWebServer();
wifiInitialized = true;
Serial.println("Access Point Started");
Serial.print("IP Address: ");
Serial.println(WiFi.softAPIP());
}
tft.setFont(&Swansea6pt7b);
drawMenuIcon(ICON_SETTINGS, CONTENT_MARGIN + 25);
if (settingsText != prevSettingsText) {
int yBase = CONTENT_MARGIN + ICON_HEIGHT + 0;
int yStep = 30;
drawCenteredText(settingsText, yBase + yStep + 0, safeColor565(255, 255, 255));
prevSettingsText = settingsText;
}
}
break;
}
}
