mirror of https://github.com/meshcore-dev/MeshCore
LegoManACM
4 days ago
4 changed files with 291 additions and 10 deletions
@ -0,0 +1,137 @@ |
|||
#pragma once |
|||
#include <Arduino.h> |
|||
#include <math.h> |
|||
#include "target.h" |
|||
|
|||
#ifndef STATIONARY_SECS |
|||
#define STATIONARY_SECS 600 |
|||
#endif |
|||
#ifndef MOVE_THRESHOLD_M |
|||
#define MOVE_THRESHOLD_M 8 |
|||
#endif |
|||
#ifndef WATCHDOG_POLL_SECS |
|||
#define WATCHDOG_POLL_SECS 20 |
|||
#endif |
|||
// Don't poll at all until this many seconds after boot.
|
|||
// Gives sensors.begin() / NMEA parser time to fully initialise.
|
|||
#ifndef WATCHDOG_BOOT_DELAY_SECS |
|||
#define WATCHDOG_BOOT_DELAY_SECS 30 |
|||
#endif |
|||
|
|||
class MyMesh; |
|||
|
|||
class MobilityWatchdog { |
|||
public: |
|||
enum State { DRIVING, PARKED }; |
|||
|
|||
explicit MobilityWatchdog(MyMesh& mesh) |
|||
: _mesh(mesh), _state(DRIVING), |
|||
_lastPollS(0), _stationaryStartS(0), |
|||
_anchorLat(0.0), _anchorLon(0.0), |
|||
_gpsEverValid(false) {} |
|||
|
|||
void begin() { |
|||
char reply[64]; |
|||
_mesh.handleCommand(0, "set repeat off", reply); |
|||
Serial.println("[watchdog] driving mode — repeat off"); |
|||
} |
|||
|
|||
void loop() { |
|||
uint32_t now_s = millis() / 1000UL; |
|||
|
|||
// Wait for boot delay before doing anything
|
|||
if (now_s < (uint32_t)WATCHDOG_BOOT_DELAY_SECS) return; |
|||
|
|||
if ((now_s - _lastPollS) < (uint32_t)WATCHDOG_POLL_SECS) return; |
|||
_lastPollS = now_s; |
|||
|
|||
// Safely get the location provider
|
|||
LocationProvider* gps = sensors.getLocationProvider(); |
|||
if (!gps) return; |
|||
|
|||
// isValid() and coordinate reads are virtual calls — guard with
|
|||
// a local copy of the pointer to avoid any race with sensors.loop()
|
|||
bool valid = false; |
|||
long rawLat = 0, rawLon = 0; |
|||
|
|||
// Wrap in a simple validity check before reading coords
|
|||
valid = gps->isValid(); |
|||
if (!valid) { |
|||
if (!_gpsEverValid) { |
|||
Serial.println("[watchdog] no GPS fix yet"); |
|||
} |
|||
_stationaryStartS = 0; |
|||
return; |
|||
} |
|||
|
|||
rawLat = gps->getLatitude(); |
|||
rawLon = gps->getLongitude(); |
|||
|
|||
// Sanity check — discard obviously bad reads
|
|||
if (rawLat == 0 && rawLon == 0) return; |
|||
|
|||
_gpsEverValid = true; |
|||
|
|||
double lat = rawLat / 1000000.0; |
|||
double lon = rawLon / 1000000.0; |
|||
|
|||
if (_stationaryStartS == 0) { |
|||
_anchorLat = lat; |
|||
_anchorLon = lon; |
|||
_stationaryStartS = now_s; |
|||
Serial.printf("[watchdog] first fix %.6f, %.6f\n", lat, lon); |
|||
return; |
|||
} |
|||
|
|||
float distM = _haversineM(_anchorLat, _anchorLon, lat, lon); |
|||
|
|||
if (distM > (float)MOVE_THRESHOLD_M) { |
|||
if (_state == PARKED) { |
|||
Serial.printf("[watchdog] moving (%.1fm) — repeat off\n", distM); |
|||
char reply[64]; |
|||
_mesh.handleCommand(0, "advert", reply); |
|||
_mesh.handleCommand(0, "set repeat off", reply); |
|||
_state = DRIVING; |
|||
} |
|||
_anchorLat = lat; |
|||
_anchorLon = lon; |
|||
_stationaryStartS = now_s; |
|||
return; |
|||
} |
|||
|
|||
uint32_t stationaryS = now_s - _stationaryStartS; |
|||
if (_state == DRIVING && stationaryS >= (uint32_t)STATIONARY_SECS) { |
|||
Serial.println("[watchdog] parked — repeat on"); |
|||
char reply[64]; |
|||
_mesh.handleCommand(0, "set repeat on", reply); |
|||
_mesh.handleCommand(0, "advert", reply); |
|||
_state = PARKED; |
|||
} else if (_state == DRIVING) { |
|||
Serial.printf("[watchdog] stationary %lus/%us (%.1fm)\n", |
|||
(unsigned long)stationaryS, |
|||
(unsigned)STATIONARY_SECS, |
|||
distM); |
|||
} |
|||
} |
|||
|
|||
State getState() const { return _state; } |
|||
|
|||
private: |
|||
MyMesh& _mesh; |
|||
State _state; |
|||
uint32_t _lastPollS; |
|||
uint32_t _stationaryStartS; |
|||
double _anchorLat, _anchorLon; |
|||
bool _gpsEverValid; |
|||
|
|||
static float _haversineM(double lat1, double lon1, |
|||
double lat2, double lon2) { |
|||
const double R = 6371000.0; |
|||
double dLat = (lat2 - lat1) * M_PI / 180.0; |
|||
double dLon = (lon2 - lon1) * M_PI / 180.0; |
|||
double a = sin(dLat/2)*sin(dLat/2) |
|||
+ cos(lat1*M_PI/180.0)*cos(lat2*M_PI/180.0) |
|||
* sin(dLon/2)*sin(dLon/2); |
|||
return (float)(R * 2.0 * atan2(sqrt(a), sqrt(1.0 - a))); |
|||
} |
|||
}; |
|||
@ -0,0 +1,112 @@ |
|||
#include <Arduino.h> |
|||
#include <Mesh.h> |
|||
#include "../simple_repeater/MyMesh.h" |
|||
#include "MobilityWatchdog.h" |
|||
|
|||
#ifdef DISPLAY_CLASS |
|||
#include "UITask.h" |
|||
static UITask ui_task(display); |
|||
#endif |
|||
|
|||
StdRNG fast_rng; |
|||
SimpleMeshTables tables; |
|||
|
|||
MyMesh the_mesh(board, radio_driver, *new ArduinoMillis(), fast_rng, rtc_clock, tables); |
|||
static MobilityWatchdog watchdog(the_mesh); |
|||
|
|||
void halt() { |
|||
while (1) ; |
|||
} |
|||
|
|||
static char command[160]; |
|||
unsigned long lastActive = 0; |
|||
unsigned long nextSleepinSecs = 120; |
|||
|
|||
void setup() { |
|||
Serial.begin(115200); |
|||
|
|||
board.begin(); |
|||
|
|||
if (!radio_init()) { |
|||
halt(); |
|||
} |
|||
|
|||
fast_rng.begin(radio_get_rng_seed()); |
|||
|
|||
LittleFS.begin(); |
|||
FILESYSTEM* fs = &LittleFS; |
|||
IdentityStore store(LittleFS, "/identity"); |
|||
store.begin(); |
|||
|
|||
if (!store.load("_main", the_mesh.self_id)) { |
|||
the_mesh.self_id = radio_new_identity(); |
|||
int count = 0; |
|||
while (count < 10 && (the_mesh.self_id.pub_key[0] == 0x00 || the_mesh.self_id.pub_key[0] == 0xFF)) { |
|||
the_mesh.self_id = radio_new_identity(); count++; |
|||
} |
|||
store.save("_main", the_mesh.self_id); |
|||
} |
|||
|
|||
command[0] = 0; |
|||
lastActive = millis(); |
|||
|
|||
sensors.begin(); |
|||
the_mesh.begin(fs); |
|||
|
|||
#ifdef DISPLAY_CLASS |
|||
ui_task.begin(the_mesh.getNodePrefs(), FIRMWARE_BUILD_DATE, FIRMWARE_VERSION); |
|||
#endif |
|||
|
|||
watchdog.begin(); |
|||
|
|||
#if ENABLE_ADVERT_ON_BOOT == 1 |
|||
the_mesh.sendSelfAdvertisement(16000, false); |
|||
#endif |
|||
} |
|||
|
|||
void loop() { |
|||
// Serial CLI
|
|||
int len = strlen(command); |
|||
while (Serial.available() && len < sizeof(command)-1) { |
|||
char c = Serial.read(); |
|||
if (c != '\n') { |
|||
command[len++] = c; |
|||
command[len] = 0; |
|||
Serial.print(c); |
|||
} |
|||
if (c == '\r') break; |
|||
} |
|||
if (len == sizeof(command)-1) { |
|||
command[sizeof(command)-1] = '\r'; |
|||
} |
|||
if (len > 0 && command[len - 1] == '\r') { |
|||
Serial.print('\n'); |
|||
command[len - 1] = 0; |
|||
char reply[160]; |
|||
the_mesh.handleCommand(0, command, reply); |
|||
if (reply[0]) { |
|||
Serial.print(" -> "); Serial.println(reply); |
|||
} |
|||
command[0] = 0; |
|||
} |
|||
|
|||
the_mesh.loop(); |
|||
sensors.loop(); |
|||
watchdog.loop(); |
|||
|
|||
#ifdef DISPLAY_CLASS |
|||
ui_task.loop(); |
|||
#endif |
|||
|
|||
rtc_clock.tick(); |
|||
|
|||
if (the_mesh.getNodePrefs()->powersaving_enabled && !the_mesh.hasPendingWork()) { |
|||
if (the_mesh.millisHasNowPassed(lastActive + nextSleepinSecs * 1000)) { |
|||
board.sleep(1800); |
|||
lastActive = millis(); |
|||
nextSleepinSecs = 5; |
|||
} else { |
|||
nextSleepinSecs += 5; |
|||
} |
|||
} |
|||
} |
|||
Loading…
Reference in new issue