Upload files to "robot_v2_esp32c6"
This commit is contained in:
@@ -0,0 +1,314 @@
|
|||||||
|
\
|
||||||
|
/****************************************************
|
||||||
|
* ROBOT V2
|
||||||
|
* ESP32-C6 + NEO-M8N + MPU6050 + MMC5883MA + BMP180
|
||||||
|
* Sender data til Home Assistant via REST
|
||||||
|
****************************************************/
|
||||||
|
|
||||||
|
#include <WiFi.h>
|
||||||
|
#include <HTTPClient.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
#include <TinyGPSPlus.h>
|
||||||
|
#include <Adafruit_BMP085.h>
|
||||||
|
#include <MPU6050_tockn.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
// ---------------- WIFI / HA ----------------
|
||||||
|
const char* WIFI_SSID = "DIT_WIFI";
|
||||||
|
const char* WIFI_PASSWORD = "DIT_PASS";
|
||||||
|
|
||||||
|
const char* HA_HOST = "192.168.2.10";
|
||||||
|
const uint16_t HA_PORT = 8123;
|
||||||
|
const char* HA_TOKEN = "DIN_LONG_LIVED_ACCESS_TOKEN";
|
||||||
|
|
||||||
|
// ---------------- PINS ----------------
|
||||||
|
const int I2C_SDA_PIN = 6;
|
||||||
|
const int I2C_SCL_PIN = 7;
|
||||||
|
|
||||||
|
const int GPS_RX_PIN = 17; // ESP RX <- GPS TX
|
||||||
|
const int GPS_TX_PIN = 16; // ESP TX -> GPS RX
|
||||||
|
|
||||||
|
// ---------------- GPS ----------------
|
||||||
|
HardwareSerial GPSserial(1);
|
||||||
|
TinyGPSPlus gps;
|
||||||
|
|
||||||
|
// ---------------- BMP180 ----------------
|
||||||
|
Adafruit_BMP085 bmp;
|
||||||
|
|
||||||
|
// ---------------- MPU6050 ----------------
|
||||||
|
MPU6050 mpu6050(Wire);
|
||||||
|
|
||||||
|
// ---------------- MMC5883MA ----------------
|
||||||
|
const uint8_t MMC5883_ADDR = 0x30;
|
||||||
|
|
||||||
|
// MMC5883MA registers
|
||||||
|
const uint8_t MMC_REG_XOUT_L = 0x00;
|
||||||
|
const uint8_t MMC_REG_STATUS = 0x07;
|
||||||
|
const uint8_t MMC_REG_CTRL_0 = 0x08;
|
||||||
|
const uint8_t MMC_REG_CTRL_1 = 0x09;
|
||||||
|
|
||||||
|
// ---------------- Logic ----------------
|
||||||
|
bool isMoving = false;
|
||||||
|
bool gpsFix = false;
|
||||||
|
|
||||||
|
double acceptedLat = 0.0;
|
||||||
|
double acceptedLon = 0.0;
|
||||||
|
double acceptedSpeedKmh = 0.0;
|
||||||
|
double acceptedHdop = 99.9;
|
||||||
|
int acceptedSats = 0;
|
||||||
|
float headingDeg = 0.0;
|
||||||
|
unsigned long lastAcceptedFixMs = 0;
|
||||||
|
|
||||||
|
unsigned long lastImuMs = 0;
|
||||||
|
unsigned long lastGpsLogicMs = 0;
|
||||||
|
unsigned long lastPushMs = 0;
|
||||||
|
|
||||||
|
const float MOTION_THRESHOLD = 0.08f;
|
||||||
|
const unsigned long STILL_HOLD_MS = 3000;
|
||||||
|
unsigned long lastMotionDetectedMs = 0;
|
||||||
|
|
||||||
|
// ---------------- Utils ----------------
|
||||||
|
String haBaseUrl() {
|
||||||
|
return "http://" + String(HA_HOST) + ":" + String(HA_PORT);
|
||||||
|
}
|
||||||
|
|
||||||
|
void wifiConnect() {
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
|
||||||
|
|
||||||
|
unsigned long startMs = millis();
|
||||||
|
while (WiFi.status() != WL_CONNECTED) {
|
||||||
|
delay(500);
|
||||||
|
if (millis() - startMs > 30000) {
|
||||||
|
ESP.restart();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool postState(const String& entityId, const String& state, const String& unit = "", const String& devClass = "", const String& stateClass = "") {
|
||||||
|
WiFiClient client;
|
||||||
|
HTTPClient http;
|
||||||
|
String url = haBaseUrl() + "/api/states/" + entityId;
|
||||||
|
|
||||||
|
if (!http.begin(client, url)) return false;
|
||||||
|
|
||||||
|
http.addHeader("Authorization", "Bearer " + String(HA_TOKEN));
|
||||||
|
http.addHeader("Content-Type", "application/json");
|
||||||
|
|
||||||
|
String json = "{\"state\":\"" + state + "\",\"attributes\":{";
|
||||||
|
bool first = true;
|
||||||
|
|
||||||
|
if (unit.length()) {
|
||||||
|
json += "\"unit_of_measurement\":\"" + unit + "\"";
|
||||||
|
first = false;
|
||||||
|
}
|
||||||
|
if (devClass.length()) {
|
||||||
|
if (!first) json += ",";
|
||||||
|
json += "\"device_class\":\"" + devClass + "\"";
|
||||||
|
first = false;
|
||||||
|
}
|
||||||
|
if (stateClass.length()) {
|
||||||
|
if (!first) json += ",";
|
||||||
|
json += "\"state_class\":\"" + stateClass + "\"";
|
||||||
|
first = false;
|
||||||
|
}
|
||||||
|
if (!first) json += ",";
|
||||||
|
json += "\"friendly_name\":\"" + entityId + "\"";
|
||||||
|
json += "}}";
|
||||||
|
|
||||||
|
int code = http.POST(json);
|
||||||
|
http.end();
|
||||||
|
return (code >= 200 && code < 300);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ---------------- MMC5883MA helpers ----------------
|
||||||
|
void mmcWrite8(uint8_t reg, uint8_t val) {
|
||||||
|
Wire.beginTransmission(MMC5883_ADDR);
|
||||||
|
Wire.write(reg);
|
||||||
|
Wire.write(val);
|
||||||
|
Wire.endTransmission();
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t mmcRead8(uint8_t reg) {
|
||||||
|
Wire.beginTransmission(MMC5883_ADDR);
|
||||||
|
Wire.write(reg);
|
||||||
|
Wire.endTransmission(false);
|
||||||
|
Wire.requestFrom((int)MMC5883_ADDR, 1);
|
||||||
|
if (!Wire.available()) return 0;
|
||||||
|
return Wire.read();
|
||||||
|
}
|
||||||
|
|
||||||
|
void mmcReadXYZ(uint16_t &x, uint16_t &y, uint16_t &z) {
|
||||||
|
// Start measurement
|
||||||
|
mmcWrite8(MMC_REG_CTRL_0, 0x01); // TM_M
|
||||||
|
delay(10);
|
||||||
|
|
||||||
|
// Wait until measurement done
|
||||||
|
unsigned long start = millis();
|
||||||
|
while (!(mmcRead8(MMC_REG_STATUS) & 0x01)) {
|
||||||
|
if (millis() - start > 20) break;
|
||||||
|
delay(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
Wire.beginTransmission(MMC5883_ADDR);
|
||||||
|
Wire.write(MMC_REG_XOUT_L);
|
||||||
|
Wire.endTransmission(false);
|
||||||
|
Wire.requestFrom((int)MMC5883_ADDR, 6);
|
||||||
|
|
||||||
|
if (Wire.available() >= 6) {
|
||||||
|
uint8_t xl = Wire.read();
|
||||||
|
uint8_t xh = Wire.read();
|
||||||
|
uint8_t yl = Wire.read();
|
||||||
|
uint8_t yh = Wire.read();
|
||||||
|
uint8_t zl = Wire.read();
|
||||||
|
uint8_t zh = Wire.read();
|
||||||
|
|
||||||
|
x = ((uint16_t)xh << 8) | xl;
|
||||||
|
y = ((uint16_t)yh << 8) | yl;
|
||||||
|
z = ((uint16_t)zh << 8) | zl;
|
||||||
|
} else {
|
||||||
|
x = y = z = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void mmcInit() {
|
||||||
|
// Optional SET/RESET cycle
|
||||||
|
mmcWrite8(MMC_REG_CTRL_0, 0x08); // SET
|
||||||
|
delay(2);
|
||||||
|
mmcWrite8(MMC_REG_CTRL_0, 0x10); // RESET
|
||||||
|
delay(2);
|
||||||
|
|
||||||
|
// BW = 00 default 16-bit
|
||||||
|
mmcWrite8(MMC_REG_CTRL_1, 0x00);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Raw unsigned output is centered around 32768 counts
|
||||||
|
float mmcReadHeading() {
|
||||||
|
uint16_t rx, ry, rz;
|
||||||
|
mmcReadXYZ(rx, ry, rz);
|
||||||
|
|
||||||
|
float x = (float)rx - 32768.0f;
|
||||||
|
float y = (float)ry - 32768.0f;
|
||||||
|
|
||||||
|
float heading = atan2f(y, x) * 180.0f / PI;
|
||||||
|
if (heading < 0) heading += 360.0f;
|
||||||
|
return heading;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ---------------- Motion detect ----------------
|
||||||
|
void updateMotion() {
|
||||||
|
mpu6050.update();
|
||||||
|
|
||||||
|
float ax = mpu6050.getAccX();
|
||||||
|
float ay = mpu6050.getAccY();
|
||||||
|
float az = mpu6050.getAccZ();
|
||||||
|
|
||||||
|
float magnitude = sqrtf(ax * ax + ay * ay + az * az);
|
||||||
|
float deviation = fabsf(magnitude - 1.0f);
|
||||||
|
|
||||||
|
if (deviation > MOTION_THRESHOLD) {
|
||||||
|
lastMotionDetectedMs = millis();
|
||||||
|
isMoving = true;
|
||||||
|
} else {
|
||||||
|
if (millis() - lastMotionDetectedMs > STILL_HOLD_MS) {
|
||||||
|
isMoving = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ---------------- GPS logic ----------------
|
||||||
|
void processGPS() {
|
||||||
|
while (GPSserial.available()) {
|
||||||
|
gps.encode(GPSserial.read());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (millis() - lastGpsLogicMs < 1000) return;
|
||||||
|
lastGpsLogicMs = millis();
|
||||||
|
|
||||||
|
bool validLocation = gps.location.isValid();
|
||||||
|
int sats = gps.satellites.isValid() ? gps.satellites.value() : 0;
|
||||||
|
double hdop = gps.hdop.isValid() ? gps.hdop.hdop() : 99.9;
|
||||||
|
|
||||||
|
gpsFix = validLocation && sats >= 4 && hdop <= 5.0;
|
||||||
|
|
||||||
|
if (isMoving && gpsFix) {
|
||||||
|
acceptedLat = gps.location.lat();
|
||||||
|
acceptedLon = gps.location.lng();
|
||||||
|
acceptedSpeedKmh = gps.speed.isValid() ? gps.speed.kmph() : 0.0;
|
||||||
|
acceptedHdop = hdop;
|
||||||
|
acceptedSats = sats;
|
||||||
|
lastAcceptedFixMs = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (lastAcceptedFixMs == 0 && gpsFix) {
|
||||||
|
acceptedLat = gps.location.lat();
|
||||||
|
acceptedLon = gps.location.lng();
|
||||||
|
acceptedSpeedKmh = gps.speed.isValid() ? gps.speed.kmph() : 0.0;
|
||||||
|
acceptedHdop = hdop;
|
||||||
|
acceptedSats = sats;
|
||||||
|
lastAcceptedFixMs = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (isMoving) {
|
||||||
|
headingDeg = mmcReadHeading();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// ---------------- Push to HA ----------------
|
||||||
|
void pushToHA() {
|
||||||
|
unsigned long ageSec = (lastAcceptedFixMs == 0) ? 999999 : ((millis() - lastAcceptedFixMs) / 1000);
|
||||||
|
|
||||||
|
float tempC = bmp.readTemperature();
|
||||||
|
float pressureHpa = bmp.readPressure() / 100.0f;
|
||||||
|
|
||||||
|
postState("sensor.robot_gps_fix_raw", gpsFix ? "on" : "off");
|
||||||
|
postState("sensor.robot_motion_raw", isMoving ? "on" : "off");
|
||||||
|
|
||||||
|
postState("sensor.robot_gps_latitude", String(acceptedLat, 6));
|
||||||
|
postState("sensor.robot_gps_longitude", String(acceptedLon, 6));
|
||||||
|
postState("sensor.robot_gps_satellites", String(acceptedSats));
|
||||||
|
postState("sensor.robot_gps_hdop", String(acceptedHdop, 1));
|
||||||
|
postState("sensor.robot_gps_speed_kmh", String(acceptedSpeedKmh, 1), "km/h", "speed", "measurement");
|
||||||
|
postState("sensor.robot_heading_deg", String(headingDeg, 1), "°");
|
||||||
|
postState("sensor.robot_last_position_age", String(ageSec), "s", "duration", "measurement");
|
||||||
|
postState("sensor.robot_temperature", String(tempC, 1), "°C", "temperature", "measurement");
|
||||||
|
postState("sensor.robot_pressure", String(pressureHpa, 1), "hPa", "atmospheric_pressure", "measurement");
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
|
||||||
|
Wire.setClock(400000);
|
||||||
|
|
||||||
|
wifiConnect();
|
||||||
|
|
||||||
|
GPSserial.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
|
||||||
|
|
||||||
|
if (!bmp.begin()) {
|
||||||
|
// BMP180 ikke fundet - fortsæt alligevel
|
||||||
|
}
|
||||||
|
|
||||||
|
mpu6050.begin();
|
||||||
|
mpu6050.calcGyroOffsets(true);
|
||||||
|
|
||||||
|
mmcInit();
|
||||||
|
|
||||||
|
lastMotionDetectedMs = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
if (WiFi.status() != WL_CONNECTED) {
|
||||||
|
wifiConnect();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (millis() - lastImuMs >= 50) {
|
||||||
|
lastImuMs = millis();
|
||||||
|
updateMotion();
|
||||||
|
}
|
||||||
|
|
||||||
|
processGPS();
|
||||||
|
|
||||||
|
if (millis() - lastPushMs >= 5000) {
|
||||||
|
lastPushMs = millis();
|
||||||
|
pushToHA();
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user