kevin/meshtastic_firmware
1
0
Fork 0
Code Issues Pull Requests Actions 5 Packages Projects Releases Wiki Activity

add a .clang-format file (#9154)

Browse Source
This commit is contained in:
Jorropo
2026-01-03 14:19:24 -06:00
committed by GitHub
co-authored by GitHub
parent abab6ce815
commit 0d11331d18
771 changed files with 77752 additions and 83184 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.clang-format
+2
View File
@@ -0,0 +1,2 @@
BasedOnStyle: LLVM
ColumnLimit: 150
.clusterfuzzlite/router_fuzzer.cpp
+129 -136
View File
@@ -16,8 +16,7 @@
#include "mesh/TypeConversions.h" #include "mesh/TypeConversions.h"
#include "mesh/mesh-pb-constants.h" #include "mesh/mesh-pb-constants.h"
namespace namespace {
{
constexpr uint32_t nodeId = 0x12345678; constexpr uint32_t nodeId = 0x12345678;
// Set to true when lateInitVariant finishes. Used to ensure lateInitVariant was called during startup. // Set to true when lateInitVariant finishes. Used to ensure lateInitVariant was called during startup.
bool hasBeenConfigured = false; bool hasBeenConfigured = false;
@@ -35,135 +34,130 @@ std::condition_variable loopCV;
std::thread meshtasticThread; std::thread meshtasticThread;
// This exception is thrown when the portuino main thread should exit. // This exception is thrown when the portuino main thread should exit.
class ShouldExitException : public std::runtime_error class ShouldExitException : public std::runtime_error {
{ public:
public: using std::runtime_error::runtime_error;
using std::runtime_error::runtime_error;
}; };
// Start the loop for one test case and wait till the loop has completed. This ensures fuzz // Start the loop for one test case and wait till the loop has completed. This ensures fuzz
// test cases do not overlap with one another. This helps the fuzzer attribute a crash to the // test cases do not overlap with one another. This helps the fuzzer attribute a crash to the
// single, currently running, test case. // single, currently running, test case.
void runLoopOnce() void runLoopOnce() {
{ realHardware = true; // Avoids delay(100) within portduino/main.cpp
realHardware = true; // Avoids delay(100) within portduino/main.cpp std::unique_lock<std::mutex> lck(loopLock);
std::unique_lock<std::mutex> lck(loopLock); fuzzerRunning = true;
fuzzerRunning = true; loopCanRun = true;
loopCanRun = true; loopCV.notify_one();
loopCV.notify_one(); loopCV.wait(lck, [] { return !loopCanRun && loopIsWaiting; });
loopCV.wait(lck, [] { return !loopCanRun && loopIsWaiting; });
} }
} // namespace } // namespace
// Called in the main Arduino loop function to determine if the loop can delay/sleep before running again. // Called in the main Arduino loop function to determine if the loop can delay/sleep before running again.
// We use this as a way to block the loop from sleeping and to start the loop function immediately when a // We use this as a way to block the loop from sleeping and to start the loop function immediately when a
// fuzzer input is ready. // fuzzer input is ready.
bool loopCanSleep() bool loopCanSleep() {
{ std::unique_lock<std::mutex> lck(loopLock);
std::unique_lock<std::mutex> lck(loopLock); loopIsWaiting = true;
loopIsWaiting = true; loopCV.notify_one();
loopCV.notify_one(); loopCV.wait(lck, [] { return loopCanRun || loopShouldExit; });
loopCV.wait(lck, [] { return loopCanRun || loopShouldExit; }); loopIsWaiting = false;
loopIsWaiting = false; if (loopShouldExit)
if (loopShouldExit) throw ShouldExitException("exit");
throw ShouldExitException("exit"); if (!fuzzerRunning)
if (!fuzzerRunning) return true; // The loop can sleep before the fuzzer starts.
return true; // The loop can sleep before the fuzzer starts. loopCanRun = false; // Only run the loop once before waiting again.
loopCanRun = false; // Only run the loop once before waiting again. return false;
return false;
} }
// Called just prior to starting Meshtastic. Allows for setting config values before startup. // Called just prior to starting Meshtastic. Allows for setting config values before startup.
void lateInitVariant() void lateInitVariant() {
{ portduino_config.logoutputlevel = level_error;
portduino_config.logoutputlevel = level_error; channelFile.channels[0] = meshtastic_Channel{
channelFile.channels[0] = meshtastic_Channel{ .has_settings = true,
.has_settings = true, .settings =
.settings = meshtastic_ChannelSettings{
meshtastic_ChannelSettings{ .psk = {.size = 1, .bytes = {/*defaultpskIndex=*/1}},
.psk = {.size = 1, .bytes = {/*defaultpskIndex=*/1}}, .name = "LongFast",
.name = "LongFast", .uplink_enabled = true,
.uplink_enabled = true, .has_module_settings = true,
.has_module_settings = true, .module_settings = {.position_precision = 16},
.module_settings = {.position_precision = 16}, },
}, .role = meshtastic_Channel_Role_PRIMARY,
.role = meshtastic_Channel_Role_PRIMARY, };
}; config.security.admin_key[0] = {
config.security.admin_key[0] = { .size = 32,
.size = 32, .bytes = {0xcd, 0xc0, 0xb4, 0x3c, 0x53, 0x24, 0xdf, 0x13, 0xca, 0x5a, 0xa6, 0x0c, 0x0d, 0xec, 0x85, 0x5a,
.bytes = {0xcd, 0xc0, 0xb4, 0x3c, 0x53, 0x24, 0xdf, 0x13, 0xca, 0x5a, 0xa6, 0x0c, 0x0d, 0xec, 0x85, 0x5a, 0x4c, 0xf6, 0x1a, 0x96, 0x04, 0x1a, 0x3e, 0xfc, 0xbb, 0x8e, 0x33, 0x71, 0xe5, 0xfc, 0xff, 0x3c},
0x4c, 0xf6, 0x1a, 0x96, 0x04, 0x1a, 0x3e, 0xfc, 0xbb, 0x8e, 0x33, 0x71, 0xe5, 0xfc, 0xff, 0x3c}, };
}; config.security.admin_key_count = 1;
config.security.admin_key_count = 1; config.lora.region = meshtastic_Config_LoRaConfig_RegionCode_US;
config.lora.region = meshtastic_Config_LoRaConfig_RegionCode_US; moduleConfig.has_mqtt = true;
moduleConfig.has_mqtt = true; moduleConfig.mqtt = meshtastic_ModuleConfig_MQTTConfig{
moduleConfig.mqtt = meshtastic_ModuleConfig_MQTTConfig{ .enabled = true,
.enabled = true, .proxy_to_client_enabled = true,
.proxy_to_client_enabled = true, };
}; moduleConfig.has_store_forward = true;
moduleConfig.has_store_forward = true; moduleConfig.store_forward = meshtastic_ModuleConfig_StoreForwardConfig{
moduleConfig.store_forward = meshtastic_ModuleConfig_StoreForwardConfig{ .enabled = true,
.enabled = true, .history_return_max = 4,
.history_return_max = 4, .history_return_window = 600,
.history_return_window = 600, .is_server = true,
.is_server = true, };
}; meshtastic_Position fixedGPS = meshtastic_Position{
meshtastic_Position fixedGPS = meshtastic_Position{ .has_latitude_i = true,
.has_latitude_i = true, .latitude_i = static_cast<uint32_t>(1 * 1e7),
.latitude_i = static_cast<uint32_t>(1 * 1e7), .has_longitude_i = true,
.has_longitude_i = true, .longitude_i = static_cast<uint32_t>(3 * 1e7),
.longitude_i = static_cast<uint32_t>(3 * 1e7), .has_altitude = true,
.has_altitude = true, .altitude = 64,
.altitude = 64, .location_source = meshtastic_Position_LocSource_LOC_MANUAL,
.location_source = meshtastic_Position_LocSource_LOC_MANUAL, };
}; nodeDB->setLocalPosition(fixedGPS);
nodeDB->setLocalPosition(fixedGPS); config.has_position = true;
config.has_position = true; config.position.fixed_position = true;
config.position.fixed_position = true; meshtastic_NodeInfoLite *info = nodeDB->getMeshNode(nodeDB->getNodeNum());
meshtastic_NodeInfoLite *info = nodeDB->getMeshNode(nodeDB->getNodeNum()); info->has_position = true;
info->has_position = true; info->position = TypeConversions::ConvertToPositionLite(fixedGPS);
info->position = TypeConversions::ConvertToPositionLite(fixedGPS); hasBeenConfigured = true;
hasBeenConfigured = true;
} }
extern "C" { extern "C" {
int portduino_main(int argc, char **argv); // Renamed "main" function from Meshtastic binary. int portduino_main(int argc, char **argv); // Renamed "main" function from Meshtastic binary.
// Start Meshtastic in a thread and wait till it has reached the ON state. // Start Meshtastic in a thread and wait till it has reached the ON state.
int LLVMFuzzerInitialize(int *argc, char ***argv) int LLVMFuzzerInitialize(int *argc, char ***argv) {
{ portduino_config.maxtophone = 5;
portduino_config.maxtophone = 5;
meshtasticThread = std::thread([program = *argv[0]]() { meshtasticThread = std::thread([program = *argv[0]]() {
char nodeIdStr[12]; char nodeIdStr[12];
strcpy(nodeIdStr, std::to_string(nodeId).c_str()); strcpy(nodeIdStr, std::to_string(nodeId).c_str());
int argc = 7; int argc = 7;
char *argv[] = {program, "-d", "/tmp/meshtastic", "-h", nodeIdStr, "-p", "0", nullptr}; char *argv[] = {program, "-d", "/tmp/meshtastic", "-h", nodeIdStr, "-p", "0", nullptr};
try { try {
portduino_main(argc, argv); portduino_main(argc, argv);
} catch (const ShouldExitException &) { } catch (const ShouldExitException &) {
}
});
std::atexit([] {
{
const std::lock_guard<std::mutex> lck(loopLock);
loopShouldExit = true;
loopCV.notify_one();
}
meshtasticThread.join();
});
// Wait for startup.
for (int i = 1; i < 20; ++i) {
if (powerFSM.getState() == &stateON) {
assert(hasBeenConfigured);
assert(router);
assert(nodeDB);
return 0;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
} }
return 1; });
std::atexit([] {
{
const std::lock_guard<std::mutex> lck(loopLock);
loopShouldExit = true;
loopCV.notify_one();
}
meshtasticThread.join();
});
// Wait for startup.
for (int i = 1; i < 20; ++i) {
if (powerFSM.getState() == &stateON) {
assert(hasBeenConfigured);
assert(router);
assert(nodeDB);
return 0;
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
return 1;
} }
// This is the main entrypoint for the fuzzer (the fuzz target). The fuzzer will provide an array of bytes to be // This is the main entrypoint for the fuzzer (the fuzz target). The fuzzer will provide an array of bytes to be
@@ -173,34 +167,33 @@ int LLVMFuzzerInitialize(int *argc, char ***argv)
// //
// This guide provides best practices for writing a fuzzer target. // This guide provides best practices for writing a fuzzer target.
// https://github.com/google/fuzzing/blob/master/docs/good-fuzz-target.md // https://github.com/google/fuzzing/blob/master/docs/good-fuzz-target.md
int LLVMFuzzerTestOneInput(const uint8_t *data, size_t length) int LLVMFuzzerTestOneInput(const uint8_t *data, size_t length) {
{ meshtastic_MeshPacket p = meshtastic_MeshPacket_init_default;
meshtastic_MeshPacket p = meshtastic_MeshPacket_init_default; pb_istream_t stream = pb_istream_from_buffer(data, length);
pb_istream_t stream = pb_istream_from_buffer(data, length); // Ignore any inputs that fail to decode or have fields set that are not transmitted over LoRa.
// Ignore any inputs that fail to decode or have fields set that are not transmitted over LoRa. if (!pb_decode(&stream, &meshtastic_MeshPacket_msg, &p) || p.rx_time || p.rx_snr || p.priority || p.rx_rssi || p.delayed || p.public_key.size ||
if (!pb_decode(&stream, &meshtastic_MeshPacket_msg, &p) || p.rx_time || p.rx_snr || p.priority || p.rx_rssi || p.delayed || p.next_hop || p.relay_node || p.tx_after)
p.public_key.size || p.next_hop || p.relay_node || p.tx_after) return -1; // Reject: The input will not be added to the corpus.
return -1; // Reject: The input will not be added to the corpus. if (p.which_payload_variant == meshtastic_MeshPacket_decoded_tag) {
if (p.which_payload_variant == meshtastic_MeshPacket_decoded_tag) { meshtastic_Data d;
meshtastic_Data d; stream = pb_istream_from_buffer(p.decoded.payload.bytes, p.decoded.payload.size);
stream = pb_istream_from_buffer(p.decoded.payload.bytes, p.decoded.payload.size); if (!pb_decode(&stream, &meshtastic_Data_msg, &d))
if (!pb_decode(&stream, &meshtastic_Data_msg, &d)) return -1; // Reject: The input will not be added to the corpus.
return -1; // Reject: The input will not be added to the corpus. }
}
// Provide default values for a few fields so the fuzzer doesn't need to guess them. // Provide default values for a few fields so the fuzzer doesn't need to guess them.
if (p.from == 0) if (p.from == 0)
p.from = nodeDB->getNodeNum(); p.from = nodeDB->getNodeNum();
if (p.to == 0) if (p.to == 0)
p.to = nodeDB->getNodeNum(); p.to = nodeDB->getNodeNum();
static uint32_t packetId = 0; static uint32_t packetId = 0;
if (p.id == 0) if (p.id == 0)
p.id == ++packetId; p.id == ++packetId;
if (p.pki_encrypted && config.security.admin_key_count) if (p.pki_encrypted && config.security.admin_key_count)
memcpy(&p.public_key, &config.security.admin_key[0], sizeof(p.public_key)); memcpy(&p.public_key, &config.security.admin_key[0], sizeof(p.public_key));
router->enqueueReceivedMessage(packetPool.allocCopy(p)); router->enqueueReceivedMessage(packetPool.allocCopy(p));
runLoopOnce(); runLoopOnce();
return 0; // Accept: The input may be added to the corpus. return 0; // Accept: The input may be added to the corpus.
} }
} }
src/AmbientLightingThread.h
+115 -124
View File
@@ -21,192 +21,183 @@ Adafruit_NeoPixel pixels(NEOPIXEL_COUNT, NEOPIXEL_DATA, NEOPIXEL_TYPE);
extern unPhone unphone; extern unPhone unphone;
#endif #endif
namespace concurrency namespace concurrency {
{ class AmbientLightingThread : public concurrency::OSThread {
class AmbientLightingThread : public concurrency::OSThread public:
{ explicit AmbientLightingThread(ScanI2C::DeviceType type) : OSThread("AmbientLighting") {
public: notifyDeepSleepObserver.observe(&notifyDeepSleep); // Let us know when shutdown() is issued.
explicit AmbientLightingThread(ScanI2C::DeviceType type) : OSThread("AmbientLighting")
{
notifyDeepSleepObserver.observe(&notifyDeepSleep); // Let us know when shutdown() is issued.
// Enables Ambient Lighting by default if conditions are meet. // Enables Ambient Lighting by default if conditions are meet.
#ifdef HAS_RGB_LED #ifdef HAS_RGB_LED
#ifdef ENABLE_AMBIENTLIGHTING #ifdef ENABLE_AMBIENTLIGHTING
moduleConfig.ambient_lighting.led_state = true; moduleConfig.ambient_lighting.led_state = true;
#endif #endif
#endif #endif
// Uncomment to test module // Uncomment to test module
// moduleConfig.ambient_lighting.led_state = true; // moduleConfig.ambient_lighting.led_state = true;
// moduleConfig.ambient_lighting.current = 10; // moduleConfig.ambient_lighting.current = 10;
// Default to a color based on our node number // Default to a color based on our node number
// moduleConfig.ambient_lighting.red = (myNodeInfo.my_node_num & 0xFF0000) >> 16; // moduleConfig.ambient_lighting.red = (myNodeInfo.my_node_num & 0xFF0000) >> 16;
// moduleConfig.ambient_lighting.green = (myNodeInfo.my_node_num & 0x00FF00) >> 8; // moduleConfig.ambient_lighting.green = (myNodeInfo.my_node_num & 0x00FF00) >> 8;
// moduleConfig.ambient_lighting.blue = myNodeInfo.my_node_num & 0x0000FF; // moduleConfig.ambient_lighting.blue = myNodeInfo.my_node_num & 0x0000FF;
#if defined(HAS_NCP5623) || defined(HAS_LP5562) #if defined(HAS_NCP5623) || defined(HAS_LP5562)
_type = type; _type = type;
if (_type == ScanI2C::DeviceType::NONE) { if (_type == ScanI2C::DeviceType::NONE) {
LOG_DEBUG("AmbientLighting Disable due to no RGB leds found on I2C bus"); LOG_DEBUG("AmbientLighting Disable due to no RGB leds found on I2C bus");
disable(); disable();
return; return;
} }
#endif #endif
#ifdef HAS_RGB_LED #ifdef HAS_RGB_LED
if (!moduleConfig.ambient_lighting.led_state) { if (!moduleConfig.ambient_lighting.led_state) {
LOG_DEBUG("AmbientLighting Disable due to moduleConfig.ambient_lighting.led_state OFF"); LOG_DEBUG("AmbientLighting Disable due to moduleConfig.ambient_lighting.led_state OFF");
disable(); disable();
return; return;
} }
LOG_DEBUG("AmbientLighting init"); LOG_DEBUG("AmbientLighting init");
#ifdef HAS_NCP5623 #ifdef HAS_NCP5623
if (_type == ScanI2C::NCP5623) { if (_type == ScanI2C::NCP5623) {
rgb.begin(); rgb.begin();
#endif #endif
#ifdef HAS_LP5562 #ifdef HAS_LP5562
if (_type == ScanI2C::LP5562) { if (_type == ScanI2C::LP5562) {
rgbw.begin(); rgbw.begin();
#endif #endif
#ifdef RGBLED_RED #ifdef RGBLED_RED
pinMode(RGBLED_RED, OUTPUT); pinMode(RGBLED_RED, OUTPUT);
pinMode(RGBLED_GREEN, OUTPUT); pinMode(RGBLED_GREEN, OUTPUT);
pinMode(RGBLED_BLUE, OUTPUT); pinMode(RGBLED_BLUE, OUTPUT);
#endif #endif
#ifdef HAS_NEOPIXEL #ifdef HAS_NEOPIXEL
pixels.begin(); // Initialise the pixel(s) pixels.begin(); // Initialise the pixel(s)
pixels.clear(); // Set all pixel colors to 'off' pixels.clear(); // Set all pixel colors to 'off'
pixels.setBrightness(moduleConfig.ambient_lighting.current); pixels.setBrightness(moduleConfig.ambient_lighting.current);
#endif #endif
setLighting(); setLighting();
#endif #endif
#if defined(HAS_NCP5623) || defined(HAS_LP5562) #if defined(HAS_NCP5623) || defined(HAS_LP5562)
} }
#endif #endif
} }
protected: protected:
int32_t runOnce() override int32_t runOnce() override {
{
#ifdef HAS_RGB_LED #ifdef HAS_RGB_LED
#if defined(HAS_NCP5623) || defined(HAS_LP5562) #if defined(HAS_NCP5623) || defined(HAS_LP5562)
if ((_type == ScanI2C::NCP5623 || _type == ScanI2C::LP5562) && moduleConfig.ambient_lighting.led_state) { if ((_type == ScanI2C::NCP5623 || _type == ScanI2C::LP5562) && moduleConfig.ambient_lighting.led_state) {
#endif #endif
setLighting(); setLighting();
return 30000; // 30 seconds to reset from any animations that may have been running from Ext. Notification return 30000; // 30 seconds to reset from any animations that may have been running from Ext. Notification
#if defined(HAS_NCP5623) || defined(HAS_LP5562) #if defined(HAS_NCP5623) || defined(HAS_LP5562)
} }
#endif #endif
#endif #endif
return disable(); return disable();
} }
// When shutdown() is issued, setLightingOff will be called. // When shutdown() is issued, setLightingOff will be called.
CallbackObserver<AmbientLightingThread, void *> notifyDeepSleepObserver = CallbackObserver<AmbientLightingThread, void *> notifyDeepSleepObserver =
CallbackObserver<AmbientLightingThread, void *>(this, &AmbientLightingThread::setLightingOff); CallbackObserver<AmbientLightingThread, void *>(this, &AmbientLightingThread::setLightingOff);
private: private:
ScanI2C::DeviceType _type = ScanI2C::DeviceType::NONE; ScanI2C::DeviceType _type = ScanI2C::DeviceType::NONE;
// Turn RGB lighting off, is used in junction to shutdown() // Turn RGB lighting off, is used in junction to shutdown()
int setLightingOff(void *unused) int setLightingOff(void *unused) {
{
#ifdef HAS_NCP5623 #ifdef HAS_NCP5623
rgb.setCurrent(0); rgb.setCurrent(0);
rgb.setRed(0); rgb.setRed(0);
rgb.setGreen(0); rgb.setGreen(0);
rgb.setBlue(0); rgb.setBlue(0);
LOG_INFO("OFF: NCP5623 Ambient lighting"); LOG_INFO("OFF: NCP5623 Ambient lighting");
#endif #endif
#ifdef HAS_LP5562 #ifdef HAS_LP5562
rgbw.setCurrent(0); rgbw.setCurrent(0);
rgbw.setRed(0); rgbw.setRed(0);
rgbw.setGreen(0); rgbw.setGreen(0);
rgbw.setBlue(0); rgbw.setBlue(0);
rgbw.setWhite(0); rgbw.setWhite(0);
LOG_INFO("OFF: LP5562 Ambient lighting"); LOG_INFO("OFF: LP5562 Ambient lighting");
#endif #endif
#ifdef HAS_NEOPIXEL #ifdef HAS_NEOPIXEL
pixels.clear(); pixels.clear();
pixels.show(); pixels.show();
LOG_INFO("OFF: NeoPixel Ambient lighting"); LOG_INFO("OFF: NeoPixel Ambient lighting");
#endif #endif
#ifdef RGBLED_CA #ifdef RGBLED_CA
analogWrite(RGBLED_RED, 255 - 0); analogWrite(RGBLED_RED, 255 - 0);
analogWrite(RGBLED_GREEN, 255 - 0); analogWrite(RGBLED_GREEN, 255 - 0);
analogWrite(RGBLED_BLUE, 255 - 0); analogWrite(RGBLED_BLUE, 255 - 0);
LOG_INFO("OFF: Ambient light RGB Common Anode"); LOG_INFO("OFF: Ambient light RGB Common Anode");
#elif defined(RGBLED_RED) #elif defined(RGBLED_RED)
analogWrite(RGBLED_RED, 0); analogWrite(RGBLED_RED, 0);
analogWrite(RGBLED_GREEN, 0); analogWrite(RGBLED_GREEN, 0);
analogWrite(RGBLED_BLUE, 0); analogWrite(RGBLED_BLUE, 0);
LOG_INFO("OFF: Ambient light RGB Common Cathode"); LOG_INFO("OFF: Ambient light RGB Common Cathode");
#endif #endif
#ifdef UNPHONE #ifdef UNPHONE
unphone.rgb(0, 0, 0); unphone.rgb(0, 0, 0);
LOG_INFO("OFF: unPhone Ambient lighting"); LOG_INFO("OFF: unPhone Ambient lighting");
#endif #endif
return 0; return 0;
} }
void setLighting() void setLighting() {
{
#ifdef HAS_NCP5623 #ifdef HAS_NCP5623
rgb.setCurrent(moduleConfig.ambient_lighting.current); rgb.setCurrent(moduleConfig.ambient_lighting.current);
rgb.setRed(moduleConfig.ambient_lighting.red); rgb.setRed(moduleConfig.ambient_lighting.red);
rgb.setGreen(moduleConfig.ambient_lighting.green); rgb.setGreen(moduleConfig.ambient_lighting.green);
rgb.setBlue(moduleConfig.ambient_lighting.blue); rgb.setBlue(moduleConfig.ambient_lighting.blue);
LOG_DEBUG("Init NCP5623 Ambient light w/ current=%d, red=%d, green=%d, blue=%d", LOG_DEBUG("Init NCP5623 Ambient light w/ current=%d, red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.current,
moduleConfig.ambient_lighting.current, moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif #endif
#ifdef HAS_LP5562 #ifdef HAS_LP5562
rgbw.setCurrent(moduleConfig.ambient_lighting.current); rgbw.setCurrent(moduleConfig.ambient_lighting.current);
rgbw.setRed(moduleConfig.ambient_lighting.red); rgbw.setRed(moduleConfig.ambient_lighting.red);
rgbw.setGreen(moduleConfig.ambient_lighting.green); rgbw.setGreen(moduleConfig.ambient_lighting.green);
rgbw.setBlue(moduleConfig.ambient_lighting.blue); rgbw.setBlue(moduleConfig.ambient_lighting.blue);
LOG_DEBUG("Init LP5562 Ambient light w/ current=%d, red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.current, LOG_DEBUG("Init LP5562 Ambient light w/ current=%d, red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.current,
moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue); moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif #endif
#ifdef HAS_NEOPIXEL #ifdef HAS_NEOPIXEL
pixels.fill(pixels.Color(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, pixels.fill(pixels.Color(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue), 0,
moduleConfig.ambient_lighting.blue), NEOPIXEL_COUNT);
0, NEOPIXEL_COUNT);
// RadioMaster Bandit has addressable LED at the two buttons // RadioMaster Bandit has addressable LED at the two buttons
// this allow us to set different lighting for them in variant.h file. // this allow us to set different lighting for them in variant.h file.
#ifdef RADIOMASTER_900_BANDIT #ifdef RADIOMASTER_900_BANDIT
#if defined(BUTTON1_COLOR) && defined(BUTTON1_COLOR_INDEX) #if defined(BUTTON1_COLOR) && defined(BUTTON1_COLOR_INDEX)
pixels.fill(BUTTON1_COLOR, BUTTON1_COLOR_INDEX, 1); pixels.fill(BUTTON1_COLOR, BUTTON1_COLOR_INDEX, 1);
#endif #endif
#if defined(BUTTON2_COLOR) && defined(BUTTON2_COLOR_INDEX) #if defined(BUTTON2_COLOR) && defined(BUTTON2_COLOR_INDEX)
pixels.fill(BUTTON2_COLOR, BUTTON2_COLOR_INDEX, 1); pixels.fill(BUTTON2_COLOR, BUTTON2_COLOR_INDEX, 1);
#endif #endif
#endif #endif
pixels.show(); pixels.show();
// LOG_DEBUG("Init NeoPixel Ambient light w/ brightness(current)=%d, red=%d, green=%d, blue=%d", // LOG_DEBUG("Init NeoPixel Ambient light w/ brightness(current)=%d, red=%d, green=%d, blue=%d",
// moduleConfig.ambient_lighting.current, moduleConfig.ambient_lighting.red, // moduleConfig.ambient_lighting.current, moduleConfig.ambient_lighting.red,
// moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue); // moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif #endif
#ifdef RGBLED_CA #ifdef RGBLED_CA
analogWrite(RGBLED_RED, 255 - moduleConfig.ambient_lighting.red); analogWrite(RGBLED_RED, 255 - moduleConfig.ambient_lighting.red);
analogWrite(RGBLED_GREEN, 255 - moduleConfig.ambient_lighting.green); analogWrite(RGBLED_GREEN, 255 - moduleConfig.ambient_lighting.green);
analogWrite(RGBLED_BLUE, 255 - moduleConfig.ambient_lighting.blue); analogWrite(RGBLED_BLUE, 255 - moduleConfig.ambient_lighting.blue);
LOG_DEBUG("Init Ambient light RGB Common Anode w/ red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.red, LOG_DEBUG("Init Ambient light RGB Common Anode w/ red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.red,
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue); moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#elif defined(RGBLED_RED) #elif defined(RGBLED_RED)
analogWrite(RGBLED_RED, moduleConfig.ambient_lighting.red); analogWrite(RGBLED_RED, moduleConfig.ambient_lighting.red);
analogWrite(RGBLED_GREEN, moduleConfig.ambient_lighting.green); analogWrite(RGBLED_GREEN, moduleConfig.ambient_lighting.green);
analogWrite(RGBLED_BLUE, moduleConfig.ambient_lighting.blue); analogWrite(RGBLED_BLUE, moduleConfig.ambient_lighting.blue);
LOG_DEBUG("Init Ambient light RGB Common Cathode w/ red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.red, LOG_DEBUG("Init Ambient light RGB Common Cathode w/ red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.red,
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue); moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif #endif
#ifdef UNPHONE #ifdef UNPHONE
unphone.rgb(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, unphone.rgb(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
moduleConfig.ambient_lighting.blue); LOG_DEBUG("Init unPhone Ambient light w/ red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green,
LOG_DEBUG("Init unPhone Ambient light w/ red=%d, green=%d, blue=%d", moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.blue);
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif #endif
} }
}; };
} // namespace concurrency } // namespace concurrency
src/AudioThread.h
+60 -67
View File
@@ -18,93 +18,86 @@ extern ExtensionIOXL9555 io;
#define AUDIO_THREAD_INTERVAL_MS 100 #define AUDIO_THREAD_INTERVAL_MS 100
class AudioThread : public concurrency::OSThread class AudioThread : public concurrency::OSThread {
{ public:
public: AudioThread() : OSThread("Audio") { initOutput(); }
AudioThread() : OSThread("Audio") { initOutput(); }
void beginRttl(const void *data, uint32_t len) void beginRttl(const void *data, uint32_t len) {
{
#ifdef T_LORA_PAGER #ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, HIGH); io.digitalWrite(EXPANDS_AMP_EN, HIGH);
#endif #endif
setCPUFast(true); setCPUFast(true);
rtttlFile = new AudioFileSourcePROGMEM(data, len); rtttlFile = new AudioFileSourcePROGMEM(data, len);
i2sRtttl = new AudioGeneratorRTTTL(); i2sRtttl = new AudioGeneratorRTTTL();
i2sRtttl->begin(rtttlFile, audioOut); i2sRtttl->begin(rtttlFile, audioOut);
}
// Also handles actually playing the RTTTL, needs to be called in loop
bool isPlaying() {
if (i2sRtttl != nullptr) {
return i2sRtttl->isRunning() && i2sRtttl->loop();
}
return false;
}
void stop() {
if (i2sRtttl != nullptr) {
i2sRtttl->stop();
delete i2sRtttl;
i2sRtttl = nullptr;
} }
// Also handles actually playing the RTTTL, needs to be called in loop if (rtttlFile != nullptr) {
bool isPlaying() delete rtttlFile;
{ rtttlFile = nullptr;
if (i2sRtttl != nullptr) {
return i2sRtttl->isRunning() && i2sRtttl->loop();
}
return false;
} }
void stop() setCPUFast(false);
{
if (i2sRtttl != nullptr) {
i2sRtttl->stop();
delete i2sRtttl;
i2sRtttl = nullptr;
}
if (rtttlFile != nullptr) {
delete rtttlFile;
rtttlFile = nullptr;
}
setCPUFast(false);
#ifdef T_LORA_PAGER #ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, LOW); io.digitalWrite(EXPANDS_AMP_EN, LOW);
#endif #endif
} }
void readAloud(const char *text) void readAloud(const char *text) {
{ if (i2sRtttl != nullptr) {
if (i2sRtttl != nullptr) { i2sRtttl->stop();
i2sRtttl->stop(); delete i2sRtttl;
delete i2sRtttl; i2sRtttl = nullptr;
i2sRtttl = nullptr; }
}
#ifdef T_LORA_PAGER #ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, HIGH); io.digitalWrite(EXPANDS_AMP_EN, HIGH);
#endif #endif
ESP8266SAM *sam = new ESP8266SAM; ESP8266SAM *sam = new ESP8266SAM;
sam->Say(audioOut, text); sam->Say(audioOut, text);
delete sam; delete sam;
setCPUFast(false); setCPUFast(false);
#ifdef T_LORA_PAGER #ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, LOW); io.digitalWrite(EXPANDS_AMP_EN, LOW);
#endif #endif
} }
protected: protected:
int32_t runOnce() override int32_t runOnce() override {
{ canSleep = true; // Assume we should not keep the board awake
canSleep = true; // Assume we should not keep the board awake
// if (i2sRtttl != nullptr && i2sRtttl->isRunning()) { // if (i2sRtttl != nullptr && i2sRtttl->isRunning()) {
// i2sRtttl->loop(); // i2sRtttl->loop();
// } // }
return AUDIO_THREAD_INTERVAL_MS; return AUDIO_THREAD_INTERVAL_MS;
} }
private: private:
void initOutput() void initOutput() {
{ audioOut = new AudioOutputI2S(1, AudioOutputI2S::EXTERNAL_I2S);
audioOut = new AudioOutputI2S(1, AudioOutputI2S::EXTERNAL_I2S); audioOut->SetPinout(DAC_I2S_BCK, DAC_I2S_WS, DAC_I2S_DOUT, DAC_I2S_MCLK);
audioOut->SetPinout(DAC_I2S_BCK, DAC_I2S_WS, DAC_I2S_DOUT, DAC_I2S_MCLK); audioOut->SetGain(0.2);
audioOut->SetGain(0.2); };
};
AudioGeneratorRTTTL *i2sRtttl = nullptr; AudioGeneratorRTTTL *i2sRtttl = nullptr;
AudioOutputI2S *audioOut = nullptr; AudioOutputI2S *audioOut = nullptr;
AudioFileSourcePROGMEM *rtttlFile = nullptr; AudioFileSourcePROGMEM *rtttlFile = nullptr;
}; };
#endif #endif
src/BluetoothCommon.cpp
+6 -12
View File
@@ -3,15 +3,9 @@
// NRF52 wants these constants as byte arrays // NRF52 wants these constants as byte arrays
// Generated here https://yupana-engineering.com/online-uuid-to-c-array-converter - but in REVERSE BYTE ORDER // Generated here https://yupana-engineering.com/online-uuid-to-c-array-converter - but in REVERSE BYTE ORDER
const uint8_t MESH_SERVICE_UUID_16[16u] = {0xfd, 0xea, 0x73, 0xe2, 0xca, 0x5d, 0xa8, 0x9f, const uint8_t MESH_SERVICE_UUID_16[16u] = {0xfd, 0xea, 0x73, 0xe2, 0xca, 0x5d, 0xa8, 0x9f, 0x1f, 0x46, 0xa8, 0x15, 0x18, 0xb2, 0xa1, 0x6b};
0x1f, 0x46, 0xa8, 0x15, 0x18, 0xb2, 0xa1, 0x6b}; const uint8_t TORADIO_UUID_16[16u] = {0xe7, 0x01, 0x44, 0x12, 0x66, 0x78, 0xdd, 0xa1, 0xad, 0x4d, 0x9e, 0x12, 0xd2, 0x76, 0x5c, 0xf7};
const uint8_t TORADIO_UUID_16[16u] = {0xe7, 0x01, 0x44, 0x12, 0x66, 0x78, 0xdd, 0xa1, const uint8_t FROMRADIO_UUID_16[16u] = {0x02, 0x00, 0x12, 0xac, 0x42, 0x02, 0x78, 0xb8, 0xed, 0x11, 0x93, 0x49, 0x9e, 0xe6, 0x55, 0x2c};
0xad, 0x4d, 0x9e, 0x12, 0xd2, 0x76, 0x5c, 0xf7}; const uint8_t FROMNUM_UUID_16[16u] = {0x53, 0x44, 0xe3, 0x47, 0x75, 0xaa, 0x70, 0xa6, 0x66, 0x4f, 0x00, 0xa8, 0x8c, 0xa1, 0x9d, 0xed};
const uint8_t FROMRADIO_UUID_16[16u] = {0x02, 0x00, 0x12, 0xac, 0x42, 0x02, 0x78, 0xb8, const uint8_t LEGACY_LOGRADIO_UUID_16[16u] = {0xe2, 0xf2, 0x1e, 0xbe, 0xc5, 0x15, 0xcf, 0xaa, 0x6b, 0x43, 0xfa, 0x78, 0x38, 0xd2, 0x6f, 0x6c};
0xed, 0x11, 0x93, 0x49, 0x9e, 0xe6, 0x55, 0x2c}; const uint8_t LOGRADIO_UUID_16[16u] = {0x47, 0x95, 0xDF, 0x8C, 0xDE, 0xE9, 0x44, 0x99, 0x23, 0x44, 0xE6, 0x06, 0x49, 0x6E, 0x3D, 0x5A};
const uint8_t FROMNUM_UUID_16[16u] = {0x53, 0x44, 0xe3, 0x47, 0x75, 0xaa, 0x70, 0xa6,
0x66, 0x4f, 0x00, 0xa8, 0x8c, 0xa1, 0x9d, 0xed};
const uint8_t LEGACY_LOGRADIO_UUID_16[16u] = {0xe2, 0xf2, 0x1e, 0xbe, 0xc5, 0x15, 0xcf, 0xaa,
0x6b, 0x43, 0xfa, 0x78, 0x38, 0xd2, 0x6f, 0x6c};
const uint8_t LOGRADIO_UUID_16[16u] = {0x47, 0x95, 0xDF, 0x8C, 0xDE, 0xE9, 0x44, 0x99,
0x23, 0x44, 0xE6, 0x06, 0x49, 0x6E, 0x3D, 0x5A};
src/BluetoothCommon.h
+7 -8
View File
@@ -21,12 +21,11 @@ extern const uint8_t MESH_SERVICE_UUID_16[], TORADIO_UUID_16[16u], FROMRADIO_UUI
/// Given a level between 0-100, update the BLE attribute /// Given a level between 0-100, update the BLE attribute
void updateBatteryLevel(uint8_t level); void updateBatteryLevel(uint8_t level);
class BluetoothApi class BluetoothApi {
{ public:
public: virtual void setup();
virtual void setup(); virtual void shutdown();
virtual void shutdown(); virtual void clearBonds();
virtual void clearBonds(); virtual bool isConnected();
virtual bool isConnected(); virtual int getRssi() = 0;
virtual int getRssi() = 0;
}; };
src/BluetoothStatus.h
+76 -83
View File
@@ -5,113 +5,106 @@
#include "meshUtils.h" #include "meshUtils.h"
#include <Arduino.h> #include <Arduino.h>
namespace meshtastic namespace meshtastic {
{
// Describes the state of the Bluetooth connection // Describes the state of the Bluetooth connection
// Allows display to handle pairing events without each UI needing to explicitly hook the Bluefruit / NimBLE code // Allows display to handle pairing events without each UI needing to explicitly hook the Bluefruit / NimBLE code
class BluetoothStatus : public Status class BluetoothStatus : public Status {
{ public:
public: enum class ConnectionState {
enum class ConnectionState { DISCONNECTED,
DISCONNECTED, PAIRING,
PAIRING, CONNECTED,
CONNECTED, };
};
private: private:
CallbackObserver<BluetoothStatus, const BluetoothStatus *> statusObserver = CallbackObserver<BluetoothStatus, const BluetoothStatus *> statusObserver =
CallbackObserver<BluetoothStatus, const BluetoothStatus *>(this, &BluetoothStatus::updateStatus); CallbackObserver<BluetoothStatus, const BluetoothStatus *>(this, &BluetoothStatus::updateStatus);
ConnectionState state = ConnectionState::DISCONNECTED; ConnectionState state = ConnectionState::DISCONNECTED;
std::string passkey; // Stored as string, because Bluefruit allows passkeys with a leading zero std::string passkey; // Stored as string, because Bluefruit allows passkeys with a leading zero
public: public:
BluetoothStatus() { statusType = STATUS_TYPE_BLUETOOTH; } BluetoothStatus() { statusType = STATUS_TYPE_BLUETOOTH; }
// New BluetoothStatus: connected or disconnected // New BluetoothStatus: connected or disconnected
explicit BluetoothStatus(ConnectionState state) explicit BluetoothStatus(ConnectionState state) {
{ assert(state != ConnectionState::PAIRING); // If pairing, use constructor which specifies passkey
assert(state != ConnectionState::PAIRING); // If pairing, use constructor which specifies passkey statusType = STATUS_TYPE_BLUETOOTH;
statusType = STATUS_TYPE_BLUETOOTH; this->state = state;
this->state = state; }
// New BluetoothStatus: pairing, with passkey
explicit BluetoothStatus(const std::string &passkey) : Status() {
statusType = STATUS_TYPE_BLUETOOTH;
this->state = ConnectionState::PAIRING;
this->passkey = passkey;
}
ConnectionState getConnectionState() const { return this->state; }
std::string getPasskey() const {
assert(state == ConnectionState::PAIRING);
return this->passkey;
}
void observe(Observable<const BluetoothStatus *> *source) { statusObserver.observe(source); }
bool matches(const BluetoothStatus *newStatus) const {
if (this->state == newStatus->getConnectionState()) {
// Same state: CONNECTED / DISCONNECTED
if (this->state != ConnectionState::PAIRING)
return true;
// Same state: PAIRING, and passkey matches
else if (this->getPasskey() == newStatus->getPasskey())
return true;
} }
// New BluetoothStatus: pairing, with passkey return false;
explicit BluetoothStatus(const std::string &passkey) : Status() }
{
statusType = STATUS_TYPE_BLUETOOTH;
this->state = ConnectionState::PAIRING;
this->passkey = passkey;
}
ConnectionState getConnectionState() const { return this->state; } int updateStatus(const BluetoothStatus *newStatus) {
// Has the status changed?
if (!matches(newStatus)) {
// Copy the members
state = newStatus->getConnectionState();
if (state == ConnectionState::PAIRING)
passkey = newStatus->getPasskey();
std::string getPasskey() const // Tell anyone interested that we have an update
{ onNewStatus.notifyObservers(this);
assert(state == ConnectionState::PAIRING);
return this->passkey;
}
void observe(Observable<const BluetoothStatus *> *source) { statusObserver.observe(source); } // Debug only:
switch (state) {
bool matches(const BluetoothStatus *newStatus) const case ConnectionState::PAIRING:
{ LOG_DEBUG("BluetoothStatus PAIRING, key=%s", passkey.c_str());
if (this->state == newStatus->getConnectionState()) { break;
// Same state: CONNECTED / DISCONNECTED case ConnectionState::CONNECTED:
if (this->state != ConnectionState::PAIRING) LOG_DEBUG("BluetoothStatus CONNECTED");
return true;
// Same state: PAIRING, and passkey matches
else if (this->getPasskey() == newStatus->getPasskey())
return true;
}
return false;
}
int updateStatus(const BluetoothStatus *newStatus)
{
// Has the status changed?
if (!matches(newStatus)) {
// Copy the members
state = newStatus->getConnectionState();
if (state == ConnectionState::PAIRING)
passkey = newStatus->getPasskey();
// Tell anyone interested that we have an update
onNewStatus.notifyObservers(this);
// Debug only:
switch (state) {
case ConnectionState::PAIRING:
LOG_DEBUG("BluetoothStatus PAIRING, key=%s", passkey.c_str());
break;
case ConnectionState::CONNECTED:
LOG_DEBUG("BluetoothStatus CONNECTED");
#ifdef BLE_LED #ifdef BLE_LED
#ifdef BLE_LED_INVERTED #ifdef BLE_LED_INVERTED
digitalWrite(BLE_LED, LOW); digitalWrite(BLE_LED, LOW);
#else #else
digitalWrite(BLE_LED, HIGH); digitalWrite(BLE_LED, HIGH);
#endif #endif
#endif #endif
break; break;
case ConnectionState::DISCONNECTED: case ConnectionState::DISCONNECTED:
LOG_DEBUG("BluetoothStatus DISCONNECTED"); LOG_DEBUG("BluetoothStatus DISCONNECTED");
#ifdef BLE_LED #ifdef BLE_LED
#ifdef BLE_LED_INVERTED #ifdef BLE_LED_INVERTED
digitalWrite(BLE_LED, HIGH); digitalWrite(BLE_LED, HIGH);
#else #else
digitalWrite(BLE_LED, LOW); digitalWrite(BLE_LED, LOW);
#endif #endif
#endif #endif
break; break;
} }
}
return 0;
} }
return 0;
}
}; };
} // namespace meshtastic } // namespace meshtastic
src/DebugConfiguration.cpp
+108 -126
View File
@@ -31,168 +31,150 @@ SOFTWARE.*/
#endif #endif
/// A C wrapper for LOG_DEBUG that can be used from arduino C libs that don't know about C++ or meshtastic /// A C wrapper for LOG_DEBUG that can be used from arduino C libs that don't know about C++ or meshtastic
extern "C" void logLegacy(const char *level, const char *fmt, ...) extern "C" void logLegacy(const char *level, const char *fmt, ...) {
{ va_list args;
va_list args; va_start(args, fmt);
va_start(args, fmt); if (console)
if (console) console->vprintf(level, fmt, args);
console->vprintf(level, fmt, args); va_end(args);
va_end(args);
} }
#if HAS_NETWORKING #if HAS_NETWORKING
Syslog::Syslog(UDP &client) Syslog::Syslog(UDP &client) {
{ this->_client = &client;
this->_client = &client; this->_server = NULL;
this->_port = 0;
this->_deviceHostname = SYSLOG_NILVALUE;
this->_appName = SYSLOG_NILVALUE;
this->_priDefault = LOGLEVEL_KERN;
}
Syslog &Syslog::server(const char *server, uint16_t port) {
if (this->_ip.fromString(server)) {
this->_server = NULL; this->_server = NULL;
this->_port = 0; } else {
this->_deviceHostname = SYSLOG_NILVALUE; this->_server = server;
this->_appName = SYSLOG_NILVALUE; }
this->_priDefault = LOGLEVEL_KERN; this->_port = port;
return *this;
} }
Syslog &Syslog::server(const char *server, uint16_t port) Syslog &Syslog::server(IPAddress ip, uint16_t port) {
{ this->_ip = ip;
if (this->_ip.fromString(server)) { this->_server = NULL;
this->_server = NULL; this->_port = port;
} else { return *this;
this->_server = server;
}
this->_port = port;
return *this;
} }
Syslog &Syslog::server(IPAddress ip, uint16_t port) Syslog &Syslog::deviceHostname(const char *deviceHostname) {
{ this->_deviceHostname = (deviceHostname == NULL) ? SYSLOG_NILVALUE : deviceHostname;
this->_ip = ip; return *this;
this->_server = NULL;
this->_port = port;
return *this;
} }
Syslog &Syslog::deviceHostname(const char *deviceHostname) Syslog &Syslog::appName(const char *appName) {
{ this->_appName = (appName == NULL) ? SYSLOG_NILVALUE : appName;
this->_deviceHostname = (deviceHostname == NULL) ? SYSLOG_NILVALUE : deviceHostname; return *this;
return *this;
} }
Syslog &Syslog::appName(const char *appName) Syslog &Syslog::defaultPriority(uint16_t pri) {
{ this->_priDefault = pri;
this->_appName = (appName == NULL) ? SYSLOG_NILVALUE : appName; return *this;
return *this;
} }
Syslog &Syslog::defaultPriority(uint16_t pri) Syslog &Syslog::logMask(uint8_t priMask) {
{ this->_priMask = priMask;
this->_priDefault = pri; return *this;
return *this;
} }
Syslog &Syslog::logMask(uint8_t priMask) void Syslog::enable() {
{ this->_client->begin(this->_port);
this->_priMask = priMask; this->_enabled = true;
return *this;
} }
void Syslog::enable() void Syslog::disable() {
{ this->_enabled = false;
this->_client->begin(this->_port); this->_client->stop();
this->_enabled = true;
} }
void Syslog::disable() bool Syslog::isEnabled() { return this->_enabled; }
{
this->_enabled = false;
this->_client->stop();
}
bool Syslog::isEnabled() bool Syslog::vlogf(uint16_t pri, const char *fmt, va_list args) { return this->vlogf(pri, this->_appName, fmt, args); }
{
return this->_enabled;
}
bool Syslog::vlogf(uint16_t pri, const char *fmt, va_list args) bool Syslog::vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args) {
{ char *message;
return this->vlogf(pri, this->_appName, fmt, args); size_t initialLen;
} size_t len;
bool result;
bool Syslog::vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args) initialLen = strlen(fmt);
{
char *message;
size_t initialLen;
size_t len;
bool result;
initialLen = strlen(fmt); message = new char[initialLen + 1];
message = new char[initialLen + 1];
len = vsnprintf(message, initialLen + 1, fmt, args);
if (len > initialLen) {
delete[] message;
message = new char[len + 1];
vsnprintf(message, len + 1, fmt, args);
}
result = this->_sendLog(pri, appName, message);
len = vsnprintf(message, initialLen + 1, fmt, args);
if (len > initialLen) {
delete[] message; delete[] message;
return result; message = new char[len + 1];
vsnprintf(message, len + 1, fmt, args);
}
result = this->_sendLog(pri, appName, message);
delete[] message;
return result;
} }
inline bool Syslog::_sendLog(uint16_t pri, const char *appName, const char *message) inline bool Syslog::_sendLog(uint16_t pri, const char *appName, const char *message) {
{ int result;
int result;
#ifdef ARCH_PORTDUINO #ifdef ARCH_PORTDUINO
bool utf = !portduino_config.ascii_logs; bool utf = !portduino_config.ascii_logs;
#else #else
bool utf = true; bool utf = true;
#endif #endif
if (!this->_enabled) if (!this->_enabled)
return false; return false;
if ((this->_server == NULL && this->_ip == INADDR_NONE) || this->_port == 0) if ((this->_server == NULL && this->_ip == INADDR_NONE) || this->_port == 0)
return false; return false;
// Check priority against priMask values.
if ((LOG_MASK(LOG_PRI(pri)) & this->_priMask) == 0)
return true;
// Set default facility if none specified.
if ((pri & LOG_FACMASK) == 0)
pri = LOG_MAKEPRI(LOG_FAC(this->_priDefault), pri);
if (this->_server != NULL) {
result = this->_client->beginPacket(this->_server, this->_port);
} else {
result = this->_client->beginPacket(this->_ip, this->_port);
}
if (result != 1)
return false;
this->_client->print('<');
this->_client->print(pri);
this->_client->print(F(">1 - "));
this->_client->print(this->_deviceHostname);
this->_client->print(' ');
this->_client->print(appName);
this->_client->print(F(" - - - "));
if (utf) {
this->_client->print(F("\xEF\xBB\xBF"));
} else {
this->_client->print(F(" "));
}
this->_client->print(F("["));
this->_client->print(int(millis() / 1000));
this->_client->print(F("]: "));
this->_client->print(message);
this->_client->endPacket();
// Check priority against priMask values.
if ((LOG_MASK(LOG_PRI(pri)) & this->_priMask) == 0)
return true; return true;
// Set default facility if none specified.
if ((pri & LOG_FACMASK) == 0)
pri = LOG_MAKEPRI(LOG_FAC(this->_priDefault), pri);
if (this->_server != NULL) {
result = this->_client->beginPacket(this->_server, this->_port);
} else {
result = this->_client->beginPacket(this->_ip, this->_port);
}
if (result != 1)
return false;
this->_client->print('<');
this->_client->print(pri);
this->_client->print(F(">1 - "));
this->_client->print(this->_deviceHostname);
this->_client->print(' ');
this->_client->print(appName);
this->_client->print(F(" - - - "));
if (utf) {
this->_client->print(F("\xEF\xBB\xBF"));
} else {
this->_client->print(F(" "));
}
this->_client->print(F("["));
this->_client->print(int(millis() / 1000));
this->_client->print(F("]: "));
this->_client->print(message);
this->_client->endPacket();
return true;
} }
#endif #endif
src/DebugConfiguration.h
+32 -33
View File
@@ -74,13 +74,13 @@ extern MemGet memGet;
// Macro-based heap debugging // Macro-based heap debugging
#define DEBUG_HEAP_BEFORE auto heapBefore = memGet.getFreeHeap(); #define DEBUG_HEAP_BEFORE auto heapBefore = memGet.getFreeHeap();
#define DEBUG_HEAP_AFTER(context, ptr) \ #define DEBUG_HEAP_AFTER(context, ptr) \
do { \ do { \
auto heapAfter = memGet.getFreeHeap(); \ auto heapAfter = memGet.getFreeHeap(); \
if (heapBefore != heapAfter) { \ if (heapBefore != heapAfter) { \
LOG_HEAP("Alloc in %s pointer 0x%x, size: %u, free: %u", context, ptr, heapBefore - heapAfter, heapAfter); \ LOG_HEAP("Alloc in %s pointer 0x%x, size: %u, free: %u", context, ptr, heapBefore - heapAfter, heapAfter); \
} \ } \
} while (0) } while (0)
#else #else
#define LOG_HEAP(...) #define LOG_HEAP(...)
@@ -162,37 +162,36 @@ extern "C" void logLegacy(const char *level, const char *fmt, ...);
#if HAS_NETWORKING #if HAS_NETWORKING
class Syslog class Syslog {
{ private:
private: UDP *_client;
UDP *_client; IPAddress _ip;
IPAddress _ip; const char *_server;
const char *_server; uint16_t _port;
uint16_t _port; const char *_deviceHostname;
const char *_deviceHostname; const char *_appName;
const char *_appName; uint16_t _priDefault;
uint16_t _priDefault; uint8_t _priMask = 0xff;
uint8_t _priMask = 0xff; bool _enabled = false;
bool _enabled = false;
bool _sendLog(uint16_t pri, const char *appName, const char *message); bool _sendLog(uint16_t pri, const char *appName, const char *message);
public: public:
explicit Syslog(UDP &client); explicit Syslog(UDP &client);
Syslog &server(const char *server, uint16_t port); Syslog &server(const char *server, uint16_t port);
Syslog &server(IPAddress ip, uint16_t port); Syslog &server(IPAddress ip, uint16_t port);
Syslog &deviceHostname(const char *deviceHostname); Syslog &deviceHostname(const char *deviceHostname);
Syslog &appName(const char *appName); Syslog &appName(const char *appName);
Syslog &defaultPriority(uint16_t pri = LOGLEVEL_KERN); Syslog &defaultPriority(uint16_t pri = LOGLEVEL_KERN);
Syslog &logMask(uint8_t priMask); Syslog &logMask(uint8_t priMask);
void enable(); void enable();
void disable(); void disable();
bool isEnabled(); bool isEnabled();
bool vlogf(uint16_t pri, const char *fmt, va_list args) __attribute__((format(printf, 3, 0))); bool vlogf(uint16_t pri, const char *fmt, va_list args) __attribute__((format(printf, 3, 0)));
bool vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args) __attribute__((format(printf, 3, 0))); bool vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args) __attribute__((format(printf, 3, 0)));
}; };
#endif // HAS_NETWORKING #endif // HAS_NETWORKING
src/DisplayFormatters.cpp
+76 -79
View File
@@ -1,86 +1,83 @@
#include "DisplayFormatters.h" #include "DisplayFormatters.h"
const char *DisplayFormatters::getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName, const char *DisplayFormatters::getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName, bool usePreset) {
bool usePreset)
{
// If use_preset is false, always return "Custom" // If use_preset is false, always return "Custom"
if (!usePreset) { if (!usePreset) {
return "Custom"; return "Custom";
} }
switch (preset) { switch (preset) {
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_TURBO: case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_TURBO:
return useShortName ? "ShortT" : "ShortTurbo"; return useShortName ? "ShortT" : "ShortTurbo";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_SLOW: case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_SLOW:
return useShortName ? "ShortS" : "ShortSlow"; return useShortName ? "ShortS" : "ShortSlow";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_FAST: case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_FAST:
return useShortName ? "ShortF" : "ShortFast"; return useShortName ? "ShortF" : "ShortFast";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_SLOW: case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_SLOW:
return useShortName ? "MedS" : "MediumSlow"; return useShortName ? "MedS" : "MediumSlow";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_FAST: case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_FAST:
return useShortName ? "MedF" : "MediumFast"; return useShortName ? "MedF" : "MediumFast";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_SLOW: case meshtastic_Config_LoRaConfig_ModemPreset_LONG_SLOW:
return useShortName ? "LongS" : "LongSlow"; return useShortName ? "LongS" : "LongSlow";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_FAST: case meshtastic_Config_LoRaConfig_ModemPreset_LONG_FAST:
return useShortName ? "LongF" : "LongFast"; return useShortName ? "LongF" : "LongFast";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_TURBO: case meshtastic_Config_LoRaConfig_ModemPreset_LONG_TURBO:
return useShortName ? "LongT" : "LongTurbo"; return useShortName ? "LongT" : "LongTurbo";
break; break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_MODERATE: case meshtastic_Config_LoRaConfig_ModemPreset_LONG_MODERATE:
return useShortName ? "LongM" : "LongMod"; return useShortName ? "LongM" : "LongMod";
break; break;
default: default:
return useShortName ? "Custom" : "Invalid"; return useShortName ? "Custom" : "Invalid";
break; break;
} }
} }
const char *DisplayFormatters::getDeviceRole(meshtastic_Config_DeviceConfig_Role role) const char *DisplayFormatters::getDeviceRole(meshtastic_Config_DeviceConfig_Role role) {
{ switch (role) {
switch (role) { case meshtastic_Config_DeviceConfig_Role_CLIENT:
case meshtastic_Config_DeviceConfig_Role_CLIENT: return "Client";
return "Client"; break;
break; case meshtastic_Config_DeviceConfig_Role_CLIENT_MUTE:
case meshtastic_Config_DeviceConfig_Role_CLIENT_MUTE: return "Client Mute";
return "Client Mute"; break;
break; case meshtastic_Config_DeviceConfig_Role_CLIENT_HIDDEN:
case meshtastic_Config_DeviceConfig_Role_CLIENT_HIDDEN: return "Client Hidden";
return "Client Hidden"; break;
break; case meshtastic_Config_DeviceConfig_Role_CLIENT_BASE:
case meshtastic_Config_DeviceConfig_Role_CLIENT_BASE: return "Client Base";
return "Client Base"; break;
break; case meshtastic_Config_DeviceConfig_Role_LOST_AND_FOUND:
case meshtastic_Config_DeviceConfig_Role_LOST_AND_FOUND: return "Lost and Found";
return "Lost and Found"; break;
break; case meshtastic_Config_DeviceConfig_Role_TRACKER:
case meshtastic_Config_DeviceConfig_Role_TRACKER: return "Tracker";
return "Tracker"; break;
break; case meshtastic_Config_DeviceConfig_Role_SENSOR:
case meshtastic_Config_DeviceConfig_Role_SENSOR: return "Sensor";
return "Sensor"; break;
break; case meshtastic_Config_DeviceConfig_Role_TAK:
case meshtastic_Config_DeviceConfig_Role_TAK: return "TAK";
return "TAK"; break;
break; case meshtastic_Config_DeviceConfig_Role_TAK_TRACKER:
case meshtastic_Config_DeviceConfig_Role_TAK_TRACKER: return "TAK Tracker";
return "TAK Tracker"; break;
break; case meshtastic_Config_DeviceConfig_Role_ROUTER:
case meshtastic_Config_DeviceConfig_Role_ROUTER: return "Router";
return "Router"; break;
break; case meshtastic_Config_DeviceConfig_Role_ROUTER_LATE:
case meshtastic_Config_DeviceConfig_Role_ROUTER_LATE: return "Router Late";
return "Router Late"; break;
break; default:
default: return "Unknown";
return "Unknown"; break;
break; }
}
} }
src/DisplayFormatters.h
+4 -6
View File
@@ -1,10 +1,8 @@
#pragma once #pragma once
#include "NodeDB.h" #include "NodeDB.h"
class DisplayFormatters class DisplayFormatters {
{ public:
public: static const char *getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName, bool usePreset);
static const char *getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName, static const char *getDeviceRole(meshtastic_Config_DeviceConfig_Role role);
bool usePreset);
static const char *getDeviceRole(meshtastic_Config_DeviceConfig_Role role);
}; };
src/FSCommon.cpp
+198 -206
View File
@@ -1,11 +1,11 @@
/** /**
* @file FSCommon.cpp * @file FSCommon.cpp
* @brief This file contains functions for common filesystem operations such as copying, renaming, listing and deleting files and * @brief This file contains functions for common filesystem operations such as copying, renaming, listing and deleting
* directories. * files and directories.
* *
* The functions in this file are used to perform common filesystem operations such as copying, renaming, listing and deleting * The functions in this file are used to perform common filesystem operations such as copying, renaming, listing and
* files and directories. These functions are used in the Meshtastic-device project to manage files and directories on the * deleting files and directories. These functions are used in the Meshtastic-device project to manage files and
* device's filesystem. * directories on the device's filesystem.
* *
*/ */
#include "FSCommon.h" #include "FSCommon.h"
@@ -37,34 +37,33 @@ SPIClass SPI_HSPI(HSPI);
* @param to The path of the destination file. * @param to The path of the destination file.
* @return true if the file was successfully copied, false otherwise. * @return true if the file was successfully copied, false otherwise.
*/ */
bool copyFile(const char *from, const char *to) bool copyFile(const char *from, const char *to) {
{
#ifdef FSCom #ifdef FSCom
// take SPI Lock // take SPI Lock
concurrency::LockGuard g(spiLock); concurrency::LockGuard g(spiLock);
unsigned char cbuffer[16]; unsigned char cbuffer[16];
File f1 = FSCom.open(from, FILE_O_READ); File f1 = FSCom.open(from, FILE_O_READ);
if (!f1) { if (!f1) {
LOG_ERROR("Failed to open source file %s", from); LOG_ERROR("Failed to open source file %s", from);
return false; return false;
} }
File f2 = FSCom.open(to, FILE_O_WRITE); File f2 = FSCom.open(to, FILE_O_WRITE);
if (!f2) { if (!f2) {
LOG_ERROR("Failed to open destination file %s", to); LOG_ERROR("Failed to open destination file %s", to);
return false; return false;
} }
while (f1.available() > 0) { while (f1.available() > 0) {
byte i = f1.read(cbuffer, 16); byte i = f1.read(cbuffer, 16);
f2.write(cbuffer, i); f2.write(cbuffer, i);
} }
f2.flush(); f2.flush();
f2.close(); f2.close();
f1.close(); f1.close();
return true; return true;
#endif #endif
} }
@@ -76,24 +75,23 @@ bool copyFile(const char *from, const char *to)
* *
* @return True if the file was successfully renamed, false otherwise. * @return True if the file was successfully renamed, false otherwise.
*/ */
bool renameFile(const char *pathFrom, const char *pathTo) bool renameFile(const char *pathFrom, const char *pathTo) {
{
#ifdef FSCom #ifdef FSCom
#ifdef ARCH_ESP32 #ifdef ARCH_ESP32
// take SPI Lock // take SPI Lock
spiLock->lock(); spiLock->lock();
// rename was fixed for ESP32 IDF LittleFS in April // rename was fixed for ESP32 IDF LittleFS in April
bool result = FSCom.rename(pathFrom, pathTo); bool result = FSCom.rename(pathFrom, pathTo);
spiLock->unlock(); spiLock->unlock();
return result; return result;
#else #else
// copyFile does its own locking. // copyFile does its own locking.
if (copyFile(pathFrom, pathTo) && FSCom.remove(pathFrom)) { if (copyFile(pathFrom, pathTo) && FSCom.remove(pathFrom)) {
return true; return true;
} else { } else {
return false; return false;
} }
#endif #endif
#endif #endif
@@ -111,45 +109,44 @@ bool renameFile(const char *pathFrom, const char *pathTo)
* @param levels The number of levels of subdirectories to list. * @param levels The number of levels of subdirectories to list.
* @return A vector of strings containing the full path of each file in the directory. * @return A vector of strings containing the full path of each file in the directory.
*/ */
std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels) std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels) {
{ std::vector<meshtastic_FileInfo> filenames = {};
std::vector<meshtastic_FileInfo> filenames = {};
#ifdef FSCom #ifdef FSCom
File root = FSCom.open(dirname, FILE_O_READ); File root = FSCom.open(dirname, FILE_O_READ);
if (!root) if (!root)
return filenames;
if (!root.isDirectory())
return filenames;
File file = root.openNextFile();
while (file) {
if (file.isDirectory() && !String(file.name()).endsWith(".")) {
if (levels) {
#ifdef ARCH_ESP32
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.path(), levels - 1);
#else
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.name(), levels - 1);
#endif
filenames.insert(filenames.end(), subDirFilenames.begin(), subDirFilenames.end());
file.close();
}
} else {
meshtastic_FileInfo fileInfo = {"", static_cast<uint32_t>(file.size())};
#ifdef ARCH_ESP32
strcpy(fileInfo.file_name, file.path());
#else
strcpy(fileInfo.file_name, file.name());
#endif
if (!String(fileInfo.file_name).endsWith(".")) {
filenames.push_back(fileInfo);
}
file.close();
}
file = root.openNextFile();
}
root.close();
#endif
return filenames; return filenames;
if (!root.isDirectory())
return filenames;
File file = root.openNextFile();
while (file) {
if (file.isDirectory() && !String(file.name()).endsWith(".")) {
if (levels) {
#ifdef ARCH_ESP32
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.path(), levels - 1);
#else
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.name(), levels - 1);
#endif
filenames.insert(filenames.end(), subDirFilenames.begin(), subDirFilenames.end());
file.close();
}
} else {
meshtastic_FileInfo fileInfo = {"", static_cast<uint32_t>(file.size())};
#ifdef ARCH_ESP32
strcpy(fileInfo.file_name, file.path());
#else
strcpy(fileInfo.file_name, file.name());
#endif
if (!String(fileInfo.file_name).endsWith(".")) {
filenames.push_back(fileInfo);
}
file.close();
}
file = root.openNextFile();
}
root.close();
#endif
return filenames;
} }
/** /**
@@ -160,100 +157,98 @@ std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels)
* @param levels The number of levels of subdirectories to list. * @param levels The number of levels of subdirectories to list.
* @param del Whether or not to delete the contents of the directory after listing. * @param del Whether or not to delete the contents of the directory after listing.
*/ */
void listDir(const char *dirname, uint8_t levels, bool del) void listDir(const char *dirname, uint8_t levels, bool del) {
{
#ifdef FSCom #ifdef FSCom
#if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO)) #if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
char buffer[255]; char buffer[255];
#endif #endif
File root = FSCom.open(dirname, FILE_O_READ); File root = FSCom.open(dirname, FILE_O_READ);
if (!root) { if (!root) {
return; return;
} }
if (!root.isDirectory()) { if (!root.isDirectory()) {
return; return;
} }
File file = root.openNextFile(); File file = root.openNextFile();
while ( while (file && file.name()[0]) { // This file.name() check is a workaround for a bug in the Adafruit LittleFS nrf52
file && // glue (see issue 4395)
file.name()[0]) { // This file.name() check is a workaround for a bug in the Adafruit LittleFS nrf52 glue (see issue 4395) if (file.isDirectory() && !String(file.name()).endsWith(".")) {
if (file.isDirectory() && !String(file.name()).endsWith(".")) { if (levels) {
if (levels) {
#ifdef ARCH_ESP32 #ifdef ARCH_ESP32
listDir(file.path(), levels - 1, del); listDir(file.path(), levels - 1, del);
if (del) { if (del) {
LOG_DEBUG("Remove %s", file.path()); LOG_DEBUG("Remove %s", file.path());
strncpy(buffer, file.path(), sizeof(buffer)); strncpy(buffer, file.path(), sizeof(buffer));
file.close(); file.close();
FSCom.rmdir(buffer); FSCom.rmdir(buffer);
} else {
file.close();
}
#elif (defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
listDir(file.name(), levels - 1, del);
if (del) {
LOG_DEBUG("Remove %s", file.name());
strncpy(buffer, file.name(), sizeof(buffer));
file.close();
FSCom.rmdir(buffer);
} else {
file.close();
}
#else
LOG_DEBUG(" %s (directory)", file.name());
listDir(file.name(), levels - 1, del);
file.close();
#endif
}
} else { } else {
#ifdef ARCH_ESP32 file.close();
if (del) {
LOG_DEBUG("Delete %s", file.path());
strncpy(buffer, file.path(), sizeof(buffer));
file.close();
FSCom.remove(buffer);
} else {
LOG_DEBUG(" %s (%i Bytes)", file.path(), file.size());
file.close();
}
#elif (defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
if (del) {
LOG_DEBUG("Delete %s", file.name());
strncpy(buffer, file.name(), sizeof(buffer));
file.close();
FSCom.remove(buffer);
} else {
LOG_DEBUG(" %s (%i Bytes)", file.name(), file.size());
file.close();
}
#else
LOG_DEBUG(" %s (%i Bytes)", file.name(), file.size());
file.close();
#endif
} }
file = root.openNextFile();
}
#ifdef ARCH_ESP32
if (del) {
LOG_DEBUG("Remove %s", root.path());
strncpy(buffer, root.path(), sizeof(buffer));
root.close();
FSCom.rmdir(buffer);
} else {
root.close();
}
#elif (defined(ARCH_RP2040) || defined(ARCH_PORTDUINO)) #elif (defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
if (del) { listDir(file.name(), levels - 1, del);
LOG_DEBUG("Remove %s", root.name()); if (del) {
strncpy(buffer, root.name(), sizeof(buffer)); LOG_DEBUG("Remove %s", file.name());
root.close(); strncpy(buffer, file.name(), sizeof(buffer));
FSCom.rmdir(buffer); file.close();
} else { FSCom.rmdir(buffer);
root.close(); } else {
} file.close();
}
#else #else
LOG_DEBUG(" %s (directory)", file.name());
listDir(file.name(), levels - 1, del);
file.close();
#endif
}
} else {
#ifdef ARCH_ESP32
if (del) {
LOG_DEBUG("Delete %s", file.path());
strncpy(buffer, file.path(), sizeof(buffer));
file.close();
FSCom.remove(buffer);
} else {
LOG_DEBUG(" %s (%i Bytes)", file.path(), file.size());
file.close();
}
#elif (defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
if (del) {
LOG_DEBUG("Delete %s", file.name());
strncpy(buffer, file.name(), sizeof(buffer));
file.close();
FSCom.remove(buffer);
} else {
LOG_DEBUG(" %s (%i Bytes)", file.name(), file.size());
file.close();
}
#else
LOG_DEBUG(" %s (%i Bytes)", file.name(), file.size());
file.close();
#endif
}
file = root.openNextFile();
}
#ifdef ARCH_ESP32
if (del) {
LOG_DEBUG("Remove %s", root.path());
strncpy(buffer, root.path(), sizeof(buffer));
root.close(); root.close();
FSCom.rmdir(buffer);
} else {
root.close();
}
#elif (defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
if (del) {
LOG_DEBUG("Remove %s", root.name());
strncpy(buffer, root.name(), sizeof(buffer));
root.close();
FSCom.rmdir(buffer);
} else {
root.close();
}
#else
root.close();
#endif #endif
#endif #endif
} }
@@ -265,15 +260,14 @@ void listDir(const char *dirname, uint8_t levels, bool del)
* *
* @param dirname The name of the directory to remove. * @param dirname The name of the directory to remove.
*/ */
void rmDir(const char *dirname) void rmDir(const char *dirname) {
{
#ifdef FSCom #ifdef FSCom
#if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO)) #if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
listDir(dirname, 10, true); listDir(dirname, 10, true);
#elif defined(ARCH_NRF52) #elif defined(ARCH_NRF52)
// nRF52 implementation of LittleFS has a recursive delete function // nRF52 implementation of LittleFS has a recursive delete function
FSCom.rmdir_r(dirname); FSCom.rmdir_r(dirname);
#endif #endif
#endif #endif
@@ -284,55 +278,53 @@ void rmDir(const char *dirname)
*/ */
__attribute__((weak, noinline)) void preFSBegin() {} __attribute__((weak, noinline)) void preFSBegin() {}
void fsInit() void fsInit() {
{
#ifdef FSCom #ifdef FSCom
concurrency::LockGuard g(spiLock); concurrency::LockGuard g(spiLock);
preFSBegin(); preFSBegin();
if (!FSBegin()) { if (!FSBegin()) {
LOG_ERROR("Filesystem mount failed"); LOG_ERROR("Filesystem mount failed");
// assert(0); This auto-formats the partition, so no need to fail here. // assert(0); This auto-formats the partition, so no need to fail here.
} }
#if defined(ARCH_ESP32) #if defined(ARCH_ESP32)
LOG_DEBUG("Filesystem files (%d/%d Bytes):", FSCom.usedBytes(), FSCom.totalBytes()); LOG_DEBUG("Filesystem files (%d/%d Bytes):", FSCom.usedBytes(), FSCom.totalBytes());
#else #else
LOG_DEBUG("Filesystem files:"); LOG_DEBUG("Filesystem files:");
#endif #endif
listDir("/", 10); listDir("/", 10);
#endif #endif
} }
/** /**
* Initializes the SD card and mounts the file system. * Initializes the SD card and mounts the file system.
*/ */
void setupSDCard() void setupSDCard() {
{
#if defined(HAS_SDCARD) && !defined(SDCARD_USE_SOFT_SPI) #if defined(HAS_SDCARD) && !defined(SDCARD_USE_SOFT_SPI)
concurrency::LockGuard g(spiLock); concurrency::LockGuard g(spiLock);
SDHandler.begin(SPI_SCK, SPI_MISO, SPI_MOSI); SDHandler.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
if (!SD.begin(SDCARD_CS, SDHandler, SD_SPI_FREQUENCY)) { if (!SD.begin(SDCARD_CS, SDHandler, SD_SPI_FREQUENCY)) {
LOG_DEBUG("No SD_MMC card detected"); LOG_DEBUG("No SD_MMC card detected");
return; return;
} }
uint8_t cardType = SD.cardType(); uint8_t cardType = SD.cardType();
if (cardType == CARD_NONE) { if (cardType == CARD_NONE) {
LOG_DEBUG("No SD_MMC card attached"); LOG_DEBUG("No SD_MMC card attached");
return; return;
} }
LOG_DEBUG("SD_MMC Card Type: "); LOG_DEBUG("SD_MMC Card Type: ");
if (cardType == CARD_MMC) { if (cardType == CARD_MMC) {
LOG_DEBUG("MMC"); LOG_DEBUG("MMC");
} else if (cardType == CARD_SD) { } else if (cardType == CARD_SD) {
LOG_DEBUG("SDSC"); LOG_DEBUG("SDSC");
} else if (cardType == CARD_SDHC) { } else if (cardType == CARD_SDHC) {
LOG_DEBUG("SDHC"); LOG_DEBUG("SDHC");
} else { } else {
LOG_DEBUG("UNKNOWN"); LOG_DEBUG("UNKNOWN");
} }
uint64_t cardSize = SD.cardSize() / (1024 * 1024); uint64_t cardSize = SD.cardSize() / (1024 * 1024);
LOG_DEBUG("SD Card Size: %lu MB", (uint32_t)cardSize); LOG_DEBUG("SD Card Size: %lu MB", (uint32_t)cardSize);
LOG_DEBUG("Total space: %lu MB", (uint32_t)(SD.totalBytes() / (1024 * 1024))); LOG_DEBUG("Total space: %lu MB", (uint32_t)(SD.totalBytes() / (1024 * 1024)));
LOG_DEBUG("Used space: %lu MB", (uint32_t)(SD.usedBytes() / (1024 * 1024))); LOG_DEBUG("Used space: %lu MB", (uint32_t)(SD.usedBytes() / (1024 * 1024)));
#endif #endif
} }
src/Fusion/FusionAhrs.c
+295 -327
View File
@@ -43,18 +43,17 @@ static inline int Clamp(const int value, const int min, const int max);
* @brief Initialises the AHRS algorithm structure. * @brief Initialises the AHRS algorithm structure.
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
*/ */
void FusionAhrsInitialise(FusionAhrs *const ahrs) void FusionAhrsInitialise(FusionAhrs *const ahrs) {
{ const FusionAhrsSettings settings = {
const FusionAhrsSettings settings = { .convention = FusionConventionNwu,
.convention = FusionConventionNwu, .gain = 0.5f,
.gain = 0.5f, .gyroscopeRange = 0.0f,
.gyroscopeRange = 0.0f, .accelerationRejection = 90.0f,
.accelerationRejection = 90.0f, .magneticRejection = 90.0f,
.magneticRejection = 90.0f, .recoveryTriggerPeriod = 0,
.recoveryTriggerPeriod = 0, };
}; FusionAhrsSetSettings(ahrs, &settings);
FusionAhrsSetSettings(ahrs, &settings); FusionAhrsReset(ahrs);
FusionAhrsReset(ahrs);
} }
/** /**
@@ -62,21 +61,20 @@ void FusionAhrsInitialise(FusionAhrs *const ahrs)
* algorithm while maintaining the current settings. * algorithm while maintaining the current settings.
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
*/ */
void FusionAhrsReset(FusionAhrs *const ahrs) void FusionAhrsReset(FusionAhrs *const ahrs) {
{ ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
ahrs->quaternion = FUSION_IDENTITY_QUATERNION; ahrs->accelerometer = FUSION_VECTOR_ZERO;
ahrs->accelerometer = FUSION_VECTOR_ZERO; ahrs->initialising = true;
ahrs->initialising = true; ahrs->rampedGain = INITIAL_GAIN;
ahrs->rampedGain = INITIAL_GAIN; ahrs->angularRateRecovery = false;
ahrs->angularRateRecovery = false; ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; ahrs->accelerometerIgnored = false;
ahrs->accelerometerIgnored = false; ahrs->accelerationRecoveryTrigger = 0;
ahrs->accelerationRecoveryTrigger = 0; ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; ahrs->magnetometerIgnored = false;
ahrs->magnetometerIgnored = false; ahrs->magneticRecoveryTrigger = 0;
ahrs->magneticRecoveryTrigger = 0; ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
} }
/** /**
@@ -84,28 +82,25 @@ void FusionAhrsReset(FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @param settings Settings. * @param settings Settings.
*/ */
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) {
{ ahrs->settings.convention = settings->convention;
ahrs->settings.convention = settings->convention; ahrs->settings.gain = settings->gain;
ahrs->settings.gain = settings->gain; ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange; ahrs->settings.accelerationRejection =
ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
? FLT_MAX ahrs->settings.magneticRejection =
: powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
ahrs->settings.magneticRejection = ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod; ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; ahrs->settings.accelerationRejection = FLT_MAX;
if ((settings->gain == 0.0f) || ahrs->settings.magneticRejection = FLT_MAX;
(settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero }
ahrs->settings.accelerationRejection = FLT_MAX; if (ahrs->initialising == false) {
ahrs->settings.magneticRejection = FLT_MAX; ahrs->rampedGain = ahrs->settings.gain;
} }
if (ahrs->initialising == false) { ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
ahrs->rampedGain = ahrs->settings.gain;
}
ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
} }
/** /**
@@ -117,119 +112,113 @@ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *con
* @param magnetometer Magnetometer measurement in arbitrary units. * @param magnetometer Magnetometer measurement in arbitrary units.
* @param deltaTime Delta time in seconds. * @param deltaTime Delta time in seconds.
*/ */
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer,
const FusionVector magnetometer, const float deltaTime) const float deltaTime) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
// Store accelerometer // Store accelerometer
ahrs->accelerometer = accelerometer; ahrs->accelerometer = accelerometer;
// Reinitialise if gyroscope range exceeded // Reinitialise if gyroscope range exceeded
if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) ||
(fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) { (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
const FusionQuaternion quaternion = ahrs->quaternion; const FusionQuaternion quaternion = ahrs->quaternion;
FusionAhrsReset(ahrs); FusionAhrsReset(ahrs);
ahrs->quaternion = quaternion; ahrs->quaternion = quaternion;
ahrs->angularRateRecovery = true; ahrs->angularRateRecovery = true;
}
// Ramp down gain during initialisation
if (ahrs->initialising) {
ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime;
if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) {
ahrs->rampedGain = ahrs->settings.gain;
ahrs->initialising = false;
ahrs->angularRateRecovery = false;
}
}
// Calculate direction of gravity indicated by algorithm
const FusionVector halfGravity = HalfGravity(ahrs);
// Calculate accelerometer feedback
FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
ahrs->accelerometerIgnored = true;
if (FusionVectorIsZero(accelerometer) == false) {
// Calculate accelerometer feedback scaled by 0.5
ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity);
// Don't ignore accelerometer if acceleration error below threshold
if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) {
ahrs->accelerometerIgnored = false;
ahrs->accelerationRecoveryTrigger -= 9;
} else {
ahrs->accelerationRecoveryTrigger += 1;
} }
// Ramp down gain during initialisation // Don't ignore accelerometer during acceleration recovery
if (ahrs->initialising) { if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) {
ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; ahrs->accelerationRecoveryTimeout = 0;
if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) { ahrs->accelerometerIgnored = false;
ahrs->rampedGain = ahrs->settings.gain; } else {
ahrs->initialising = false; ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->angularRateRecovery = false; }
} ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
// Apply accelerometer feedback
if (ahrs->accelerometerIgnored == false) {
halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
}
}
// Calculate magnetometer feedback
FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
ahrs->magnetometerIgnored = true;
if (FusionVectorIsZero(magnetometer) == false) {
// Calculate direction of magnetic field indicated by algorithm
const FusionVector halfMagnetic = HalfMagnetic(ahrs);
// Calculate magnetometer feedback scaled by 0.5
ahrs->halfMagnetometerFeedback = Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);
// Don't ignore magnetometer if magnetic error below threshold
if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) {
ahrs->magnetometerIgnored = false;
ahrs->magneticRecoveryTrigger -= 9;
} else {
ahrs->magneticRecoveryTrigger += 1;
} }
// Calculate direction of gravity indicated by algorithm // Don't ignore magnetometer during magnetic recovery
const FusionVector halfGravity = HalfGravity(ahrs); if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
ahrs->magneticRecoveryTimeout = 0;
// Calculate accelerometer feedback ahrs->magnetometerIgnored = false;
FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; } else {
ahrs->accelerometerIgnored = true; ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
if (FusionVectorIsZero(accelerometer) == false) {
// Calculate accelerometer feedback scaled by 0.5
ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity);
// Don't ignore accelerometer if acceleration error below threshold
if (ahrs->initialising ||
((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) {
ahrs->accelerometerIgnored = false;
ahrs->accelerationRecoveryTrigger -= 9;
} else {
ahrs->accelerationRecoveryTrigger += 1;
}
// Don't ignore accelerometer during acceleration recovery
if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) {
ahrs->accelerationRecoveryTimeout = 0;
ahrs->accelerometerIgnored = false;
} else {
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
}
ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
// Apply accelerometer feedback
if (ahrs->accelerometerIgnored == false) {
halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback;
}
} }
ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
// Calculate magnetometer feedback // Apply magnetometer feedback
FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; if (ahrs->magnetometerIgnored == false) {
ahrs->magnetometerIgnored = true; halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
if (FusionVectorIsZero(magnetometer) == false) {
// Calculate direction of magnetic field indicated by algorithm
const FusionVector halfMagnetic = HalfMagnetic(ahrs);
// Calculate magnetometer feedback scaled by 0.5
ahrs->halfMagnetometerFeedback =
Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic);
// Don't ignore magnetometer if magnetic error below threshold
if (ahrs->initialising ||
((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) {
ahrs->magnetometerIgnored = false;
ahrs->magneticRecoveryTrigger -= 9;
} else {
ahrs->magneticRecoveryTrigger += 1;
}
// Don't ignore magnetometer during magnetic recovery
if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
ahrs->magneticRecoveryTimeout = 0;
ahrs->magnetometerIgnored = false;
} else {
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
}
ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod);
// Apply magnetometer feedback
if (ahrs->magnetometerIgnored == false) {
halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
}
} }
}
// Convert gyroscope to radians per second scaled by 0.5 // Convert gyroscope to radians per second scaled by 0.5
const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
// Apply feedback to gyroscope // Apply feedback to gyroscope
const FusionVector adjustedHalfGyroscope = FusionVectorAdd( const FusionVector adjustedHalfGyroscope = FusionVectorAdd(
halfGyroscope, halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
// Integrate rate of change of quaternion // Integrate rate of change of quaternion
ahrs->quaternion = FusionQuaternionAdd( ahrs->quaternion = FusionQuaternionAdd(
ahrs->quaternion, ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
// Normalise quaternion // Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
#undef Q #undef Q
} }
@@ -238,29 +227,28 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return Direction of gravity scaled by 0.5. * @return Direction of gravity scaled by 0.5.
*/ */
static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
switch (ahrs->settings.convention) { switch (ahrs->settings.convention) {
case FusionConventionNwu: case FusionConventionNwu:
case FusionConventionEnu: { case FusionConventionEnu: {
const FusionVector halfGravity = {.axis = { const FusionVector halfGravity = {.axis = {
.x = Q.x * Q.z - Q.w * Q.y, .x = Q.x * Q.z - Q.w * Q.y,
.y = Q.y * Q.z + Q.w * Q.x, .y = Q.y * Q.z + Q.w * Q.x,
.z = Q.w * Q.w - 0.5f + Q.z * Q.z, .z = Q.w * Q.w - 0.5f + Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by 0.5 }}; // third column of transposed rotation matrix scaled by 0.5
return halfGravity; return halfGravity;
} }
case FusionConventionNed: { case FusionConventionNed: {
const FusionVector halfGravity = {.axis = { const FusionVector halfGravity = {.axis = {
.x = Q.w * Q.y - Q.x * Q.z, .x = Q.w * Q.y - Q.x * Q.z,
.y = -1.0f * (Q.y * Q.z + Q.w * Q.x), .y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 0.5f - Q.w * Q.w - Q.z * Q.z, .z = 0.5f - Q.w * Q.w - Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by -0.5 }}; // third column of transposed rotation matrix scaled by -0.5
return halfGravity; return halfGravity;
} }
} }
return FUSION_VECTOR_ZERO; // avoid compiler warning return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q #undef Q
} }
@@ -269,36 +257,35 @@ static inline FusionVector HalfGravity(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return Direction of the magnetic field scaled by 0.5. * @return Direction of the magnetic field scaled by 0.5.
*/ */
static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
switch (ahrs->settings.convention) { switch (ahrs->settings.convention) {
case FusionConventionNwu: { case FusionConventionNwu: {
const FusionVector halfMagnetic = {.axis = { const FusionVector halfMagnetic = {.axis = {
.x = Q.x * Q.y + Q.w * Q.z, .x = Q.x * Q.y + Q.w * Q.z,
.y = Q.w * Q.w - 0.5f + Q.y * Q.y, .y = Q.w * Q.w - 0.5f + Q.y * Q.y,
.z = Q.y * Q.z - Q.w * Q.x, .z = Q.y * Q.z - Q.w * Q.x,
}}; // second column of transposed rotation matrix scaled by 0.5 }}; // second column of transposed rotation matrix scaled by 0.5
return halfMagnetic; return halfMagnetic;
} }
case FusionConventionEnu: { case FusionConventionEnu: {
const FusionVector halfMagnetic = {.axis = { const FusionVector halfMagnetic = {.axis = {
.x = 0.5f - Q.w * Q.w - Q.x * Q.x, .x = 0.5f - Q.w * Q.w - Q.x * Q.x,
.y = Q.w * Q.z - Q.x * Q.y, .y = Q.w * Q.z - Q.x * Q.y,
.z = -1.0f * (Q.x * Q.z + Q.w * Q.y), .z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
}}; // first column of transposed rotation matrix scaled by -0.5 }}; // first column of transposed rotation matrix scaled by -0.5
return halfMagnetic; return halfMagnetic;
} }
case FusionConventionNed: { case FusionConventionNed: {
const FusionVector halfMagnetic = {.axis = { const FusionVector halfMagnetic = {.axis = {
.x = -1.0f * (Q.x * Q.y + Q.w * Q.z), .x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
.y = 0.5f - Q.w * Q.w - Q.y * Q.y, .y = 0.5f - Q.w * Q.w - Q.y * Q.y,
.z = Q.w * Q.x - Q.y * Q.z, .z = Q.w * Q.x - Q.y * Q.z,
}}; // second column of transposed rotation matrix scaled by -0.5 }}; // second column of transposed rotation matrix scaled by -0.5
return halfMagnetic; return halfMagnetic;
} }
} }
return FUSION_VECTOR_ZERO; // avoid compiler warning return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q #undef Q
} }
@@ -308,12 +295,11 @@ static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs)
* @param reference Reference. * @param reference Reference.
* @return Feedback. * @return Feedback.
*/ */
static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) {
{ if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference)); }
} return FusionVectorCrossProduct(sensor, reference);
return FusionVectorCrossProduct(sensor, reference);
} }
/** /**
@@ -323,15 +309,14 @@ static inline FusionVector Feedback(const FusionVector sensor, const FusionVecto
* @param max Maximum value. * @param max Maximum value.
* @return Value limited to maximum and minimum. * @return Value limited to maximum and minimum.
*/ */
static inline int Clamp(const int value, const int min, const int max) static inline int Clamp(const int value, const int min, const int max) {
{ if (value < min) {
if (value < min) { return min;
return min; }
} if (value > max) {
if (value > max) { return max;
return max; }
} return value;
return value;
} }
/** /**
@@ -342,17 +327,15 @@ static inline int Clamp(const int value, const int min, const int max)
* @param accelerometer Accelerometer measurement in g. * @param accelerometer Accelerometer measurement in g.
* @param deltaTime Delta time in seconds. * @param deltaTime Delta time in seconds.
*/ */
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) {
const float deltaTime)
{
// Update AHRS algorithm // Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
// Zero heading during initialisation // Zero heading during initialisation
if (ahrs->initialising) { if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f); FusionAhrsSetHeading(ahrs, 0.0f);
} }
} }
/** /**
@@ -364,25 +347,24 @@ void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector g
* @param heading Heading measurement in degrees. * @param heading Heading measurement in degrees.
* @param deltaTime Delta time in seconds. * @param deltaTime Delta time in seconds.
*/ */
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading,
const float heading, const float deltaTime) const float deltaTime) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
// Calculate roll // Calculate roll
const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);
// Calculate magnetometer // Calculate magnetometer
const float headingRadians = FusionDegreesToRadians(heading); const float headingRadians = FusionDegreesToRadians(heading);
const float sinHeadingRadians = sinf(headingRadians); const float sinHeadingRadians = sinf(headingRadians);
const FusionVector magnetometer = {.axis = { const FusionVector magnetometer = {.axis = {
.x = cosf(headingRadians), .x = cosf(headingRadians),
.y = -1.0f * cosf(roll) * sinHeadingRadians, .y = -1.0f * cosf(roll) * sinHeadingRadians,
.z = sinHeadingRadians * sinf(roll), .z = sinHeadingRadians * sinf(roll),
}}; }};
// Update AHRS algorithm // Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
#undef Q #undef Q
} }
@@ -391,20 +373,14 @@ void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth. * @return Quaternion describing the sensor relative to the Earth.
*/ */
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { return ahrs->quaternion; }
{
return ahrs->quaternion;
}
/** /**
* @brief Sets the quaternion describing the sensor relative to the Earth. * @brief Sets the quaternion describing the sensor relative to the Earth.
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @param quaternion Quaternion describing the sensor relative to the Earth. * @param quaternion Quaternion describing the sensor relative to the Earth.
*/ */
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) { ahrs->quaternion = quaternion; }
{
ahrs->quaternion = quaternion;
}
/** /**
* @brief Returns the linear acceleration measurement equal to the accelerometer * @brief Returns the linear acceleration measurement equal to the accelerometer
@@ -412,28 +388,27 @@ void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quat
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return Linear acceleration measurement in g. * @return Linear acceleration measurement in g.
*/ */
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
// Calculate gravity in the sensor coordinate frame // Calculate gravity in the sensor coordinate frame
const FusionVector gravity = {.axis = { const FusionVector gravity = {.axis = {
.x = 2.0f * (Q.x * Q.z - Q.w * Q.y), .x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
.y = 2.0f * (Q.y * Q.z + Q.w * Q.x), .y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
}}; // third column of transposed rotation matrix }}; // third column of transposed rotation matrix
// Remove gravity from accelerometer measurement // Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) { switch (ahrs->settings.convention) {
case FusionConventionNwu: case FusionConventionNwu:
case FusionConventionEnu: { case FusionConventionEnu: {
return FusionVectorSubtract(ahrs->accelerometer, gravity); return FusionVectorSubtract(ahrs->accelerometer, gravity);
} }
case FusionConventionNed: { case FusionConventionNed: {
return FusionVectorAdd(ahrs->accelerometer, gravity); return FusionVectorAdd(ahrs->accelerometer, gravity);
} }
} }
return FUSION_VECTOR_ZERO; // avoid compiler warning return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q #undef Q
} }
@@ -443,36 +418,35 @@ FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return Earth acceleration measurement in g. * @return Earth acceleration measurement in g.
*/ */
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
#define A ahrs->accelerometer.axis #define A ahrs->accelerometer.axis
// Calculate accelerometer measurement in the Earth coordinate frame // Calculate accelerometer measurement in the Earth coordinate frame
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x; const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y; const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z; const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y; const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z; const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z; const float qyqz = Q.y * Q.z;
FusionVector accelerometer = {.axis = { FusionVector accelerometer = {.axis = {
.x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z),
.y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z),
.z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z), .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z),
}}; // rotation matrix multiplied with the accelerometer }}; // rotation matrix multiplied with the accelerometer
// Remove gravity from accelerometer measurement // Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) { switch (ahrs->settings.convention) {
case FusionConventionNwu: case FusionConventionNwu:
case FusionConventionEnu: case FusionConventionEnu:
accelerometer.axis.z -= 1.0f; accelerometer.axis.z -= 1.0f;
break; break;
case FusionConventionNed: case FusionConventionNed:
accelerometer.axis.z += 1.0f; accelerometer.axis.z += 1.0f;
break; break;
} }
return accelerometer; return accelerometer;
#undef Q #undef Q
#undef A #undef A
} }
@@ -482,22 +456,18 @@ FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return AHRS algorithm internal states. * @return AHRS algorithm internal states.
*/ */
FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) {
{ const FusionAhrsInternalStates internalStates = {
const FusionAhrsInternalStates internalStates = { .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
.accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), .accelerometerIgnored = ahrs->accelerometerIgnored,
.accelerometerIgnored = ahrs->accelerometerIgnored, .accelerationRecoveryTrigger =
.accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
ahrs->settings.recoveryTriggerPeriod == 0 .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
? 0.0f .magnetometerIgnored = ahrs->magnetometerIgnored,
: (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod, .magneticRecoveryTrigger =
.magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
.magnetometerIgnored = ahrs->magnetometerIgnored, };
.magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 return internalStates;
? 0.0f
: (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
};
return internalStates;
} }
/** /**
@@ -505,15 +475,14 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @return AHRS algorithm flags. * @return AHRS algorithm flags.
*/ */
FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) {
{ const FusionAhrsFlags flags = {
const FusionAhrsFlags flags = { .initialising = ahrs->initialising,
.initialising = ahrs->initialising, .angularRateRecovery = ahrs->angularRateRecovery,
.angularRateRecovery = ahrs->angularRateRecovery, .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
.accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout, .magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
.magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout, };
}; return flags;
return flags;
} }
/** /**
@@ -523,18 +492,17 @@ FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs)
* @param ahrs AHRS algorithm structure. * @param ahrs AHRS algorithm structure.
* @param heading Heading angle in degrees. * @param heading Heading angle in degrees.
*/ */
void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) {
{
#define Q ahrs->quaternion.element #define Q ahrs->quaternion.element
const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z);
const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading));
const FusionQuaternion rotation = {.element = { const FusionQuaternion rotation = {.element = {
.w = cosf(halfYawMinusHeading), .w = cosf(halfYawMinusHeading),
.x = 0.0f, .x = 0.0f,
.y = 0.0f, .y = 0.0f,
.z = -1.0f * sinf(halfYawMinusHeading), .z = -1.0f * sinf(halfYawMinusHeading),
}}; }};
ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
#undef Q #undef Q
} }
src/Fusion/FusionAhrs.h
+36 -37
View File
@@ -22,12 +22,12 @@
* @brief AHRS algorithm settings. * @brief AHRS algorithm settings.
*/ */
typedef struct { typedef struct {
FusionConvention convention; FusionConvention convention;
float gain; float gain;
float gyroscopeRange; float gyroscopeRange;
float accelerationRejection; float accelerationRejection;
float magneticRejection; float magneticRejection;
unsigned int recoveryTriggerPeriod; unsigned int recoveryTriggerPeriod;
} FusionAhrsSettings; } FusionAhrsSettings;
/** /**
@@ -35,43 +35,43 @@ typedef struct {
* must not be accessed by the application. * must not be accessed by the application.
*/ */
typedef struct { typedef struct {
FusionAhrsSettings settings; FusionAhrsSettings settings;
FusionQuaternion quaternion; FusionQuaternion quaternion;
FusionVector accelerometer; FusionVector accelerometer;
bool initialising; bool initialising;
float rampedGain; float rampedGain;
float rampedGainStep; float rampedGainStep;
bool angularRateRecovery; bool angularRateRecovery;
FusionVector halfAccelerometerFeedback; FusionVector halfAccelerometerFeedback;
FusionVector halfMagnetometerFeedback; FusionVector halfMagnetometerFeedback;
bool accelerometerIgnored; bool accelerometerIgnored;
int accelerationRecoveryTrigger; int accelerationRecoveryTrigger;
int accelerationRecoveryTimeout; int accelerationRecoveryTimeout;
bool magnetometerIgnored; bool magnetometerIgnored;
int magneticRecoveryTrigger; int magneticRecoveryTrigger;
int magneticRecoveryTimeout; int magneticRecoveryTimeout;
} FusionAhrs; } FusionAhrs;
/** /**
* @brief AHRS algorithm internal states. * @brief AHRS algorithm internal states.
*/ */
typedef struct { typedef struct {
float accelerationError; float accelerationError;
bool accelerometerIgnored; bool accelerometerIgnored;
float accelerationRecoveryTrigger; float accelerationRecoveryTrigger;
float magneticError; float magneticError;
bool magnetometerIgnored; bool magnetometerIgnored;
float magneticRecoveryTrigger; float magneticRecoveryTrigger;
} FusionAhrsInternalStates; } FusionAhrsInternalStates;
/** /**
* @brief AHRS algorithm flags. * @brief AHRS algorithm flags.
*/ */
typedef struct { typedef struct {
bool initialising; bool initialising;
bool angularRateRecovery; bool angularRateRecovery;
bool accelerationRecovery; bool accelerationRecovery;
bool magneticRecovery; bool magneticRecovery;
} FusionAhrsFlags; } FusionAhrsFlags;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@@ -83,14 +83,13 @@ void FusionAhrsReset(FusionAhrs *const ahrs);
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings);
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer,
const FusionVector magnetometer, const float deltaTime); const float deltaTime);
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime);
const float deltaTime);
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading,
const float heading, const float deltaTime); const float deltaTime);
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);
src/Fusion/FusionAxes.h
+146 -147
View File
@@ -22,30 +22,30 @@
* then alignment is +Y-X+Z. * then alignment is +Y-X+Z.
*/ */
typedef enum { typedef enum {
FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */
FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ FusionAxesAlignmentPXNZPY, /* +X-Z+Y */
FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */
FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ FusionAxesAlignmentPXPZNY, /* +X+Z-Y */
FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */
FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ FusionAxesAlignmentNXPZPY, /* -X+Z+Y */
FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */
FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ FusionAxesAlignmentNXNZNY, /* -X-Z-Y */
FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */
FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ FusionAxesAlignmentPYNZNX, /* +Y-Z-X */
FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */
FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ FusionAxesAlignmentPYPZPX, /* +Y+Z+X */
FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */
FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ FusionAxesAlignmentNYNZPX, /* -Y-Z+X */
FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */
FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ FusionAxesAlignmentNYPZNX, /* -Y+Z-X */
FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ FusionAxesAlignmentPZPYNX, /* +Z+Y-X */
FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ FusionAxesAlignmentPZPXPY, /* +Z+X+Y */
FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ FusionAxesAlignmentPZNYPX, /* +Z-Y+X */
FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ FusionAxesAlignmentPZNXNY, /* +Z-X-Y */
FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ FusionAxesAlignmentNZPYPX, /* -Z+Y+X */
FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ FusionAxesAlignmentNZNXPY, /* -Z-X+Y */
FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ FusionAxesAlignmentNZNYNX, /* -Z-Y-X */
FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ FusionAxesAlignmentNZPXNY, /* -Z+X-Y */
} FusionAxesAlignment; } FusionAxesAlignment;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@@ -57,129 +57,128 @@ typedef enum {
* @param alignment Axes alignment. * @param alignment Axes alignment.
* @return Sensor axes aligned with the body axes. * @return Sensor axes aligned with the body axes.
*/ */
static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) {
{ FusionVector result;
FusionVector result; switch (alignment) {
switch (alignment) { case FusionAxesAlignmentPXPYPZ:
case FusionAxesAlignmentPXPYPZ: break;
break; case FusionAxesAlignmentPXNZPY:
case FusionAxesAlignmentPXNZPY: result.axis.x = +sensor.axis.x;
result.axis.x = +sensor.axis.x; result.axis.y = -sensor.axis.z;
result.axis.y = -sensor.axis.z; result.axis.z = +sensor.axis.y;
result.axis.z = +sensor.axis.y; return result;
return result; case FusionAxesAlignmentPXNYNZ:
case FusionAxesAlignmentPXNYNZ: result.axis.x = +sensor.axis.x;
result.axis.x = +sensor.axis.x; result.axis.y = -sensor.axis.y;
result.axis.y = -sensor.axis.y; result.axis.z = -sensor.axis.z;
result.axis.z = -sensor.axis.z; return result;
return result; case FusionAxesAlignmentPXPZNY:
case FusionAxesAlignmentPXPZNY: result.axis.x = +sensor.axis.x;
result.axis.x = +sensor.axis.x; result.axis.y = +sensor.axis.z;
result.axis.y = +sensor.axis.z; result.axis.z = -sensor.axis.y;
result.axis.z = -sensor.axis.y; return result;
return result; case FusionAxesAlignmentNXPYNZ:
case FusionAxesAlignmentNXPYNZ: result.axis.x = -sensor.axis.x;
result.axis.x = -sensor.axis.x; result.axis.y = +sensor.axis.y;
result.axis.y = +sensor.axis.y; result.axis.z = -sensor.axis.z;
result.axis.z = -sensor.axis.z; return result;
return result; case FusionAxesAlignmentNXPZPY:
case FusionAxesAlignmentNXPZPY: result.axis.x = -sensor.axis.x;
result.axis.x = -sensor.axis.x; result.axis.y = +sensor.axis.z;
result.axis.y = +sensor.axis.z; result.axis.z = +sensor.axis.y;
result.axis.z = +sensor.axis.y; return result;
return result; case FusionAxesAlignmentNXNYPZ:
case FusionAxesAlignmentNXNYPZ: result.axis.x = -sensor.axis.x;
result.axis.x = -sensor.axis.x; result.axis.y = -sensor.axis.y;
result.axis.y = -sensor.axis.y; result.axis.z = +sensor.axis.z;
result.axis.z = +sensor.axis.z; return result;
return result; case FusionAxesAlignmentNXNZNY:
case FusionAxesAlignmentNXNZNY: result.axis.x = -sensor.axis.x;
result.axis.x = -sensor.axis.x; result.axis.y = -sensor.axis.z;
result.axis.y = -sensor.axis.z; result.axis.z = -sensor.axis.y;
result.axis.z = -sensor.axis.y; return result;
return result; case FusionAxesAlignmentPYNXPZ:
case FusionAxesAlignmentPYNXPZ: result.axis.x = +sensor.axis.y;
result.axis.x = +sensor.axis.y; result.axis.y = -sensor.axis.x;
result.axis.y = -sensor.axis.x; result.axis.z = +sensor.axis.z;
result.axis.z = +sensor.axis.z; return result;
return result; case FusionAxesAlignmentPYNZNX:
case FusionAxesAlignmentPYNZNX: result.axis.x = +sensor.axis.y;
result.axis.x = +sensor.axis.y; result.axis.y = -sensor.axis.z;
result.axis.y = -sensor.axis.z; result.axis.z = -sensor.axis.x;
result.axis.z = -sensor.axis.x; return result;
return result; case FusionAxesAlignmentPYPXNZ:
case FusionAxesAlignmentPYPXNZ: result.axis.x = +sensor.axis.y;
result.axis.x = +sensor.axis.y; result.axis.y = +sensor.axis.x;
result.axis.y = +sensor.axis.x; result.axis.z = -sensor.axis.z;
result.axis.z = -sensor.axis.z; return result;
return result; case FusionAxesAlignmentPYPZPX:
case FusionAxesAlignmentPYPZPX: result.axis.x = +sensor.axis.y;
result.axis.x = +sensor.axis.y; result.axis.y = +sensor.axis.z;
result.axis.y = +sensor.axis.z; result.axis.z = +sensor.axis.x;
result.axis.z = +sensor.axis.x; return result;
return result; case FusionAxesAlignmentNYPXPZ:
case FusionAxesAlignmentNYPXPZ: result.axis.x = -sensor.axis.y;
result.axis.x = -sensor.axis.y; result.axis.y = +sensor.axis.x;
result.axis.y = +sensor.axis.x; result.axis.z = +sensor.axis.z;
result.axis.z = +sensor.axis.z; return result;
return result; case FusionAxesAlignmentNYNZPX:
case FusionAxesAlignmentNYNZPX: result.axis.x = -sensor.axis.y;
result.axis.x = -sensor.axis.y; result.axis.y = -sensor.axis.z;
result.axis.y = -sensor.axis.z; result.axis.z = +sensor.axis.x;
result.axis.z = +sensor.axis.x; return result;
return result; case FusionAxesAlignmentNYNXNZ:
case FusionAxesAlignmentNYNXNZ: result.axis.x = -sensor.axis.y;
result.axis.x = -sensor.axis.y; result.axis.y = -sensor.axis.x;
result.axis.y = -sensor.axis.x; result.axis.z = -sensor.axis.z;
result.axis.z = -sensor.axis.z; return result;
return result; case FusionAxesAlignmentNYPZNX:
case FusionAxesAlignmentNYPZNX: result.axis.x = -sensor.axis.y;
result.axis.x = -sensor.axis.y; result.axis.y = +sensor.axis.z;
result.axis.y = +sensor.axis.z; result.axis.z = -sensor.axis.x;
result.axis.z = -sensor.axis.x; return result;
return result; case FusionAxesAlignmentPZPYNX:
case FusionAxesAlignmentPZPYNX: result.axis.x = +sensor.axis.z;
result.axis.x = +sensor.axis.z; result.axis.y = +sensor.axis.y;
result.axis.y = +sensor.axis.y; result.axis.z = -sensor.axis.x;
result.axis.z = -sensor.axis.x; return result;
return result; case FusionAxesAlignmentPZPXPY:
case FusionAxesAlignmentPZPXPY: result.axis.x = +sensor.axis.z;
result.axis.x = +sensor.axis.z; result.axis.y = +sensor.axis.x;
result.axis.y = +sensor.axis.x; result.axis.z = +sensor.axis.y;
result.axis.z = +sensor.axis.y; return result;
return result; case FusionAxesAlignmentPZNYPX:
case FusionAxesAlignmentPZNYPX: result.axis.x = +sensor.axis.z;
result.axis.x = +sensor.axis.z; result.axis.y = -sensor.axis.y;
result.axis.y = -sensor.axis.y; result.axis.z = +sensor.axis.x;
result.axis.z = +sensor.axis.x; return result;
return result; case FusionAxesAlignmentPZNXNY:
case FusionAxesAlignmentPZNXNY: result.axis.x = +sensor.axis.z;
result.axis.x = +sensor.axis.z; result.axis.y = -sensor.axis.x;
result.axis.y = -sensor.axis.x; result.axis.z = -sensor.axis.y;
result.axis.z = -sensor.axis.y; return result;
return result; case FusionAxesAlignmentNZPYPX:
case FusionAxesAlignmentNZPYPX: result.axis.x = -sensor.axis.z;
result.axis.x = -sensor.axis.z; result.axis.y = +sensor.axis.y;
result.axis.y = +sensor.axis.y; result.axis.z = +sensor.axis.x;
result.axis.z = +sensor.axis.x; return result;
return result; case FusionAxesAlignmentNZNXPY:
case FusionAxesAlignmentNZNXPY: result.axis.x = -sensor.axis.z;
result.axis.x = -sensor.axis.z; result.axis.y = -sensor.axis.x;
result.axis.y = -sensor.axis.x; result.axis.z = +sensor.axis.y;
result.axis.z = +sensor.axis.y; return result;
return result; case FusionAxesAlignmentNZNYNX:
case FusionAxesAlignmentNZNYNX: result.axis.x = -sensor.axis.z;
result.axis.x = -sensor.axis.z; result.axis.y = -sensor.axis.y;
result.axis.y = -sensor.axis.y; result.axis.z = -sensor.axis.x;
result.axis.z = -sensor.axis.x; return result;
return result; case FusionAxesAlignmentNZPXNY:
case FusionAxesAlignmentNZPXNY: result.axis.x = -sensor.axis.z;
result.axis.x = -sensor.axis.z; result.axis.y = +sensor.axis.x;
result.axis.y = +sensor.axis.x; result.axis.z = -sensor.axis.y;
result.axis.z = -sensor.axis.y; return result;
return result; }
} return sensor; // avoid compiler warning
return sensor; // avoid compiler warning
} }
#endif #endif
src/Fusion/FusionCalibration.h
+5 -8
View File
@@ -23,11 +23,9 @@
* @param offset Offset. * @param offset Offset.
* @return Calibrated measurement. * @return Calibrated measurement.
*/ */
static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity,
const FusionVector sensitivity, const FusionVector offset) const FusionVector offset) {
{ return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
return FusionMatrixMultiplyVector(misalignment,
FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
} }
/** /**
@@ -38,9 +36,8 @@ static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibr
* @return Calibrated measurement. * @return Calibrated measurement.
*/ */
static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
const FusionVector hardIronOffset) const FusionVector hardIronOffset) {
{ return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
} }
#endif #endif
src/Fusion/FusionCompass.c
+21 -23
View File
@@ -22,29 +22,27 @@
* @param magnetometer Magnetometer measurement in any calibrated units. * @param magnetometer Magnetometer measurement in any calibrated units.
* @return Heading angle in degrees. * @return Heading angle in degrees.
*/ */
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer) {
const FusionVector magnetometer) switch (convention) {
{ case FusionConventionNwu: {
switch (convention) { const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
case FusionConventionNwu: { const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); }
return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); case FusionConventionEnu: {
} const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
case FusionConventionEnu: { const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f);
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x));
const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f); }
return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x)); case FusionConventionNed: {
} const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f);
case FusionConventionNed: { const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f); const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer)); return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up)); }
return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); }
} return 0; // avoid compiler warning
}
return 0; // avoid compiler warning
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
src/Fusion/FusionCompass.h
+1 -2
View File
@@ -17,8 +17,7 @@
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Function declarations // Function declarations
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer);
const FusionVector magnetometer);
#endif #endif
src/Fusion/FusionConvention.h
+3 -3
View File
@@ -14,9 +14,9 @@
* @brief Earth axes convention. * @brief Earth axes convention.
*/ */
typedef enum { typedef enum {
FusionConventionNwu, /* North-West-Up */ FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */ FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */ FusionConventionNed, /* North-East-Down */
} FusionConvention; } FusionConvention;
#endif #endif
src/Fusion/FusionMath.h
+164 -196
View File
@@ -21,27 +21,27 @@
* @brief 3D vector. * @brief 3D vector.
*/ */
typedef union { typedef union {
float array[3]; float array[3];
struct { struct {
float x; float x;
float y; float y;
float z; float z;
} axis; } axis;
} FusionVector; } FusionVector;
/** /**
* @brief Quaternion. * @brief Quaternion.
*/ */
typedef union { typedef union {
float array[4]; float array[4];
struct { struct {
float w; float w;
float x; float x;
float y; float y;
float z; float z;
} element; } element;
} FusionQuaternion; } FusionQuaternion;
/** /**
@@ -49,19 +49,19 @@ typedef union {
* See http://en.wikipedia.org/wiki/Row-major_order * See http://en.wikipedia.org/wiki/Row-major_order
*/ */
typedef union { typedef union {
float array[3][3]; float array[3][3];
struct { struct {
float xx; float xx;
float xy; float xy;
float xz; float xz;
float yx; float yx;
float yy; float yy;
float yz; float yz;
float zx; float zx;
float zy; float zy;
float zz; float zz;
} element; } element;
} FusionMatrix; } FusionMatrix;
/** /**
@@ -69,13 +69,13 @@ typedef union {
* X, Y, and Z respectively. * X, Y, and Z respectively.
*/ */
typedef union { typedef union {
float array[3]; float array[3];
struct { struct {
float roll; float roll;
float pitch; float pitch;
float yaw; float yaw;
} angle; } angle;
} FusionEuler; } FusionEuler;
/** /**
@@ -124,20 +124,14 @@ typedef union {
* @param degrees Degrees. * @param degrees Degrees.
* @return Radians. * @return Radians.
*/ */
static inline float FusionDegreesToRadians(const float degrees) static inline float FusionDegreesToRadians(const float degrees) { return degrees * ((float)M_PI / 180.0f); }
{
return degrees * ((float)M_PI / 180.0f);
}
/** /**
* @brief Converts radians to degrees. * @brief Converts radians to degrees.
* @param radians Radians. * @param radians Radians.
* @return Degrees. * @return Degrees.
*/ */
static inline float FusionRadiansToDegrees(const float radians) static inline float FusionRadiansToDegrees(const float radians) { return radians * (180.0f / (float)M_PI); }
{
return radians * (180.0f / (float)M_PI);
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inline functions - Arc sine // Inline functions - Arc sine
@@ -147,15 +141,14 @@ static inline float FusionRadiansToDegrees(const float radians)
* @param value Value. * @param value Value.
* @return Arc sine of the value. * @return Arc sine of the value.
*/ */
static inline float FusionAsin(const float value) static inline float FusionAsin(const float value) {
{ if (value <= -1.0f) {
if (value <= -1.0f) { return (float)M_PI / -2.0f;
return (float)M_PI / -2.0f; }
} if (value >= 1.0f) {
if (value >= 1.0f) { return (float)M_PI / 2.0f;
return (float)M_PI / 2.0f; }
} return asinf(value);
return asinf(value);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@@ -169,17 +162,16 @@ static inline float FusionAsin(const float value)
* @param x Operand. * @param x Operand.
* @return Reciprocal of the square root of x. * @return Reciprocal of the square root of x.
*/ */
static inline float FusionFastInverseSqrt(const float x) static inline float FusionFastInverseSqrt(const float x) {
{
typedef union { typedef union {
float f; float f;
int32_t i; int32_t i;
} Union32; } Union32;
Union32 union32 = {.f = x}; Union32 union32 = {.f = x};
union32.i = 0x5F1F1412 - (union32.i >> 1); union32.i = 0x5F1F1412 - (union32.i >> 1);
return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
} }
#endif #endif
@@ -192,9 +184,8 @@ static inline float FusionFastInverseSqrt(const float x)
* @param vector Vector. * @param vector Vector.
* @return True if the vector is zero. * @return True if the vector is zero.
*/ */
static inline bool FusionVectorIsZero(const FusionVector vector) static inline bool FusionVectorIsZero(const FusionVector vector) {
{ return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
} }
/** /**
@@ -203,14 +194,13 @@ static inline bool FusionVectorIsZero(const FusionVector vector)
* @param vectorB Vector B. * @param vectorB Vector B.
* @return Sum of two vectors. * @return Sum of two vectors.
*/ */
static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) {
{ const FusionVector result = {.axis = {
const FusionVector result = {.axis = { .x = vectorA.axis.x + vectorB.axis.x,
.x = vectorA.axis.x + vectorB.axis.x, .y = vectorA.axis.y + vectorB.axis.y,
.y = vectorA.axis.y + vectorB.axis.y, .z = vectorA.axis.z + vectorB.axis.z,
.z = vectorA.axis.z + vectorB.axis.z, }};
}}; return result;
return result;
} }
/** /**
@@ -219,14 +209,13 @@ static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const Fus
* @param vectorB Vector B. * @param vectorB Vector B.
* @return Vector B subtracted from vector A. * @return Vector B subtracted from vector A.
*/ */
static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) {
{ const FusionVector result = {.axis = {
const FusionVector result = {.axis = { .x = vectorA.axis.x - vectorB.axis.x,
.x = vectorA.axis.x - vectorB.axis.x, .y = vectorA.axis.y - vectorB.axis.y,
.y = vectorA.axis.y - vectorB.axis.y, .z = vectorA.axis.z - vectorB.axis.z,
.z = vectorA.axis.z - vectorB.axis.z, }};
}}; return result;
return result;
} }
/** /**
@@ -234,10 +223,7 @@ static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, cons
* @param vector Vector. * @param vector Vector.
* @return Sum of the elements. * @return Sum of the elements.
*/ */
static inline float FusionVectorSum(const FusionVector vector) static inline float FusionVectorSum(const FusionVector vector) { return vector.axis.x + vector.axis.y + vector.axis.z; }
{
return vector.axis.x + vector.axis.y + vector.axis.z;
}
/** /**
* @brief Returns the multiplication of a vector by a scalar. * @brief Returns the multiplication of a vector by a scalar.
@@ -245,14 +231,13 @@ static inline float FusionVectorSum(const FusionVector vector)
* @param scalar Scalar. * @param scalar Scalar.
* @return Multiplication of a vector by a scalar. * @return Multiplication of a vector by a scalar.
*/ */
static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) {
{ const FusionVector result = {.axis = {
const FusionVector result = {.axis = { .x = vector.axis.x * scalar,
.x = vector.axis.x * scalar, .y = vector.axis.y * scalar,
.y = vector.axis.y * scalar, .z = vector.axis.z * scalar,
.z = vector.axis.z * scalar, }};
}}; return result;
return result;
} }
/** /**
@@ -261,14 +246,13 @@ static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector,
* @param vectorB Vector B. * @param vectorB Vector B.
* @return Hadamard product. * @return Hadamard product.
*/ */
static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) {
{ const FusionVector result = {.axis = {
const FusionVector result = {.axis = { .x = vectorA.axis.x * vectorB.axis.x,
.x = vectorA.axis.x * vectorB.axis.x, .y = vectorA.axis.y * vectorB.axis.y,
.y = vectorA.axis.y * vectorB.axis.y, .z = vectorA.axis.z * vectorB.axis.z,
.z = vectorA.axis.z * vectorB.axis.z, }};
}}; return result;
return result;
} }
/** /**
@@ -277,16 +261,15 @@ static inline FusionVector FusionVectorHadamardProduct(const FusionVector vector
* @param vectorB Vector B. * @param vectorB Vector B.
* @return Cross product. * @return Cross product.
*/ */
static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) {
{
#define A vectorA.axis #define A vectorA.axis
#define B vectorB.axis #define B vectorB.axis
const FusionVector result = {.axis = { const FusionVector result = {.axis = {
.x = A.y * B.z - A.z * B.y, .x = A.y * B.z - A.z * B.y,
.y = A.z * B.x - A.x * B.z, .y = A.z * B.x - A.x * B.z,
.z = A.x * B.y - A.y * B.x, .z = A.x * B.y - A.y * B.x,
}}; }};
return result; return result;
#undef A #undef A
#undef B #undef B
} }
@@ -297,9 +280,8 @@ static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA,
* @param vectorB Vector B. * @param vectorB Vector B.
* @return Dot product. * @return Dot product.
*/ */
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) {
{ return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
} }
/** /**
@@ -307,34 +289,27 @@ static inline float FusionVectorDotProduct(const FusionVector vectorA, const Fus
* @param vector Vector. * @param vector Vector.
* @return Vector magnitude squared. * @return Vector magnitude squared.
*/ */
static inline float FusionVectorMagnitudeSquared(const FusionVector vector) static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); }
{
return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
}
/** /**
* @brief Returns the vector magnitude. * @brief Returns the vector magnitude.
* @param vector Vector. * @param vector Vector.
* @return Vector magnitude. * @return Vector magnitude.
*/ */
static inline float FusionVectorMagnitude(const FusionVector vector) static inline float FusionVectorMagnitude(const FusionVector vector) { return sqrtf(FusionVectorMagnitudeSquared(vector)); }
{
return sqrtf(FusionVectorMagnitudeSquared(vector));
}
/** /**
* @brief Returns the normalised vector. * @brief Returns the normalised vector.
* @param vector Vector. * @param vector Vector.
* @return Normalised vector. * @return Normalised vector.
*/ */
static inline FusionVector FusionVectorNormalise(const FusionVector vector) static inline FusionVector FusionVectorNormalise(const FusionVector vector) {
{
#ifdef FUSION_USE_NORMAL_SQRT #ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
#else #else
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
#endif #endif
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@@ -346,15 +321,14 @@ static inline FusionVector FusionVectorNormalise(const FusionVector vector)
* @param quaternionB Quaternion B. * @param quaternionB Quaternion B.
* @return Sum of two quaternions. * @return Sum of two quaternions.
*/ */
static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
{ const FusionQuaternion result = {.element = {
const FusionQuaternion result = {.element = { .w = quaternionA.element.w + quaternionB.element.w,
.w = quaternionA.element.w + quaternionB.element.w, .x = quaternionA.element.x + quaternionB.element.x,
.x = quaternionA.element.x + quaternionB.element.x, .y = quaternionA.element.y + quaternionB.element.y,
.y = quaternionA.element.y + quaternionB.element.y, .z = quaternionA.element.z + quaternionB.element.z,
.z = quaternionA.element.z + quaternionB.element.z, }};
}}; return result;
return result;
} }
/** /**
@@ -363,17 +337,16 @@ static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quater
* @param quaternionB Quaternion B (to be pre-multiplied). * @param quaternionB Quaternion B (to be pre-multiplied).
* @return Multiplication of two quaternions. * @return Multiplication of two quaternions.
*/ */
static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
{
#define A quaternionA.element #define A quaternionA.element
#define B quaternionB.element #define B quaternionB.element
const FusionQuaternion result = {.element = { const FusionQuaternion result = {.element = {
.w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z,
.x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y,
.y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x,
.z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w,
}}; }};
return result; return result;
#undef A #undef A
#undef B #undef B
} }
@@ -387,17 +360,16 @@ static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion q
* @param vector Vector. * @param vector Vector.
* @return Multiplication of a quaternion with a vector. * @return Multiplication of a quaternion with a vector.
*/ */
static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) {
{
#define Q quaternion.element #define Q quaternion.element
#define V vector.axis #define V vector.axis
const FusionQuaternion result = {.element = { const FusionQuaternion result = {.element = {
.w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z,
.x = Q.w * V.x + Q.y * V.z - Q.z * V.y, .x = Q.w * V.x + Q.y * V.z - Q.z * V.y,
.y = Q.w * V.y - Q.x * V.z + Q.z * V.x, .y = Q.w * V.y - Q.x * V.z + Q.z * V.x,
.z = Q.w * V.z + Q.x * V.y - Q.y * V.x, .z = Q.w * V.z + Q.x * V.y - Q.y * V.x,
}}; }};
return result; return result;
#undef Q #undef Q
#undef V #undef V
} }
@@ -407,21 +379,20 @@ static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuater
* @param quaternion Quaternion. * @param quaternion Quaternion.
* @return Normalised quaternion. * @return Normalised quaternion.
*/ */
static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) {
{
#define Q quaternion.element #define Q quaternion.element
#ifdef FUSION_USE_NORMAL_SQRT #ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
#else #else
const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z);
#endif #endif
const FusionQuaternion result = {.element = { const FusionQuaternion result = {.element = {
.w = Q.w * magnitudeReciprocal, .w = Q.w * magnitudeReciprocal,
.x = Q.x * magnitudeReciprocal, .x = Q.x * magnitudeReciprocal,
.y = Q.y * magnitudeReciprocal, .y = Q.y * magnitudeReciprocal,
.z = Q.z * magnitudeReciprocal, .z = Q.z * magnitudeReciprocal,
}}; }};
return result; return result;
#undef Q #undef Q
} }
@@ -434,15 +405,14 @@ static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion
* @param vector Vector. * @param vector Vector.
* @return Multiplication of a matrix with a vector. * @return Multiplication of a matrix with a vector.
*/ */
static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) {
{
#define R matrix.element #define R matrix.element
const FusionVector result = {.axis = { const FusionVector result = {.axis = {
.x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z,
.y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z,
.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z,
}}; }};
return result; return result;
#undef R #undef R
} }
@@ -454,28 +424,27 @@ static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix,
* @param quaternion Quaternion. * @param quaternion Quaternion.
* @return Rotation matrix. * @return Rotation matrix.
*/ */
static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) {
{
#define Q quaternion.element #define Q quaternion.element
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x; const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y; const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z; const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y; const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z; const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z; const float qyqz = Q.y * Q.z;
const FusionMatrix matrix = {.element = { const FusionMatrix matrix = {.element = {
.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x),
.xy = 2.0f * (qxqy - qwqz), .xy = 2.0f * (qxqy - qwqz),
.xz = 2.0f * (qxqz + qwqy), .xz = 2.0f * (qxqz + qwqy),
.yx = 2.0f * (qxqy + qwqz), .yx = 2.0f * (qxqy + qwqz),
.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y),
.yz = 2.0f * (qyqz - qwqx), .yz = 2.0f * (qyqz - qwqx),
.zx = 2.0f * (qxqz - qwqy), .zx = 2.0f * (qxqz - qwqy),
.zy = 2.0f * (qyqz + qwqx), .zy = 2.0f * (qyqz + qwqx),
.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z),
}}; }};
return matrix; return matrix;
#undef Q #undef Q
} }
@@ -484,16 +453,15 @@ static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quate
* @param quaternion Quaternion. * @param quaternion Quaternion.
* @return Euler angles in degrees. * @return Euler angles in degrees.
*/ */
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {
{
#define Q quaternion.element #define Q quaternion.element
const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
const FusionEuler euler = {.angle = { const FusionEuler euler = {.angle = {
.roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)),
.pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))),
.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)),
}}; }};
return euler; return euler;
#undef Q #undef Q
} }
src/Fusion/FusionOffset.c
+22 -25
View File
@@ -37,12 +37,11 @@
* @param offset Gyroscope offset algorithm structure. * @param offset Gyroscope offset algorithm structure.
* @param sampleRate Sample rate in Hz. * @param sampleRate Sample rate in Hz.
*/ */
void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) {
{ offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate);
offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate); offset->timeout = TIMEOUT * sampleRate;
offset->timeout = TIMEOUT * sampleRate; offset->timer = 0;
offset->timer = 0; offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
} }
/** /**
@@ -52,28 +51,26 @@ void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampl
* @param gyroscope Gyroscope measurement in degrees per second. * @param gyroscope Gyroscope measurement in degrees per second.
* @return Corrected gyroscope measurement in degrees per second. * @return Corrected gyroscope measurement in degrees per second.
*/ */
FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) {
{
// Subtract offset from gyroscope measurement // Subtract offset from gyroscope measurement
gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
// Reset timer if gyroscope not stationary // Reset timer if gyroscope not stationary
if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) { if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) {
offset->timer = 0; offset->timer = 0;
return gyroscope;
}
// Increment timer while gyroscope stationary
if (offset->timer < offset->timeout) {
offset->timer++;
return gyroscope;
}
// Adjust offset if timer has elapsed
offset->gyroscopeOffset =
FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient));
return gyroscope; return gyroscope;
}
// Increment timer while gyroscope stationary
if (offset->timer < offset->timeout) {
offset->timer++;
return gyroscope;
}
// Adjust offset if timer has elapsed
offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient));
return gyroscope;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
src/Fusion/FusionOffset.h
+4 -4
View File
@@ -21,10 +21,10 @@
* internally and must not be accessed by the application. * internally and must not be accessed by the application.
*/ */
typedef struct { typedef struct {
float filterCoefficient; float filterCoefficient;
unsigned int timeout; unsigned int timeout;
unsigned int timer; unsigned int timer;
FusionVector gyroscopeOffset; FusionVector gyroscopeOffset;
} FusionOffset; } FusionOffset;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
src/GPSStatus.h
+90 -102
View File
@@ -4,136 +4,124 @@
#include "configuration.h" #include "configuration.h"
#include <Arduino.h> #include <Arduino.h>
namespace meshtastic namespace meshtastic {
{
/// Describes the state of the GPS system. /// Describes the state of the GPS system.
class GPSStatus : public Status class GPSStatus : public Status {
{
private: private:
CallbackObserver<GPSStatus, const GPSStatus *> statusObserver = CallbackObserver<GPSStatus, const GPSStatus *> statusObserver = CallbackObserver<GPSStatus, const GPSStatus *>(this, &GPSStatus::updateStatus);
CallbackObserver<GPSStatus, const GPSStatus *>(this, &GPSStatus::updateStatus);
bool hasLock = false; // default to false, until we complete our first read bool hasLock = false; // default to false, until we complete our first read
bool isConnected = false; // Do we have a GPS we are talking to bool isConnected = false; // Do we have a GPS we are talking to
bool isPowerSaving = false; // Are we in power saving state bool isPowerSaving = false; // Are we in power saving state
meshtastic_Position p = meshtastic_Position_init_default; meshtastic_Position p = meshtastic_Position_init_default;
/// Time of last valid GPS fix (millis since boot) /// Time of last valid GPS fix (millis since boot)
uint32_t lastFixMillis = 0; uint32_t lastFixMillis = 0;
public: public:
GPSStatus() { statusType = STATUS_TYPE_GPS; } GPSStatus() { statusType = STATUS_TYPE_GPS; }
// preferred method // preferred method
GPSStatus(bool hasLock, bool isConnected, bool isPowerSaving, const meshtastic_Position &pos) : Status() GPSStatus(bool hasLock, bool isConnected, bool isPowerSaving, const meshtastic_Position &pos) : Status() {
{ this->hasLock = hasLock;
this->hasLock = hasLock; this->isConnected = isConnected;
this->isConnected = isConnected; this->isPowerSaving = isPowerSaving;
this->isPowerSaving = isPowerSaving;
// all-in-one struct copy // all-in-one struct copy
this->p = pos; this->p = pos;
}
GPSStatus(const GPSStatus &);
GPSStatus &operator=(const GPSStatus &);
void observe(Observable<const GPSStatus *> *source) { statusObserver.observe(source); }
bool getHasLock() const { return hasLock; }
bool getIsConnected() const { return isConnected; }
bool getIsPowerSaving() const { return isPowerSaving; }
int32_t getLatitude() const {
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.latitude_i;
} else {
return p.latitude_i;
} }
}
GPSStatus(const GPSStatus &); int32_t getLongitude() const {
GPSStatus &operator=(const GPSStatus &); if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
void observe(Observable<const GPSStatus *> *source) { statusObserver.observe(source); } return node->position.longitude_i;
} else {
bool getHasLock() const { return hasLock; } return p.longitude_i;
bool getIsConnected() const { return isConnected; }
bool getIsPowerSaving() const { return isPowerSaving; }
int32_t getLatitude() const
{
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.latitude_i;
} else {
return p.latitude_i;
}
} }
}
int32_t getLongitude() const int32_t getAltitude() const {
{ if (config.position.fixed_position) {
if (config.position.fixed_position) { meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum()); return node->position.altitude;
return node->position.longitude_i; } else {
} else { return p.altitude;
return p.longitude_i;
}
} }
}
int32_t getAltitude() const uint32_t getDOP() const { return p.PDOP; }
{
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.altitude;
} else {
return p.altitude;
}
}
uint32_t getDOP() const { return p.PDOP; } uint32_t getHeading() const { return p.ground_track; }
uint32_t getHeading() const { return p.ground_track; } uint32_t getNumSatellites() const { return p.sats_in_view; }
uint32_t getNumSatellites() const { return p.sats_in_view; } /// Return millis() when the last GPS fix occurred (0 = never)
uint32_t getLastFixMillis() const { return lastFixMillis; }
/// Return millis() when the last GPS fix occurred (0 = never) bool matches(const GPSStatus *newStatus) const {
uint32_t getLastFixMillis() const { return lastFixMillis; }
bool matches(const GPSStatus *newStatus) const
{
#ifdef GPS_DEBUG #ifdef GPS_DEBUG
LOG_DEBUG("GPSStatus.match() new pos@%x to old pos@%x", newStatus->p.timestamp, p.timestamp); LOG_DEBUG("GPSStatus.match() new pos@%x to old pos@%x", newStatus->p.timestamp, p.timestamp);
#endif #endif
return (newStatus->hasLock != hasLock || newStatus->isConnected != isConnected || return (newStatus->hasLock != hasLock || newStatus->isConnected != isConnected || newStatus->isPowerSaving != isPowerSaving ||
newStatus->isPowerSaving != isPowerSaving || newStatus->p.latitude_i != p.latitude_i || newStatus->p.latitude_i != p.latitude_i || newStatus->p.longitude_i != p.longitude_i || newStatus->p.altitude != p.altitude ||
newStatus->p.longitude_i != p.longitude_i || newStatus->p.altitude != p.altitude || newStatus->p.altitude_hae != p.altitude_hae || newStatus->p.PDOP != p.PDOP || newStatus->p.ground_track != p.ground_track ||
newStatus->p.altitude_hae != p.altitude_hae || newStatus->p.PDOP != p.PDOP || newStatus->p.ground_speed != p.ground_speed || newStatus->p.sats_in_view != p.sats_in_view);
newStatus->p.ground_track != p.ground_track || newStatus->p.ground_speed != p.ground_speed || }
newStatus->p.sats_in_view != p.sats_in_view);
int updateStatus(const GPSStatus *newStatus) {
// Only update the status if values have actually changed
bool isDirty = matches(newStatus);
if (isDirty && p.timestamp && (newStatus->p.timestamp == p.timestamp)) {
// We can NEVER be in two locations at the same time! (also PR #886)
LOG_ERROR("BUG: Positional timestamp unchanged from prev solution");
} }
int updateStatus(const GPSStatus *newStatus) initialized = true;
{ hasLock = newStatus->hasLock;
// Only update the status if values have actually changed isConnected = newStatus->isConnected;
bool isDirty = matches(newStatus);
if (isDirty && p.timestamp && (newStatus->p.timestamp == p.timestamp)) { p = newStatus->p;
// We can NEVER be in two locations at the same time! (also PR #886)
LOG_ERROR("BUG: Positional timestamp unchanged from prev solution");
}
initialized = true; if (isDirty) {
hasLock = newStatus->hasLock; if (hasLock) {
isConnected = newStatus->isConnected; // Record time of last valid GPS fix
lastFixMillis = millis();
p = newStatus->p; // In debug logs, identify position by @timestamp:stage (stage 3 = notify)
LOG_DEBUG("New GPS pos@%x:3 lat=%f lon=%f alt=%d pdop=%.2f track=%.2f speed=%.2f sats=%d", p.timestamp, p.latitude_i * 1e-7,
if (isDirty) { p.longitude_i * 1e-7, p.altitude, p.PDOP * 1e-2, p.ground_track * 1e-5, p.ground_speed * 1e-2, p.sats_in_view);
if (hasLock) { } else {
// Record time of last valid GPS fix LOG_DEBUG("No GPS lock");
lastFixMillis = millis(); }
onNewStatus.notifyObservers(this);
// In debug logs, identify position by @timestamp:stage (stage 3 = notify)
LOG_DEBUG("New GPS pos@%x:3 lat=%f lon=%f alt=%d pdop=%.2f track=%.2f speed=%.2f sats=%d", p.timestamp,
p.latitude_i * 1e-7, p.longitude_i * 1e-7, p.altitude, p.PDOP * 1e-2, p.ground_track * 1e-5,
p.ground_speed * 1e-2, p.sats_in_view);
} else {
LOG_DEBUG("No GPS lock");
}
onNewStatus.notifyObservers(this);
}
return 0;
} }
return 0;
}
}; };
} // namespace meshtastic } // namespace meshtastic
src/GpioLogic.cpp
+55 -67
View File
@@ -1,102 +1,90 @@
#include "GpioLogic.h" #include "GpioLogic.h"
#include <assert.h> #include <assert.h>
void GpioVirtPin::set(bool value) void GpioVirtPin::set(bool value) {
{ if (value != this->value) {
if (value != this->value) { this->value = value ? PinState::On : PinState::Off;
this->value = value ? PinState::On : PinState::Off; if (dependentPin)
if (dependentPin) dependentPin->update();
dependentPin->update(); }
}
} }
void GpioHwPin::set(bool value) void GpioHwPin::set(bool value) {
{ pinMode(num, OUTPUT);
pinMode(num, OUTPUT); digitalWrite(num, value);
digitalWrite(num, value);
} }
GpioTransformer::GpioTransformer(GpioPin *outPin) : outPin(outPin) {} GpioTransformer::GpioTransformer(GpioPin *outPin) : outPin(outPin) {}
void GpioTransformer::set(bool value) void GpioTransformer::set(bool value) { outPin->set(value); }
{
outPin->set(value);
}
GpioUnaryTransformer::GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioTransformer(outPin), inPin(inPin) GpioUnaryTransformer::GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioTransformer(outPin), inPin(inPin) {
{ assert(!inPin->dependentPin); // We only allow one dependent pin
assert(!inPin->dependentPin); // We only allow one dependent pin inPin->dependentPin = this;
inPin->dependentPin = this;
// Don't update at construction time, because various GpioPins might be global constructor based not yet initied because // Don't update at construction time, because various GpioPins might be global constructor based not yet initied
// order of operations for global constructors is not defined. // because order of operations for global constructors is not defined. update();
// update();
} }
/** /**
* Update the output pin based on the current state of the input pin. * Update the output pin based on the current state of the input pin.
*/ */
void GpioUnaryTransformer::update() void GpioUnaryTransformer::update() {
{ auto p = inPin->get();
auto p = inPin->get(); if (p == GpioVirtPin::PinState::Unset)
if (p == GpioVirtPin::PinState::Unset) return; // Not yet fully initialized
return; // Not yet fully initialized
set(p); set(p);
} }
/** /**
* Update the output pin based on the current state of the input pin. * Update the output pin based on the current state of the input pin.
*/ */
void GpioNotTransformer::update() void GpioNotTransformer::update() {
{ auto p = inPin->get();
auto p = inPin->get(); if (p == GpioVirtPin::PinState::Unset)
if (p == GpioVirtPin::PinState::Unset) return; // Not yet fully initialized
return; // Not yet fully initialized
set(!p); set(!p);
} }
GpioBinaryTransformer::GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation) GpioBinaryTransformer::GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation)
: GpioTransformer(outPin), inPin1(inPin1), inPin2(inPin2), operation(operation) : GpioTransformer(outPin), inPin1(inPin1), inPin2(inPin2), operation(operation) {
{ assert(!inPin1->dependentPin); // We only allow one dependent pin
assert(!inPin1->dependentPin); // We only allow one dependent pin inPin1->dependentPin = this;
inPin1->dependentPin = this; assert(!inPin2->dependentPin); // We only allow one dependent pin
assert(!inPin2->dependentPin); // We only allow one dependent pin inPin2->dependentPin = this;
inPin2->dependentPin = this;
// Don't update at construction time, because various GpioPins might be global constructor based not yet initiated because // Don't update at construction time, because various GpioPins might be global constructor based not yet initiated
// order of operations for global constructors is not defined. // because order of operations for global constructors is not defined. update();
// update();
} }
void GpioBinaryTransformer::update() void GpioBinaryTransformer::update() {
{ auto p1 = inPin1->get(), p2 = inPin2->get();
auto p1 = inPin1->get(), p2 = inPin2->get(); GpioVirtPin::PinState newValue = GpioVirtPin::PinState::Unset;
GpioVirtPin::PinState newValue = GpioVirtPin::PinState::Unset;
if (p1 == GpioVirtPin::PinState::Unset) if (p1 == GpioVirtPin::PinState::Unset)
newValue = p2; // Not yet fully initialized newValue = p2; // Not yet fully initialized
else if (p2 == GpioVirtPin::PinState::Unset) else if (p2 == GpioVirtPin::PinState::Unset)
newValue = p1; // Not yet fully initialized newValue = p1; // Not yet fully initialized
// If we've already found our value just use it, otherwise need to do the operation // If we've already found our value just use it, otherwise need to do the operation
if (newValue == GpioVirtPin::PinState::Unset) { if (newValue == GpioVirtPin::PinState::Unset) {
switch (operation) { switch (operation) {
case And: case And:
newValue = (GpioVirtPin::PinState)(p1 && p2); newValue = (GpioVirtPin::PinState)(p1 && p2);
break; break;
case Or: case Or:
newValue = (GpioVirtPin::PinState)(p1 || p2); newValue = (GpioVirtPin::PinState)(p1 || p2);
break; break;
case Xor: case Xor:
newValue = (GpioVirtPin::PinState)(p1 != p2); newValue = (GpioVirtPin::PinState)(p1 != p2);
break; break;
default: default:
assert(false); assert(false);
}
} }
set(newValue); }
set(newValue);
} }
GpioSplitter::GpioSplitter(GpioPin *outPin1, GpioPin *outPin2) : outPin1(outPin1), outPin2(outPin2) {} GpioSplitter::GpioSplitter(GpioPin *outPin1, GpioPin *outPin2) : outPin1(outPin1), outPin2(outPin2) {}
src/GpioLogic.h
+77 -85
View File
@@ -3,8 +3,9 @@
#include "configuration.h" #include "configuration.h"
/**This is a set of classes to mediate access to GPIOs in a structured way. Most usage of GPIOs do not /**This is a set of classes to mediate access to GPIOs in a structured way. Most usage of GPIOs do not
require these classes! But if your hardware has a GPIO that is 'shared' between multiple devices (i.e. a shared power enable) require these classes! But if your hardware has a GPIO that is 'shared' between multiple devices (i.e. a shared
then using these classes might be able to let you cleanly turn on that enable when either dependent device is needed. power enable) then using these classes might be able to let you cleanly turn on that enable when either dependent
device is needed.
Note: these classes are intended to be 99% inline for the common case so should have minimal impact on flash or RAM Note: these classes are intended to be 99% inline for the common case so should have minimal impact on flash or RAM
requirements. requirements.
@@ -13,23 +14,21 @@
/** /**
* A logical GPIO pin (not necessary raw hardware). * A logical GPIO pin (not necessary raw hardware).
*/ */
class GpioPin class GpioPin {
{ public:
public: virtual void set(bool value) = 0;
virtual void set(bool value) = 0;
}; };
/** /**
* A physical GPIO hw pin. * A physical GPIO hw pin.
*/ */
class GpioHwPin : public GpioPin class GpioHwPin : public GpioPin {
{ uint32_t num;
uint32_t num;
public: public:
explicit GpioHwPin(uint32_t num) : num(num) {} explicit GpioHwPin(uint32_t num) : num(num) {}
void set(bool value); void set(bool value);
}; };
class GpioTransformer; class GpioTransformer;
@@ -39,122 +38,115 @@ class GpioBinaryTransformer;
/** /**
* A virtual GPIO pin. * A virtual GPIO pin.
*/ */
class GpioVirtPin : public GpioPin class GpioVirtPin : public GpioPin {
{ friend class GpioBinaryTransformer;
friend class GpioBinaryTransformer; friend class GpioUnaryTransformer;
friend class GpioUnaryTransformer;
public: public:
enum PinState { On = true, Off = false, Unset = 2 }; enum PinState { On = true, Off = false, Unset = 2 };
void set(bool value); void set(bool value);
PinState get() const { return value; } PinState get() const { return value; }
private: private:
PinState value = PinState::Unset; PinState value = PinState::Unset;
GpioTransformer *dependentPin = NULL; GpioTransformer *dependentPin = NULL;
}; };
#include <assert.h> #include <assert.h>
/** /**
* A 'smart' trigger that can depend in a fake GPIO and if that GPIO changes, drive some other downstream GPIO to change. * A 'smart' trigger that can depend in a fake GPIO and if that GPIO changes, drive some other downstream GPIO to
* notably: the set method is not public (because it always is calculated by a subclass) * change. notably: the set method is not public (because it always is calculated by a subclass)
*/ */
class GpioTransformer class GpioTransformer {
{ public:
public: /**
/** * Update the output pin based on the current state of the input pin.
* Update the output pin based on the current state of the input pin. */
*/ virtual void update() = 0;
virtual void update() = 0;
protected: protected:
GpioTransformer(GpioPin *outPin); GpioTransformer(GpioPin *outPin);
void set(bool value); void set(bool value);
private: private:
GpioPin *outPin; GpioPin *outPin;
}; };
/** /**
* A transformer that just drives a hw pin based on a virtual pin. * A transformer that just drives a hw pin based on a virtual pin.
*/ */
class GpioUnaryTransformer : public GpioTransformer class GpioUnaryTransformer : public GpioTransformer {
{ public:
public: GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin);
GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin);
protected: protected:
friend class GpioVirtPin; friend class GpioVirtPin;
/** /**
* Update the output pin based on the current state of the input pin. * Update the output pin based on the current state of the input pin.
*/ */
virtual void update(); virtual void update();
GpioVirtPin *inPin; GpioVirtPin *inPin;
}; };
/** /**
* A transformer that performs a unary NOT operation from an input. * A transformer that performs a unary NOT operation from an input.
*/ */
class GpioNotTransformer : public GpioUnaryTransformer class GpioNotTransformer : public GpioUnaryTransformer {
{ public:
public: GpioNotTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioUnaryTransformer(inPin, outPin) {}
GpioNotTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioUnaryTransformer(inPin, outPin) {}
protected: protected:
friend class GpioVirtPin; friend class GpioVirtPin;
/** /**
* Update the output pin based on the current state of the input pin. * Update the output pin based on the current state of the input pin.
*/ */
void update(); void update();
}; };
/** /**
* A transformer that combines multiple virtual pins to drive an output pin * A transformer that combines multiple virtual pins to drive an output pin
*/ */
class GpioBinaryTransformer : public GpioTransformer class GpioBinaryTransformer : public GpioTransformer {
{
public: public:
enum Operation { And, Or, Xor }; enum Operation { And, Or, Xor };
GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation); GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation);
protected: protected:
friend class GpioVirtPin; friend class GpioVirtPin;
/** /**
* Update the output pin based on the current state of the input pins. * Update the output pin based on the current state of the input pins.
*/ */
void update(); void update();
private: private:
GpioVirtPin *inPin1; GpioVirtPin *inPin1;
GpioVirtPin *inPin2; GpioVirtPin *inPin2;
Operation operation; Operation operation;
}; };
/** /**
* Sometimes a single output GPIO single needs to drive multiple physical GPIOs. This class provides that. * Sometimes a single output GPIO single needs to drive multiple physical GPIOs. This class provides that.
*/ */
class GpioSplitter : public GpioPin class GpioSplitter : public GpioPin {
{
public: public:
GpioSplitter(GpioPin *outPin1, GpioPin *outPin2); GpioSplitter(GpioPin *outPin1, GpioPin *outPin2);
void set(bool value) void set(bool value) {
{ outPin1->set(value);
outPin1->set(value); outPin2->set(value);
outPin2->set(value); }
}
private: private:
GpioPin *outPin1; GpioPin *outPin1;
GpioPin *outPin2; GpioPin *outPin2;
}; };
src/Led.cpp
+17 -21
View File
@@ -23,16 +23,14 @@ static GpioPin &ledHwPin = ledRawHwPin;
/** /**
* A GPIO controlled by the PMU * A GPIO controlled by the PMU
*/ */
class GpioPmuPin : public GpioPin class GpioPmuPin : public GpioPin {
{ public:
public: void set(bool value) {
void set(bool value) if (pmu_found && PMU) {
{ // blink the axp led
if (pmu_found && PMU) { PMU->setChargingLedMode(value ? XPOWERS_CHG_LED_ON : XPOWERS_CHG_LED_OFF);
// blink the axp led
PMU->setChargingLedMode(value ? XPOWERS_CHG_LED_ON : XPOWERS_CHG_LED_OFF);
}
} }
}
} ledPmuHwPin; } ledPmuHwPin;
// In some cases we need to drive a PMU LED and a normal LED // In some cases we need to drive a PMU LED and a normal LED
@@ -45,19 +43,17 @@ static GpioPin &ledFinalPin = ledHwPin;
/** /**
* We monitor changes to the LED drive output because we use that as a sanity test in our power monitor stuff. * We monitor changes to the LED drive output because we use that as a sanity test in our power monitor stuff.
*/ */
class MonitoredLedPin : public GpioPin class MonitoredLedPin : public GpioPin {
{ public:
public: void set(bool value) {
void set(bool value) if (powerMon) {
{ if (value)
if (powerMon) { powerMon->setState(meshtastic_PowerMon_State_LED_On);
if (value) else
powerMon->setState(meshtastic_PowerMon_State_LED_On); powerMon->clearState(meshtastic_PowerMon_State_LED_On);
else
powerMon->clearState(meshtastic_PowerMon_State_LED_On);
}
ledFinalPin.set(value);
} }
ledFinalPin.set(value);
}
} monitoredLedPin; } monitoredLedPin;
#else #else
static GpioPin &monitoredLedPin = ledFinalPin; static GpioPin &monitoredLedPin = ledFinalPin;
src/MessageStore.cpp
+249 -280
View File
@@ -18,257 +18,238 @@ static char *g_messagePool = nullptr;
static size_t g_poolWritePos = 0; static size_t g_poolWritePos = 0;
// Reset pool (called on boot or clear) // Reset pool (called on boot or clear)
static inline void resetMessagePool() static inline void resetMessagePool() {
{ if (!g_messagePool) {
g_messagePool = static_cast<char *>(malloc(MESSAGE_TEXT_POOL_SIZE));
if (!g_messagePool) { if (!g_messagePool) {
g_messagePool = static_cast<char *>(malloc(MESSAGE_TEXT_POOL_SIZE)); LOG_ERROR("MessageStore: Failed to allocate %d bytes for message pool", MESSAGE_TEXT_POOL_SIZE);
if (!g_messagePool) { return;
LOG_ERROR("MessageStore: Failed to allocate %d bytes for message pool", MESSAGE_TEXT_POOL_SIZE);
return;
}
} }
g_poolWritePos = 0; }
memset(g_messagePool, 0, MESSAGE_TEXT_POOL_SIZE); g_poolWritePos = 0;
memset(g_messagePool, 0, MESSAGE_TEXT_POOL_SIZE);
} }
// Allocate text in pool and return offset // Allocate text in pool and return offset
// If not enough space remains, wrap around (ring buffer style) // If not enough space remains, wrap around (ring buffer style)
static inline uint16_t storeTextInPool(const char *src, size_t len) static inline uint16_t storeTextInPool(const char *src, size_t len) {
{ if (len >= MAX_MESSAGE_SIZE)
if (len >= MAX_MESSAGE_SIZE) len = MAX_MESSAGE_SIZE - 1;
len = MAX_MESSAGE_SIZE - 1;
// Wrap pool if out of space // Wrap pool if out of space
if (g_poolWritePos + len + 1 >= MESSAGE_TEXT_POOL_SIZE) { if (g_poolWritePos + len + 1 >= MESSAGE_TEXT_POOL_SIZE) {
g_poolWritePos = 0; g_poolWritePos = 0;
} }
uint16_t offset = g_poolWritePos; uint16_t offset = g_poolWritePos;
memcpy(&g_messagePool[g_poolWritePos], src, len); memcpy(&g_messagePool[g_poolWritePos], src, len);
g_messagePool[g_poolWritePos + len] = '\0'; g_messagePool[g_poolWritePos + len] = '\0';
g_poolWritePos += (len + 1); g_poolWritePos += (len + 1);
return offset; return offset;
} }
// Retrieve a const pointer to message text by offset // Retrieve a const pointer to message text by offset
static inline const char *getTextFromPool(uint16_t offset) static inline const char *getTextFromPool(uint16_t offset) {
{ if (!g_messagePool || offset >= MESSAGE_TEXT_POOL_SIZE)
if (!g_messagePool || offset >= MESSAGE_TEXT_POOL_SIZE) return "";
return ""; return &g_messagePool[offset];
return &g_messagePool[offset];
} }
// Helper: assign a timestamp (RTC if available, else boot-relative) // Helper: assign a timestamp (RTC if available, else boot-relative)
static inline void assignTimestamp(StoredMessage &sm) static inline void assignTimestamp(StoredMessage &sm) {
{ uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true);
uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true); if (nowSecs) {
if (nowSecs) { sm.timestamp = nowSecs;
sm.timestamp = nowSecs; sm.isBootRelative = false;
sm.isBootRelative = false; } else {
} else { sm.timestamp = millis() / 1000;
sm.timestamp = millis() / 1000; sm.isBootRelative = true;
sm.isBootRelative = true; }
}
} }
// Generic push with cap (used by live + persisted queues) // Generic push with cap (used by live + persisted queues)
template <typename T> static inline void pushWithLimit(std::deque<T> &queue, const T &msg) template <typename T> static inline void pushWithLimit(std::deque<T> &queue, const T &msg) {
{ if (queue.size() >= MAX_MESSAGES_SAVED)
if (queue.size() >= MAX_MESSAGES_SAVED) queue.pop_front();
queue.pop_front(); queue.push_back(msg);
queue.push_back(msg);
} }
template <typename T> static inline void pushWithLimit(std::deque<T> &queue, T &&msg) template <typename T> static inline void pushWithLimit(std::deque<T> &queue, T &&msg) {
{ if (queue.size() >= MAX_MESSAGES_SAVED)
if (queue.size() >= MAX_MESSAGES_SAVED) queue.pop_front();
queue.pop_front(); queue.emplace_back(std::move(msg));
queue.emplace_back(std::move(msg));
} }
MessageStore::MessageStore(const std::string &label) MessageStore::MessageStore(const std::string &label) {
{ filename = "/Messages_" + label + ".msgs";
filename = "/Messages_" + label + ".msgs"; resetMessagePool(); // initialize text pool on boot
resetMessagePool(); // initialize text pool on boot
} }
// Live message handling (RAM only) // Live message handling (RAM only)
void MessageStore::addLiveMessage(StoredMessage &&msg) void MessageStore::addLiveMessage(StoredMessage &&msg) { pushWithLimit(liveMessages, std::move(msg)); }
{ void MessageStore::addLiveMessage(const StoredMessage &msg) { pushWithLimit(liveMessages, msg); }
pushWithLimit(liveMessages, std::move(msg));
}
void MessageStore::addLiveMessage(const StoredMessage &msg)
{
pushWithLimit(liveMessages, msg);
}
// Add from incoming/outgoing packet // Add from incoming/outgoing packet
const StoredMessage &MessageStore::addFromPacket(const meshtastic_MeshPacket &packet) const StoredMessage &MessageStore::addFromPacket(const meshtastic_MeshPacket &packet) {
{ StoredMessage sm;
StoredMessage sm; assignTimestamp(sm);
assignTimestamp(sm); sm.channelIndex = packet.channel;
sm.channelIndex = packet.channel;
const char *payload = reinterpret_cast<const char *>(packet.decoded.payload.bytes); const char *payload = reinterpret_cast<const char *>(packet.decoded.payload.bytes);
size_t len = strnlen(payload, MAX_MESSAGE_SIZE - 1); size_t len = strnlen(payload, MAX_MESSAGE_SIZE - 1);
sm.textOffset = storeTextInPool(payload, len); sm.textOffset = storeTextInPool(payload, len);
sm.textLength = len; sm.textLength = len;
// Determine sender // Determine sender
uint32_t localNode = nodeDB->getNodeNum(); uint32_t localNode = nodeDB->getNodeNum();
sm.sender = (packet.from == 0) ? localNode : packet.from; sm.sender = (packet.from == 0) ? localNode : packet.from;
sm.dest = packet.to; sm.dest = packet.to;
bool isDM = (sm.dest != 0 && sm.dest != NODENUM_BROADCAST); bool isDM = (sm.dest != 0 && sm.dest != NODENUM_BROADCAST);
if (packet.from == 0) { if (packet.from == 0) {
sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST; sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST;
sm.ackStatus = AckStatus::NONE; sm.ackStatus = AckStatus::NONE;
} else { } else {
sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST; sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST;
sm.ackStatus = AckStatus::ACKED; sm.ackStatus = AckStatus::ACKED;
} }
addLiveMessage(sm); addLiveMessage(sm);
return liveMessages.back(); return liveMessages.back();
} }
// Outgoing/manual message // Outgoing/manual message
void MessageStore::addFromString(uint32_t sender, uint8_t channelIndex, const std::string &text) void MessageStore::addFromString(uint32_t sender, uint8_t channelIndex, const std::string &text) {
{ StoredMessage sm;
StoredMessage sm;
// Always use our local time (helper handles RTC vs boot time) // Always use our local time (helper handles RTC vs boot time)
assignTimestamp(sm); assignTimestamp(sm);
sm.sender = sender; sm.sender = sender;
sm.channelIndex = channelIndex; sm.channelIndex = channelIndex;
sm.textOffset = storeTextInPool(text.c_str(), text.size()); sm.textOffset = storeTextInPool(text.c_str(), text.size());
sm.textLength = text.size(); sm.textLength = text.size();
// Use the provided destination // Use the provided destination
sm.dest = sender; sm.dest = sender;
sm.type = MessageType::DM_TO_US; sm.type = MessageType::DM_TO_US;
// Outgoing messages always start with unknown ack status // Outgoing messages always start with unknown ack status
sm.ackStatus = AckStatus::NONE; sm.ackStatus = AckStatus::NONE;
addLiveMessage(sm); addLiveMessage(sm);
} }
#if ENABLE_MESSAGE_PERSISTENCE #if ENABLE_MESSAGE_PERSISTENCE
// Compact, fixed-size on-flash representation using offset + length // Compact, fixed-size on-flash representation using offset + length
struct __attribute__((packed)) StoredMessageRecord { struct __attribute__((packed)) StoredMessageRecord {
uint32_t timestamp; uint32_t timestamp;
uint32_t sender; uint32_t sender;
uint8_t channelIndex; uint8_t channelIndex;
uint32_t dest; uint32_t dest;
uint8_t isBootRelative; uint8_t isBootRelative;
uint8_t ackStatus; // static_cast<uint8_t>(AckStatus) uint8_t ackStatus; // static_cast<uint8_t>(AckStatus)
uint8_t type; // static_cast<uint8_t>(MessageType) uint8_t type; // static_cast<uint8_t>(MessageType)
uint16_t textLength; // message length uint16_t textLength; // message length
char text[MAX_MESSAGE_SIZE]; // store actual text here char text[MAX_MESSAGE_SIZE]; // store actual text here
}; };
// Serialize one StoredMessage to flash // Serialize one StoredMessage to flash
static inline void writeMessageRecord(SafeFile &f, const StoredMessage &m) static inline void writeMessageRecord(SafeFile &f, const StoredMessage &m) {
{ StoredMessageRecord rec = {};
StoredMessageRecord rec = {}; rec.timestamp = m.timestamp;
rec.timestamp = m.timestamp; rec.sender = m.sender;
rec.sender = m.sender; rec.channelIndex = m.channelIndex;
rec.channelIndex = m.channelIndex; rec.dest = m.dest;
rec.dest = m.dest; rec.isBootRelative = m.isBootRelative;
rec.isBootRelative = m.isBootRelative; rec.ackStatus = static_cast<uint8_t>(m.ackStatus);
rec.ackStatus = static_cast<uint8_t>(m.ackStatus); rec.type = static_cast<uint8_t>(m.type);
rec.type = static_cast<uint8_t>(m.type); rec.textLength = m.textLength;
rec.textLength = m.textLength;
// Copy the actual text into the record from RAM pool // Copy the actual text into the record from RAM pool
const char *txt = getTextFromPool(m.textOffset); const char *txt = getTextFromPool(m.textOffset);
strncpy(rec.text, txt, MAX_MESSAGE_SIZE - 1); strncpy(rec.text, txt, MAX_MESSAGE_SIZE - 1);
rec.text[MAX_MESSAGE_SIZE - 1] = '\0'; rec.text[MAX_MESSAGE_SIZE - 1] = '\0';
f.write(reinterpret_cast<const uint8_t *>(&rec), sizeof(rec)); f.write(reinterpret_cast<const uint8_t *>(&rec), sizeof(rec));
} }
// Deserialize one StoredMessage from flash; returns false on short read // Deserialize one StoredMessage from flash; returns false on short read
static inline bool readMessageRecord(File &f, StoredMessage &m) static inline bool readMessageRecord(File &f, StoredMessage &m) {
{ StoredMessageRecord rec = {};
StoredMessageRecord rec = {}; if (f.readBytes(reinterpret_cast<char *>(&rec), sizeof(rec)) != sizeof(rec))
if (f.readBytes(reinterpret_cast<char *>(&rec), sizeof(rec)) != sizeof(rec)) return false;
return false;
m.timestamp = rec.timestamp; m.timestamp = rec.timestamp;
m.sender = rec.sender; m.sender = rec.sender;
m.channelIndex = rec.channelIndex; m.channelIndex = rec.channelIndex;
m.dest = rec.dest; m.dest = rec.dest;
m.isBootRelative = rec.isBootRelative; m.isBootRelative = rec.isBootRelative;
m.ackStatus = static_cast<AckStatus>(rec.ackStatus); m.ackStatus = static_cast<AckStatus>(rec.ackStatus);
m.type = static_cast<MessageType>(rec.type); m.type = static_cast<MessageType>(rec.type);
m.textLength = rec.textLength; m.textLength = rec.textLength;
// 💡 Re-store text into pool and update offset // 💡 Re-store text into pool and update offset
m.textLength = strnlen(rec.text, MAX_MESSAGE_SIZE - 1); m.textLength = strnlen(rec.text, MAX_MESSAGE_SIZE - 1);
m.textOffset = storeTextInPool(rec.text, m.textLength); m.textOffset = storeTextInPool(rec.text, m.textLength);
return true; return true;
} }
void MessageStore::saveToFlash() void MessageStore::saveToFlash() {
{
#ifdef FSCom #ifdef FSCom
// Ensure root exists // Ensure root exists
spiLock->lock(); spiLock->lock();
FSCom.mkdir("/"); FSCom.mkdir("/");
spiLock->unlock(); spiLock->unlock();
SafeFile f(filename.c_str(), false); SafeFile f(filename.c_str(), false);
spiLock->lock(); spiLock->lock();
uint8_t count = static_cast<uint8_t>(liveMessages.size()); uint8_t count = static_cast<uint8_t>(liveMessages.size());
if (count > MAX_MESSAGES_SAVED) if (count > MAX_MESSAGES_SAVED)
count = MAX_MESSAGES_SAVED; count = MAX_MESSAGES_SAVED;
f.write(&count, 1); f.write(&count, 1);
for (uint8_t i = 0; i < count; ++i) { for (uint8_t i = 0; i < count; ++i) {
writeMessageRecord(f, liveMessages[i]); writeMessageRecord(f, liveMessages[i]);
} }
spiLock->unlock(); spiLock->unlock();
f.close(); f.close();
#endif #endif
} }
void MessageStore::loadFromFlash() void MessageStore::loadFromFlash() {
{ std::deque<StoredMessage>().swap(liveMessages);
std::deque<StoredMessage>().swap(liveMessages); resetMessagePool(); // reset pool when loading
resetMessagePool(); // reset pool when loading
#ifdef FSCom #ifdef FSCom
concurrency::LockGuard guard(spiLock); concurrency::LockGuard guard(spiLock);
if (!FSCom.exists(filename.c_str())) if (!FSCom.exists(filename.c_str()))
return; return;
auto f = FSCom.open(filename.c_str(), FILE_O_READ); auto f = FSCom.open(filename.c_str(), FILE_O_READ);
if (!f) if (!f)
return; return;
uint8_t count = 0; uint8_t count = 0;
f.readBytes(reinterpret_cast<char *>(&count), 1); f.readBytes(reinterpret_cast<char *>(&count), 1);
if (count > MAX_MESSAGES_SAVED) if (count > MAX_MESSAGES_SAVED)
count = MAX_MESSAGES_SAVED; count = MAX_MESSAGES_SAVED;
for (uint8_t i = 0; i < count; ++i) { for (uint8_t i = 0; i < count; ++i) {
StoredMessage m; StoredMessage m;
if (!readMessageRecord(f, m)) if (!readMessageRecord(f, m))
break; break;
liveMessages.push_back(m); liveMessages.push_back(m);
} }
f.close(); f.close();
#endif #endif
} }
@@ -279,146 +260,134 @@ void MessageStore::loadFromFlash() {}
#endif #endif
// Clear all messages (RAM + persisted queue) // Clear all messages (RAM + persisted queue)
void MessageStore::clearAllMessages() void MessageStore::clearAllMessages() {
{ std::deque<StoredMessage>().swap(liveMessages);
std::deque<StoredMessage>().swap(liveMessages); resetMessagePool();
resetMessagePool();
#ifdef FSCom #ifdef FSCom
SafeFile f(filename.c_str(), false); SafeFile f(filename.c_str(), false);
uint8_t count = 0; uint8_t count = 0;
f.write(&count, 1); // write "0 messages" f.write(&count, 1); // write "0 messages"
f.close(); f.close();
#endif #endif
} }
// Internal helper: erase first or last message matching a predicate // Internal helper: erase first or last message matching a predicate
template <typename Predicate> static void eraseIf(std::deque<StoredMessage> &deque, Predicate pred, bool fromBack = false) template <typename Predicate> static void eraseIf(std::deque<StoredMessage> &deque, Predicate pred, bool fromBack = false) {
{ if (fromBack) {
if (fromBack) { // Iterate from the back and erase all matches from the end
// Iterate from the back and erase all matches from the end for (auto it = deque.rbegin(); it != deque.rend();) {
for (auto it = deque.rbegin(); it != deque.rend();) { if (pred(*it)) {
if (pred(*it)) { it = std::deque<StoredMessage>::reverse_iterator(deque.erase(std::next(it).base()));
it = std::deque<StoredMessage>::reverse_iterator(deque.erase(std::next(it).base())); } else {
} else { ++it;
++it; }
}
}
} else {
// Manual forward search to erase all matches
for (auto it = deque.begin(); it != deque.end();) {
if (pred(*it)) {
it = deque.erase(it);
} else {
++it;
}
}
} }
} else {
// Manual forward search to erase all matches
for (auto it = deque.begin(); it != deque.end();) {
if (pred(*it)) {
it = deque.erase(it);
} else {
++it;
}
}
}
} }
// Delete oldest message (RAM + persisted queue) // Delete oldest message (RAM + persisted queue)
void MessageStore::deleteOldestMessage() void MessageStore::deleteOldestMessage() {
{ eraseIf(liveMessages, [](StoredMessage &) { return true; });
eraseIf(liveMessages, [](StoredMessage &) { return true; }); saveToFlash();
saveToFlash();
} }
// Delete oldest message in a specific channel // Delete oldest message in a specific channel
void MessageStore::deleteOldestMessageInChannel(uint8_t channel) void MessageStore::deleteOldestMessageInChannel(uint8_t channel) {
{ auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; };
auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; }; eraseIf(liveMessages, pred);
eraseIf(liveMessages, pred); saveToFlash();
saveToFlash();
} }
void MessageStore::deleteAllMessagesInChannel(uint8_t channel) void MessageStore::deleteAllMessagesInChannel(uint8_t channel) {
{ auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; };
auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; }; eraseIf(liveMessages, pred, false /* delete ALL, not just first */);
eraseIf(liveMessages, pred, false /* delete ALL, not just first */); saveToFlash();
saveToFlash();
} }
void MessageStore::deleteAllMessagesWithPeer(uint32_t peer) void MessageStore::deleteAllMessagesWithPeer(uint32_t peer) {
{ uint32_t local = nodeDB->getNodeNum();
uint32_t local = nodeDB->getNodeNum(); auto pred = [&](const StoredMessage &m) {
auto pred = [&](const StoredMessage &m) { if (m.type != MessageType::DM_TO_US)
if (m.type != MessageType::DM_TO_US) return false;
return false; uint32_t other = (m.sender == local) ? m.dest : m.sender;
uint32_t other = (m.sender == local) ? m.dest : m.sender; return other == peer;
return other == peer; };
}; eraseIf(liveMessages, pred, false);
eraseIf(liveMessages, pred, false); saveToFlash();
saveToFlash();
} }
// Delete oldest message in a direct chat with a node // Delete oldest message in a direct chat with a node
void MessageStore::deleteOldestMessageWithPeer(uint32_t peer) void MessageStore::deleteOldestMessageWithPeer(uint32_t peer) {
{ auto pred = [peer](const StoredMessage &m) {
auto pred = [peer](const StoredMessage &m) { if (m.type != MessageType::DM_TO_US)
if (m.type != MessageType::DM_TO_US) return false;
return false; uint32_t other = (m.sender == nodeDB->getNodeNum()) ? m.dest : m.sender;
uint32_t other = (m.sender == nodeDB->getNodeNum()) ? m.dest : m.sender; return other == peer;
return other == peer; };
}; eraseIf(liveMessages, pred);
eraseIf(liveMessages, pred); saveToFlash();
saveToFlash();
} }
std::deque<StoredMessage> MessageStore::getChannelMessages(uint8_t channel) const std::deque<StoredMessage> MessageStore::getChannelMessages(uint8_t channel) const {
{ std::deque<StoredMessage> result;
std::deque<StoredMessage> result; for (const auto &m : liveMessages) {
for (const auto &m : liveMessages) { if (m.type == MessageType::BROADCAST && m.channelIndex == channel) {
if (m.type == MessageType::BROADCAST && m.channelIndex == channel) { result.push_back(m);
result.push_back(m);
}
} }
return result; }
return result;
} }
std::deque<StoredMessage> MessageStore::getDirectMessages() const std::deque<StoredMessage> MessageStore::getDirectMessages() const {
{ std::deque<StoredMessage> result;
std::deque<StoredMessage> result; for (const auto &m : liveMessages) {
for (const auto &m : liveMessages) { if (m.type == MessageType::DM_TO_US) {
if (m.type == MessageType::DM_TO_US) { result.push_back(m);
result.push_back(m);
}
} }
return result; }
return result;
} }
// Upgrade boot-relative timestamps once RTC is valid // Upgrade boot-relative timestamps once RTC is valid
// Only same-boot boot-relative messages are healed. // Only same-boot boot-relative messages are healed.
// Persisted boot-relative messages from old boots stay ??? forever. // Persisted boot-relative messages from old boots stay ??? forever.
void MessageStore::upgradeBootRelativeTimestamps() void MessageStore::upgradeBootRelativeTimestamps() {
{ uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true);
uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true); if (nowSecs == 0)
if (nowSecs == 0) return; // Still no valid RTC
return; // Still no valid RTC
uint32_t bootNow = millis() / 1000; uint32_t bootNow = millis() / 1000;
auto fix = [&](std::deque<StoredMessage> &dq) { auto fix = [&](std::deque<StoredMessage> &dq) {
for (auto &m : dq) { for (auto &m : dq) {
if (m.isBootRelative && m.timestamp <= bootNow) { if (m.isBootRelative && m.timestamp <= bootNow) {
uint32_t bootOffset = nowSecs - bootNow; uint32_t bootOffset = nowSecs - bootNow;
m.timestamp += bootOffset; m.timestamp += bootOffset;
m.isBootRelative = false; m.isBootRelative = false;
} }
} }
}; };
fix(liveMessages); fix(liveMessages);
} }
const char *MessageStore::getText(const StoredMessage &msg) const char *MessageStore::getText(const StoredMessage &msg) {
{ // Wrapper around the internal helper
// Wrapper around the internal helper return getTextFromPool(msg.textOffset);
return getTextFromPool(msg.textOffset);
} }
uint16_t MessageStore::storeText(const char *src, size_t len) uint16_t MessageStore::storeText(const char *src, size_t len) {
{ // Wrapper around the internal helper
// Wrapper around the internal helper return storeTextInPool(src, len);
return storeTextInPool(src, len);
} }
// Global definition // Global definition
src/MessageStore.h
+58 -61
View File
@@ -39,90 +39,87 @@
// Explicit message classification // Explicit message classification
enum class MessageType : uint8_t { enum class MessageType : uint8_t {
BROADCAST = 0, // broadcast message BROADCAST = 0, // broadcast message
DM_TO_US = 1 // direct message addressed to this node DM_TO_US = 1 // direct message addressed to this node
}; };
// Delivery status for messages we sent // Delivery status for messages we sent
enum class AckStatus : uint8_t { enum class AckStatus : uint8_t {
NONE = 0, // just sent, waiting (no symbol shown) NONE = 0, // just sent, waiting (no symbol shown)
ACKED = 1, // got a valid ACK from destination ACKED = 1, // got a valid ACK from destination
NACKED = 2, // explicitly failed NACKED = 2, // explicitly failed
TIMEOUT = 3, // no ACK after retry window TIMEOUT = 3, // no ACK after retry window
RELAYED = 4 // got an ACK from relay, not destination RELAYED = 4 // got an ACK from relay, not destination
}; };
struct StoredMessage { struct StoredMessage {
uint32_t timestamp; // When message was created (secs since boot or RTC) uint32_t timestamp; // When message was created (secs since boot or RTC)
uint32_t sender; // NodeNum of sender uint32_t sender; // NodeNum of sender
uint8_t channelIndex; // Channel index used uint8_t channelIndex; // Channel index used
uint32_t dest; // Destination node (broadcast or direct) uint32_t dest; // Destination node (broadcast or direct)
MessageType type; // Derived from dest (explicit classification) MessageType type; // Derived from dest (explicit classification)
bool isBootRelative; // true = millis()/1000 fallback; false = epoch/RTC absolute bool isBootRelative; // true = millis()/1000 fallback; false = epoch/RTC absolute
AckStatus ackStatus; // Delivery status (only meaningful for our own sent messages) AckStatus ackStatus; // Delivery status (only meaningful for our own sent messages)
// Text storage metadata — rebuilt from flash at boot // Text storage metadata — rebuilt from flash at boot
uint16_t textOffset; // Offset into global text pool (valid only after loadFromFlash()) uint16_t textOffset; // Offset into global text pool (valid only after loadFromFlash())
uint16_t textLength; // Length of text in bytes uint16_t textLength; // Length of text in bytes
// Default constructor initializes all fields safely // Default constructor initializes all fields safely
StoredMessage() StoredMessage()
: timestamp(0), sender(0), channelIndex(0), dest(0xffffffff), type(MessageType::BROADCAST), isBootRelative(false), : timestamp(0), sender(0), channelIndex(0), dest(0xffffffff), type(MessageType::BROADCAST), isBootRelative(false), ackStatus(AckStatus::NONE),
ackStatus(AckStatus::NONE), textOffset(0), textLength(0) textOffset(0), textLength(0) {}
{
}
}; };
class MessageStore class MessageStore {
{ public:
public: explicit MessageStore(const std::string &label);
explicit MessageStore(const std::string &label);
// Live RAM methods (always current, used by UI and runtime) // Live RAM methods (always current, used by UI and runtime)
void addLiveMessage(StoredMessage &&msg); void addLiveMessage(StoredMessage &&msg);
void addLiveMessage(const StoredMessage &msg); // convenience overload void addLiveMessage(const StoredMessage &msg); // convenience overload
const std::deque<StoredMessage> &getLiveMessages() const { return liveMessages; } const std::deque<StoredMessage> &getLiveMessages() const { return liveMessages; }
// Add new messages from packets or manual input // Add new messages from packets or manual input
const StoredMessage &addFromPacket(const meshtastic_MeshPacket &mp); // Incoming/outgoing → RAM only const StoredMessage &addFromPacket(const meshtastic_MeshPacket &mp); // Incoming/outgoing → RAM only
void addFromString(uint32_t sender, uint8_t channelIndex, const std::string &text); // Manual add void addFromString(uint32_t sender, uint8_t channelIndex, const std::string &text); // Manual add
// Persistence methods (used only on boot/shutdown) // Persistence methods (used only on boot/shutdown)
void saveToFlash(); // Save messages to flash void saveToFlash(); // Save messages to flash
void loadFromFlash(); // Load messages from flash void loadFromFlash(); // Load messages from flash
// Clear all messages (RAM + persisted queue + text pool) // Clear all messages (RAM + persisted queue + text pool)
void clearAllMessages(); void clearAllMessages();
// Delete helpers // Delete helpers
void deleteOldestMessage(); // remove oldest from RAM (and flash on save) void deleteOldestMessage(); // remove oldest from RAM (and flash on save)
void deleteOldestMessageInChannel(uint8_t channel); void deleteOldestMessageInChannel(uint8_t channel);
void deleteOldestMessageWithPeer(uint32_t peer); void deleteOldestMessageWithPeer(uint32_t peer);
void deleteAllMessagesInChannel(uint8_t channel); void deleteAllMessagesInChannel(uint8_t channel);
void deleteAllMessagesWithPeer(uint32_t peer); void deleteAllMessagesWithPeer(uint32_t peer);
// Unified accessor (for UI code, defaults to RAM buffer) // Unified accessor (for UI code, defaults to RAM buffer)
const std::deque<StoredMessage> &getMessages() const { return liveMessages; } const std::deque<StoredMessage> &getMessages() const { return liveMessages; }
// Helper filters for future use // Helper filters for future use
std::deque<StoredMessage> getChannelMessages(uint8_t channel) const; // Only broadcast messages on a channel std::deque<StoredMessage> getChannelMessages(uint8_t channel) const; // Only broadcast messages on a channel
std::deque<StoredMessage> getDirectMessages() const; // Only direct messages std::deque<StoredMessage> getDirectMessages() const; // Only direct messages
// Upgrade boot-relative timestamps once RTC is valid // Upgrade boot-relative timestamps once RTC is valid
void upgradeBootRelativeTimestamps(); void upgradeBootRelativeTimestamps();
// Retrieve the C-string text for a stored message // Retrieve the C-string text for a stored message
static const char *getText(const StoredMessage &msg); static const char *getText(const StoredMessage &msg);
// Allocate text into pool (used by sender-side code) // Allocate text into pool (used by sender-side code)
static uint16_t storeText(const char *src, size_t len); static uint16_t storeText(const char *src, size_t len);
// Used when loading from flash to rebuild the text pool // Used when loading from flash to rebuild the text pool
static uint16_t rebuildTextFromFlash(const char *src, size_t len); static uint16_t rebuildTextFromFlash(const char *src, size_t len);
private: private:
std::deque<StoredMessage> liveMessages; // Single in-RAM message buffer (also used for persistence) std::deque<StoredMessage> liveMessages; // Single in-RAM message buffer (also used for persistence)
std::string filename; // Flash filename for persistence std::string filename; // Flash filename for persistence
}; };
// Global instance (defined in MessageStore.cpp) // Global instance (defined in MessageStore.cpp)
src/NodeStatus.h
+40 -48
View File
@@ -3,64 +3,56 @@
#include "configuration.h" #include "configuration.h"
#include <Arduino.h> #include <Arduino.h>
namespace meshtastic namespace meshtastic {
{
/// Describes the state of the NodeDB system. /// Describes the state of the NodeDB system.
class NodeStatus : public Status class NodeStatus : public Status {
{
private: private:
CallbackObserver<NodeStatus, const NodeStatus *> statusObserver = CallbackObserver<NodeStatus, const NodeStatus *> statusObserver = CallbackObserver<NodeStatus, const NodeStatus *>(this, &NodeStatus::updateStatus);
CallbackObserver<NodeStatus, const NodeStatus *>(this, &NodeStatus::updateStatus);
uint16_t numOnline = 0; uint16_t numOnline = 0;
uint16_t numTotal = 0; uint16_t numTotal = 0;
uint16_t lastNumTotal = 0; uint16_t lastNumTotal = 0;
public: public:
bool forceUpdate = false; bool forceUpdate = false;
NodeStatus() { statusType = STATUS_TYPE_NODE; } NodeStatus() { statusType = STATUS_TYPE_NODE; }
NodeStatus(uint16_t numOnline, uint16_t numTotal, bool forceUpdate = false) : Status() NodeStatus(uint16_t numOnline, uint16_t numTotal, bool forceUpdate = false) : Status() {
this->forceUpdate = forceUpdate;
this->numOnline = numOnline;
this->numTotal = numTotal;
}
NodeStatus(const NodeStatus &);
NodeStatus &operator=(const NodeStatus &);
void observe(Observable<const NodeStatus *> *source) { statusObserver.observe(source); }
uint16_t getNumOnline() const { return numOnline; }
uint16_t getNumTotal() const { return numTotal; }
uint16_t getLastNumTotal() const { return lastNumTotal; }
bool matches(const NodeStatus *newStatus) const { return (newStatus->getNumOnline() != numOnline || newStatus->getNumTotal() != numTotal); }
int updateStatus(const NodeStatus *newStatus) {
// Only update the status if values have actually changed
lastNumTotal = numTotal;
bool isDirty;
{ {
this->forceUpdate = forceUpdate; isDirty = matches(newStatus);
this->numOnline = numOnline; initialized = true;
this->numTotal = numTotal; numOnline = newStatus->getNumOnline();
numTotal = newStatus->getNumTotal();
} }
NodeStatus(const NodeStatus &); if (isDirty || newStatus->forceUpdate) {
NodeStatus &operator=(const NodeStatus &); LOG_DEBUG("Node status update: %u online, %u total", numOnline, numTotal);
onNewStatus.notifyObservers(this);
void observe(Observable<const NodeStatus *> *source) { statusObserver.observe(source); }
uint16_t getNumOnline() const { return numOnline; }
uint16_t getNumTotal() const { return numTotal; }
uint16_t getLastNumTotal() const { return lastNumTotal; }
bool matches(const NodeStatus *newStatus) const
{
return (newStatus->getNumOnline() != numOnline || newStatus->getNumTotal() != numTotal);
}
int updateStatus(const NodeStatus *newStatus)
{
// Only update the status if values have actually changed
lastNumTotal = numTotal;
bool isDirty;
{
isDirty = matches(newStatus);
initialized = true;
numOnline = newStatus->getNumOnline();
numTotal = newStatus->getNumTotal();
}
if (isDirty || newStatus->forceUpdate) {
LOG_DEBUG("Node status update: %u online, %u total", numOnline, numTotal);
onNewStatus.notifyObservers(this);
}
return 0;
} }
return 0;
}
}; };
} // namespace meshtastic } // namespace meshtastic
src/Observer.h
+58 -67
View File
@@ -8,99 +8,90 @@ template <class T> class Observable;
/** /**
* An observer which can be mixed in as a baseclass. Implement onNotify as a method in your class. * An observer which can be mixed in as a baseclass. Implement onNotify as a method in your class.
*/ */
template <class T> class Observer template <class T> class Observer {
{ std::list<Observable<T> *> observables;
std::list<Observable<T> *> observables;
public: public:
virtual ~Observer(); virtual ~Observer();
/// Stop watching the observable /// Stop watching the observable
void unobserve(Observable<T> *o); void unobserve(Observable<T> *o);
/// Start watching a specified observable /// Start watching a specified observable
void observe(Observable<T> *o); void observe(Observable<T> *o);
private: private:
friend class Observable<T>; friend class Observable<T>;
protected: protected:
/** /**
* returns 0 if other observers should continue to be called * returns 0 if other observers should continue to be called
* returns !0 if the observe calls should be aborted and this result code returned for notifyObservers * returns !0 if the observe calls should be aborted and this result code returned for notifyObservers
**/ **/
virtual int onNotify(T arg) = 0; virtual int onNotify(T arg) = 0;
}; };
/** /**
* An observer that calls an arbitrary method * An observer that calls an arbitrary method
*/ */
template <class Callback, class T> class CallbackObserver : public Observer<T> template <class Callback, class T> class CallbackObserver : public Observer<T> {
{ typedef int (Callback::*ObserverCallback)(T arg);
typedef int (Callback::*ObserverCallback)(T arg);
Callback *objPtr; Callback *objPtr;
ObserverCallback method; ObserverCallback method;
public: public:
CallbackObserver(Callback *_objPtr, ObserverCallback _method) : objPtr(_objPtr), method(_method) {} CallbackObserver(Callback *_objPtr, ObserverCallback _method) : objPtr(_objPtr), method(_method) {}
protected: protected:
virtual int onNotify(T arg) override { return (objPtr->*method)(arg); } virtual int onNotify(T arg) override { return (objPtr->*method)(arg); }
}; };
/** /**
* An observable class that will notify observers anytime notifyObservers is called. Argument type T can be any type, but for * An observable class that will notify observers anytime notifyObservers is called. Argument type T can be any type,
* performance reasons a pointer or word sized object is recommended. * but for performance reasons a pointer or word sized object is recommended.
*/ */
template <class T> class Observable template <class T> class Observable {
{ std::list<Observer<T> *> observers;
std::list<Observer<T> *> observers;
public: public:
/** /**
* Tell all observers about a change, observers can process arg as they wish * Tell all observers about a change, observers can process arg as they wish
* *
* returns !0 if an observer chose to abort processing by returning this code * returns !0 if an observer chose to abort processing by returning this code
*/ */
int notifyObservers(T arg) int notifyObservers(T arg) {
{ for (typename std::list<Observer<T> *>::const_iterator iterator = observers.begin(); iterator != observers.end(); ++iterator) {
for (typename std::list<Observer<T> *>::const_iterator iterator = observers.begin(); iterator != observers.end(); int result = (*iterator)->onNotify(arg);
++iterator) { if (result != 0)
int result = (*iterator)->onNotify(arg); return result;
if (result != 0)
return result;
}
return 0;
} }
private: return 0;
friend class Observer<T>; }
// Not called directly, instead call observer.observe private:
void addObserver(Observer<T> *o) { observers.push_back(o); } friend class Observer<T>;
void removeObserver(Observer<T> *o) { observers.remove(o); } // Not called directly, instead call observer.observe
void addObserver(Observer<T> *o) { observers.push_back(o); }
void removeObserver(Observer<T> *o) { observers.remove(o); }
}; };
template <class T> Observer<T>::~Observer() template <class T> Observer<T>::~Observer() {
{ for (typename std::list<Observable<T> *>::const_iterator iterator = observables.begin(); iterator != observables.end(); ++iterator) {
for (typename std::list<Observable<T> *>::const_iterator iterator = observables.begin(); iterator != observables.end(); (*iterator)->removeObserver(this);
++iterator) { }
(*iterator)->removeObserver(this); observables.clear();
}
observables.clear();
} }
template <class T> void Observer<T>::unobserve(Observable<T> *o) template <class T> void Observer<T>::unobserve(Observable<T> *o) {
{ o->removeObserver(this);
o->removeObserver(this); observables.remove(o);
observables.remove(o);
} }
template <class T> void Observer<T>::observe(Observable<T> *o) template <class T> void Observer<T>::observe(Observable<T> *o) {
{ observables.push_back(o);
observables.push_back(o); o->addObserver(this);
o->addObserver(this);
} }
src/Power.cpp
+916 -983
View File
File diff suppressed because it is too large Load Diff
src/PowerFSM.cpp
+237 -263
View File
@@ -31,222 +31,202 @@ FakeFsm powerFSM;
void PowerFSM_setup(){}; void PowerFSM_setup(){};
#else #else
/// Should we behave as if we have AC power now? /// Should we behave as if we have AC power now?
static bool isPowered() static bool isPowered() {
{
// Circumvent the battery sensing logic and assumes constant power if no battery pin or power mgmt IC // Circumvent the battery sensing logic and assumes constant power if no battery pin or power mgmt IC
#if !defined(BATTERY_PIN) && !defined(HAS_AXP192) && !defined(HAS_AXP2101) && !defined(NRF_APM) #if !defined(BATTERY_PIN) && !defined(HAS_AXP192) && !defined(HAS_AXP2101) && !defined(NRF_APM)
return true; return true;
#endif #endif
bool isRouter = (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER ? 1 : 0); bool isRouter = (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER ? 1 : 0);
// If we are not a router and we already have AC power go to POWER state after init, otherwise go to ON // If we are not a router and we already have AC power go to POWER state after init, otherwise go to ON
// We assume routers might be powered all the time, but from a low current (solar) source // We assume routers might be powered all the time, but from a low current (solar) source
bool isPowerSavingMode = config.power.is_power_saving || isRouter; bool isPowerSavingMode = config.power.is_power_saving || isRouter;
/* To determine if we're externally powered, assumptions /* To determine if we're externally powered, assumptions
1) If we're powered up and there's no battery, we must be getting power externally. (because we'd be dead otherwise) 1) If we're powered up and there's no battery, we must be getting power externally. (because we'd be dead
otherwise)
2) If we detect USB power from the power management chip, we must be getting power externally. 2) If we detect USB power from the power management chip, we must be getting power externally.
3) On some boards we don't have the power management chip (like AXPxxxx) so we use EXT_PWR_DETECT GPIO pin to detect 3) On some boards we don't have the power management chip (like AXPxxxx) so we use EXT_PWR_DETECT GPIO pin to
external power source (see `isVbusIn()` in `Power.cpp`) detect external power source (see `isVbusIn()` in `Power.cpp`)
*/ */
return !isPowerSavingMode && powerStatus && (!powerStatus->getHasBattery() || powerStatus->getHasUSB()); return !isPowerSavingMode && powerStatus && (!powerStatus->getHasBattery() || powerStatus->getHasUSB());
} }
static void sdsEnter() static void sdsEnter() {
{ LOG_POWERFSM("State: SDS");
LOG_POWERFSM("State: SDS"); // FIXME - make sure GPS and LORA radio are off first - because we want close to zero current draw
// FIXME - make sure GPS and LORA radio are off first - because we want close to zero current draw doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, false);
doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, false);
} }
static void lowBattSDSEnter() static void lowBattSDSEnter() {
{ LOG_POWERFSM("State: Lower batt SDS");
LOG_POWERFSM("State: Lower batt SDS"); doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, true);
doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, true);
} }
extern Power *power; extern Power *power;
static void shutdownEnter() static void shutdownEnter() {
{ LOG_POWERFSM("State: SHUTDOWN");
LOG_POWERFSM("State: SHUTDOWN"); shutdownAtMsec = millis();
shutdownAtMsec = millis();
} }
#include "error.h" #include "error.h"
static uint32_t secsSlept; static uint32_t secsSlept;
static void lsEnter() static void lsEnter() {
{ LOG_POWERFSM("lsEnter begin, ls_secs=%u", config.power.ls_secs);
LOG_POWERFSM("lsEnter begin, ls_secs=%u", config.power.ls_secs); if (screen)
if (screen) screen->setOn(false);
screen->setOn(false); secsSlept = 0; // How long have we been sleeping this time
secsSlept = 0; // How long have we been sleeping this time
// LOG_INFO("lsEnter end"); // LOG_INFO("lsEnter end");
} }
static void lsIdle() static void lsIdle() {
{ // LOG_INFO("lsIdle begin ls_secs=%u", getPref_ls_secs());
// LOG_INFO("lsIdle begin ls_secs=%u", getPref_ls_secs());
#ifdef ARCH_ESP32 #ifdef ARCH_ESP32
// Do we have more sleeping to do? // Do we have more sleeping to do?
if (secsSlept < config.power.ls_secs) { if (secsSlept < config.power.ls_secs) {
// If some other service would stall sleep, don't let sleep happen yet // If some other service would stall sleep, don't let sleep happen yet
if (doPreflightSleep()) { if (doPreflightSleep()) {
// Briefly come out of sleep long enough to blink the led once every few seconds // Briefly come out of sleep long enough to blink the led once every few seconds
uint32_t sleepTime = SLEEP_TIME; uint32_t sleepTime = SLEEP_TIME;
powerMon->setState(meshtastic_PowerMon_State_CPU_LightSleep); powerMon->setState(meshtastic_PowerMon_State_CPU_LightSleep);
ledBlink.set(false); // Never leave led on while in light sleep ledBlink.set(false); // Never leave led on while in light sleep
esp_sleep_source_t wakeCause2 = doLightSleep(sleepTime * 1000LL); esp_sleep_source_t wakeCause2 = doLightSleep(sleepTime * 1000LL);
powerMon->clearState(meshtastic_PowerMon_State_CPU_LightSleep); powerMon->clearState(meshtastic_PowerMon_State_CPU_LightSleep);
switch (wakeCause2) { switch (wakeCause2) {
case ESP_SLEEP_WAKEUP_TIMER: case ESP_SLEEP_WAKEUP_TIMER:
// Normal case: timer expired, we should just go back to sleep ASAP // Normal case: timer expired, we should just go back to sleep ASAP
ledBlink.set(true); // briefly turn on led ledBlink.set(true); // briefly turn on led
wakeCause2 = doLightSleep(100); // leave led on for 1ms wakeCause2 = doLightSleep(100); // leave led on for 1ms
secsSlept += sleepTime; secsSlept += sleepTime;
// LOG_INFO("Sleep, flash led!"); // LOG_INFO("Sleep, flash led!");
break; break;
case ESP_SLEEP_WAKEUP_UART: case ESP_SLEEP_WAKEUP_UART:
// Not currently used (because uart triggers in hw have problems) // Not currently used (because uart triggers in hw have problems)
powerFSM.trigger(EVENT_SERIAL_CONNECTED); powerFSM.trigger(EVENT_SERIAL_CONNECTED);
break; break;
default: default:
// We woke for some other reason (button press, device IRQ interrupt) // We woke for some other reason (button press, device IRQ interrupt)
#ifdef BUTTON_PIN #ifdef BUTTON_PIN
bool pressed = !digitalRead(config.device.button_gpio ? config.device.button_gpio : BUTTON_PIN); bool pressed = !digitalRead(config.device.button_gpio ? config.device.button_gpio : BUTTON_PIN);
#else #else
bool pressed = false; bool pressed = false;
#endif #endif
if (pressed) { // If we woke because of press, instead generate a PRESS event. if (pressed) { // If we woke because of press, instead generate a PRESS event.
powerFSM.trigger(EVENT_PRESS); powerFSM.trigger(EVENT_PRESS);
} else {
// Otherwise let the NB state handle the IRQ (and that state will handle stuff like IRQs etc)
// we lie and say "wake timer" because the interrupt will be handled by the regular IRQ code
powerFSM.trigger(EVENT_WAKE_TIMER);
}
break;
}
} else { } else {
// Someone says we can't sleep now, so just save some power by sleeping the CPU for 100ms or so // Otherwise let the NB state handle the IRQ (and that state will handle stuff like IRQs etc)
delay(100); // we lie and say "wake timer" because the interrupt will be handled by the regular IRQ code
powerFSM.trigger(EVENT_WAKE_TIMER);
} }
break;
}
} else { } else {
// Time to stop sleeping! // Someone says we can't sleep now, so just save some power by sleeping the CPU for 100ms or so
ledBlink.set(false); delay(100);
LOG_INFO("Reached ls_secs, service loop()");
powerFSM.trigger(EVENT_WAKE_TIMER);
} }
} else {
// Time to stop sleeping!
ledBlink.set(false);
LOG_INFO("Reached ls_secs, service loop()");
powerFSM.trigger(EVENT_WAKE_TIMER);
}
#endif #endif
} }
static void lsExit() static void lsExit() { LOG_POWERFSM("State: lsExit"); }
{
LOG_POWERFSM("State: lsExit");
}
static void nbEnter() static void nbEnter() {
{ LOG_POWERFSM("State: nbEnter");
LOG_POWERFSM("State: nbEnter"); if (screen)
if (screen) screen->setOn(false);
screen->setOn(false);
#ifdef ARCH_ESP32 #ifdef ARCH_ESP32
// Only ESP32 should turn off bluetooth // Only ESP32 should turn off bluetooth
setBluetoothEnable(false); setBluetoothEnable(false);
#endif #endif
// FIXME - check if we already have packets for phone and immediately trigger EVENT_PACKETS_FOR_PHONE // FIXME - check if we already have packets for phone and immediately trigger EVENT_PACKETS_FOR_PHONE
} }
static void darkEnter() static void darkEnter() {
{ LOG_POWERFSM("State: darkEnter");
LOG_POWERFSM("State: darkEnter"); setBluetoothEnable(true);
setBluetoothEnable(true); if (screen)
screen->setOn(false);
}
static void serialEnter() {
LOG_POWERFSM("State: serialEnter");
setBluetoothEnable(false);
if (screen) {
screen->setOn(true);
}
}
static void serialExit() {
LOG_POWERFSM("State: serialExit");
// Turn bluetooth back on when we leave serial stream API
setBluetoothEnable(true);
}
static void powerEnter() {
LOG_POWERFSM("State: powerEnter");
if (!isPowered()) {
// If we got here, we are in the wrong state - we should be in powered, let that state handle things
LOG_INFO("Loss of power in Powered");
powerFSM.trigger(EVENT_POWER_DISCONNECTED);
} else {
if (screen) if (screen)
screen->setOn(false); screen->setOn(true);
}
static void serialEnter()
{
LOG_POWERFSM("State: serialEnter");
setBluetoothEnable(false);
if (screen) {
screen->setOn(true);
}
}
static void serialExit()
{
LOG_POWERFSM("State: serialExit");
// Turn bluetooth back on when we leave serial stream API
setBluetoothEnable(true); setBluetoothEnable(true);
// within enter() the function getState() returns the state we came from
}
} }
static void powerEnter() static void powerIdle() {
{ // LOG_POWERFSM("State: powerIdle"); // very chatty
LOG_POWERFSM("State: powerEnter"); if (!isPowered()) {
if (!isPowered()) { // If we got here, we are in the wrong state
// If we got here, we are in the wrong state - we should be in powered, let that state handle things LOG_INFO("Loss of power in Powered");
LOG_INFO("Loss of power in Powered"); powerFSM.trigger(EVENT_POWER_DISCONNECTED);
powerFSM.trigger(EVENT_POWER_DISCONNECTED); }
} else {
if (screen)
screen->setOn(true);
setBluetoothEnable(true);
// within enter() the function getState() returns the state we came from
}
} }
static void powerIdle() static void powerExit() {
{ LOG_POWERFSM("State: powerExit");
// LOG_POWERFSM("State: powerIdle"); // very chatty setBluetoothEnable(true);
if (!isPowered()) {
// If we got here, we are in the wrong state
LOG_INFO("Loss of power in Powered");
powerFSM.trigger(EVENT_POWER_DISCONNECTED);
}
} }
static void powerExit() static void onEnter() {
{ LOG_POWERFSM("State: onEnter");
LOG_POWERFSM("State: powerExit"); if (screen)
setBluetoothEnable(true); screen->setOn(true);
setBluetoothEnable(true);
} }
static void onEnter() static void onIdle() {
{ LOG_POWERFSM("State: onIdle");
LOG_POWERFSM("State: onEnter"); if (isPowered()) {
if (screen) // If we got here, we are in the wrong state - we should be in powered, let that state handle things
screen->setOn(true); powerFSM.trigger(EVENT_POWER_CONNECTED);
setBluetoothEnable(true); }
} }
static void onIdle() static void bootEnter() { LOG_POWERFSM("State: bootEnter"); }
{
LOG_POWERFSM("State: onIdle");
if (isPowered()) {
// If we got here, we are in the wrong state - we should be in powered, let that state handle things
powerFSM.trigger(EVENT_POWER_CONNECTED);
}
}
static void bootEnter()
{
LOG_POWERFSM("State: bootEnter");
}
State stateSHUTDOWN(shutdownEnter, NULL, NULL, "SHUTDOWN"); State stateSHUTDOWN(shutdownEnter, NULL, NULL, "SHUTDOWN");
State stateSDS(sdsEnter, NULL, NULL, "SDS"); State stateSDS(sdsEnter, NULL, NULL, "SDS");
@@ -260,147 +240,141 @@ State stateON(onEnter, onIdle, NULL, "ON");
State statePOWER(powerEnter, powerIdle, powerExit, "POWER"); State statePOWER(powerEnter, powerIdle, powerExit, "POWER");
Fsm powerFSM(&stateBOOT); Fsm powerFSM(&stateBOOT);
void PowerFSM_setup() void PowerFSM_setup() {
{ bool isRouter = (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER ? 1 : 0);
bool isRouter = (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER ? 1 : 0); bool hasPower = isPowered();
bool hasPower = isPowered();
LOG_INFO("PowerFSM init, USB power=%d", hasPower ? 1 : 0); LOG_INFO("PowerFSM init, USB power=%d", hasPower ? 1 : 0);
powerFSM.add_timed_transition(&stateBOOT, hasPower ? &statePOWER : &stateON, 3 * 1000, NULL, "boot timeout"); powerFSM.add_timed_transition(&stateBOOT, hasPower ? &statePOWER : &stateON, 3 * 1000, NULL, "boot timeout");
// wake timer expired or a packet arrived // wake timer expired or a packet arrived
// if we are a router node, we go to NB (no need for bluetooth) otherwise we go to DARK (so we can send message to phone) // if we are a router node, we go to NB (no need for bluetooth) otherwise we go to DARK (so we can send message to
// phone)
#ifdef ARCH_ESP32 #ifdef ARCH_ESP32
powerFSM.add_transition(&stateLS, isRouter ? &stateNB : &stateDARK, EVENT_WAKE_TIMER, NULL, "Wake timer"); powerFSM.add_transition(&stateLS, isRouter ? &stateNB : &stateDARK, EVENT_WAKE_TIMER, NULL, "Wake timer");
#else // Don't go into a no-bluetooth state on low power platforms #else // Don't go into a no-bluetooth state on low power platforms
powerFSM.add_transition(&stateLS, &stateDARK, EVENT_WAKE_TIMER, NULL, "Wake timer"); powerFSM.add_transition(&stateLS, &stateDARK, EVENT_WAKE_TIMER, NULL, "Wake timer");
#endif #endif
// We need this transition, because we might not transition if we were waiting to enter light-sleep, because when we wake from // We need this transition, because we might not transition if we were waiting to enter light-sleep, because when we
// light sleep we _always_ transition to NB or dark and // wake from light sleep we _always_ transition to NB or dark and
powerFSM.add_transition(&stateLS, isRouter ? &stateNB : &stateDARK, EVENT_PACKET_FOR_PHONE, NULL, powerFSM.add_transition(&stateLS, isRouter ? &stateNB : &stateDARK, EVENT_PACKET_FOR_PHONE, NULL, "Received packet, exiting light sleep");
"Received packet, exiting light sleep"); powerFSM.add_transition(&stateNB, &stateNB, EVENT_PACKET_FOR_PHONE, NULL, "Received packet, resetting win wake");
powerFSM.add_transition(&stateNB, &stateNB, EVENT_PACKET_FOR_PHONE, NULL, "Received packet, resetting win wake");
// Handle press events - note: we ignore button presses when in API mode // Handle press events - note: we ignore button presses when in API mode
powerFSM.add_transition(&stateLS, &stateON, EVENT_PRESS, NULL, "Press"); powerFSM.add_transition(&stateLS, &stateON, EVENT_PRESS, NULL, "Press");
powerFSM.add_transition(&stateNB, &stateON, EVENT_PRESS, NULL, "Press"); powerFSM.add_transition(&stateNB, &stateON, EVENT_PRESS, NULL, "Press");
powerFSM.add_transition(&stateDARK, isPowered() ? &statePOWER : &stateON, EVENT_PRESS, NULL, "Press"); powerFSM.add_transition(&stateDARK, isPowered() ? &statePOWER : &stateON, EVENT_PRESS, NULL, "Press");
powerFSM.add_transition(&statePOWER, &statePOWER, EVENT_PRESS, NULL, "Press"); powerFSM.add_transition(&statePOWER, &statePOWER, EVENT_PRESS, NULL, "Press");
powerFSM.add_transition(&stateON, &stateON, EVENT_PRESS, NULL, "Press"); // reenter On to restart our timers powerFSM.add_transition(&stateON, &stateON, EVENT_PRESS, NULL, "Press"); // reenter On to restart our timers
powerFSM.add_transition(&stateSERIAL, &stateSERIAL, EVENT_PRESS, NULL, powerFSM.add_transition(&stateSERIAL, &stateSERIAL, EVENT_PRESS, NULL,
"Press"); // Allow button to work while in serial API "Press"); // Allow button to work while in serial API
// Handle critically low power battery by forcing deep sleep // Handle critically low power battery by forcing deep sleep
powerFSM.add_transition(&stateBOOT, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat"); powerFSM.add_transition(&stateBOOT, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat");
powerFSM.add_transition(&stateLS, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat"); powerFSM.add_transition(&stateLS, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat");
powerFSM.add_transition(&stateNB, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat"); powerFSM.add_transition(&stateNB, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat");
powerFSM.add_transition(&stateDARK, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat"); powerFSM.add_transition(&stateDARK, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat");
powerFSM.add_transition(&stateON, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat"); powerFSM.add_transition(&stateON, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat");
powerFSM.add_transition(&stateSERIAL, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat"); powerFSM.add_transition(&stateSERIAL, &stateLowBattSDS, EVENT_LOW_BATTERY, NULL, "LowBat");
// Handle being told to power off // Handle being told to power off
powerFSM.add_transition(&stateBOOT, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown"); powerFSM.add_transition(&stateBOOT, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateLS, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown"); powerFSM.add_transition(&stateLS, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateNB, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown"); powerFSM.add_transition(&stateNB, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateDARK, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown"); powerFSM.add_transition(&stateDARK, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateON, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown"); powerFSM.add_transition(&stateON, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateSERIAL, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown"); powerFSM.add_transition(&stateSERIAL, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
// Inputbroker // Inputbroker
powerFSM.add_transition(&stateLS, &stateON, EVENT_INPUT, NULL, "Input Device"); powerFSM.add_transition(&stateLS, &stateON, EVENT_INPUT, NULL, "Input Device");
powerFSM.add_transition(&stateNB, &stateON, EVENT_INPUT, NULL, "Input Device"); powerFSM.add_transition(&stateNB, &stateON, EVENT_INPUT, NULL, "Input Device");
powerFSM.add_transition(&stateDARK, &stateON, EVENT_INPUT, NULL, "Input Device"); powerFSM.add_transition(&stateDARK, &stateON, EVENT_INPUT, NULL, "Input Device");
powerFSM.add_transition(&stateON, &stateON, EVENT_INPUT, NULL, "Input Device"); // restarts the sleep timer powerFSM.add_transition(&stateON, &stateON, EVENT_INPUT, NULL, "Input Device"); // restarts the sleep timer
powerFSM.add_transition(&statePOWER, &statePOWER, EVENT_INPUT, NULL, "Input Device"); // restarts the sleep timer powerFSM.add_transition(&statePOWER, &statePOWER, EVENT_INPUT, NULL, "Input Device"); // restarts the sleep timer
powerFSM.add_transition(&stateDARK, &stateON, EVENT_BLUETOOTH_PAIR, NULL, "Bluetooth pairing"); powerFSM.add_transition(&stateDARK, &stateON, EVENT_BLUETOOTH_PAIR, NULL, "Bluetooth pairing");
powerFSM.add_transition(&stateON, &stateON, EVENT_BLUETOOTH_PAIR, NULL, "Bluetooth pairing"); powerFSM.add_transition(&stateON, &stateON, EVENT_BLUETOOTH_PAIR, NULL, "Bluetooth pairing");
// if we are a router we don't turn the screen on for these things // if we are a router we don't turn the screen on for these things
if (!isRouter) { if (!isRouter) {
// if any packet destined for phone arrives, turn on bluetooth at least // if any packet destined for phone arrives, turn on bluetooth at least
powerFSM.add_transition(&stateNB, &stateDARK, EVENT_PACKET_FOR_PHONE, NULL, "Packet for phone"); powerFSM.add_transition(&stateNB, &stateDARK, EVENT_PACKET_FOR_PHONE, NULL, "Packet for phone");
// Show the received text message // Show the received text message
powerFSM.add_transition(&stateLS, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text"); powerFSM.add_transition(&stateLS, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text");
powerFSM.add_transition(&stateNB, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text"); powerFSM.add_transition(&stateNB, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text");
powerFSM.add_transition(&stateDARK, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text"); powerFSM.add_transition(&stateDARK, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text");
powerFSM.add_transition(&stateON, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text"); // restarts the sleep timer powerFSM.add_transition(&stateON, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text"); // restarts the sleep timer
} }
// If we are not in statePOWER but get a serial connection, suppress sleep (and keep the screen on) while connected // If we are not in statePOWER but get a serial connection, suppress sleep (and keep the screen on) while connected
powerFSM.add_transition(&stateLS, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API"); powerFSM.add_transition(&stateLS, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API");
powerFSM.add_transition(&stateNB, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API"); powerFSM.add_transition(&stateNB, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API");
powerFSM.add_transition(&stateDARK, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API"); powerFSM.add_transition(&stateDARK, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API");
powerFSM.add_transition(&stateON, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API"); powerFSM.add_transition(&stateON, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API");
powerFSM.add_transition(&statePOWER, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API"); powerFSM.add_transition(&statePOWER, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API");
// If we get power connected, go to the power connect state // If we get power connected, go to the power connect state
powerFSM.add_transition(&stateLS, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect"); powerFSM.add_transition(&stateLS, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect");
powerFSM.add_transition(&stateNB, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect"); powerFSM.add_transition(&stateNB, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect");
powerFSM.add_transition(&stateDARK, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect"); powerFSM.add_transition(&stateDARK, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect");
powerFSM.add_transition(&stateON, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect"); powerFSM.add_transition(&stateON, &statePOWER, EVENT_POWER_CONNECTED, NULL, "power connect");
powerFSM.add_transition(&statePOWER, &stateON, EVENT_POWER_DISCONNECTED, NULL, "power disconnected"); powerFSM.add_transition(&statePOWER, &stateON, EVENT_POWER_DISCONNECTED, NULL, "power disconnected");
// powerFSM.add_transition(&stateSERIAL, &stateON, EVENT_POWER_DISCONNECTED, NULL, "power disconnected"); // powerFSM.add_transition(&stateSERIAL, &stateON, EVENT_POWER_DISCONNECTED, NULL, "power disconnected");
// the only way to leave state serial is for the client to disconnect (or we timeout and force disconnect them) // the only way to leave state serial is for the client to disconnect (or we timeout and force disconnect them)
// when we leave, go to ON (which might not be the correct state if we have power connected, we will fix that in onEnter) // when we leave, go to ON (which might not be the correct state if we have power connected, we will fix that in
powerFSM.add_transition(&stateSERIAL, &stateON, EVENT_SERIAL_DISCONNECTED, NULL, "serial disconnect"); // onEnter)
powerFSM.add_transition(&stateSERIAL, &stateON, EVENT_SERIAL_DISCONNECTED, NULL, "serial disconnect");
powerFSM.add_transition(&stateDARK, &stateDARK, EVENT_CONTACT_FROM_PHONE, NULL, "Contact from phone"); powerFSM.add_transition(&stateDARK, &stateDARK, EVENT_CONTACT_FROM_PHONE, NULL, "Contact from phone");
#ifdef USE_EINK #ifdef USE_EINK
// Allow E-Ink devices to suppress the screensaver, if screen timeout set to 0 // Allow E-Ink devices to suppress the screensaver, if screen timeout set to 0
if (config.display.screen_on_secs > 0) if (config.display.screen_on_secs > 0)
#endif #endif
{ {
powerFSM.add_timed_transition(&stateON, &stateDARK, powerFSM.add_timed_transition(&stateON, &stateDARK, Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs), NULL, "Screen-on timeout");
NULL, "Screen-on timeout"); powerFSM.add_timed_transition(&statePOWER, &stateDARK, Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
powerFSM.add_timed_transition(&statePOWER, &stateDARK, NULL, "Screen-on timeout");
Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs), }
NULL, "Screen-on timeout");
}
// We never enter light-sleep or NB states on NRF52 (because the CPU uses so little power normally) // We never enter light-sleep or NB states on NRF52 (because the CPU uses so little power normally)
#ifdef ARCH_ESP32 #ifdef ARCH_ESP32
// See: https://github.com/meshtastic/firmware/issues/1071 // See: https://github.com/meshtastic/firmware/issues/1071
// Don't add power saving transitions if we are a power saving tracker or sensor or have Wifi enabled. Sleep will be initiated // Don't add power saving transitions if we are a power saving tracker or sensor or have Wifi enabled. Sleep will be
// through the modules // initiated through the modules
#if HAS_WIFI && !defined(MESHTASTIC_EXCLUDE_WIFI) #if HAS_WIFI && !defined(MESHTASTIC_EXCLUDE_WIFI)
bool isTrackerOrSensor = config.device.role == meshtastic_Config_DeviceConfig_Role_TRACKER || bool isTrackerOrSensor = config.device.role == meshtastic_Config_DeviceConfig_Role_TRACKER ||
config.device.role == meshtastic_Config_DeviceConfig_Role_TAK_TRACKER || config.device.role == meshtastic_Config_DeviceConfig_Role_TAK_TRACKER ||
config.device.role == meshtastic_Config_DeviceConfig_Role_SENSOR; config.device.role == meshtastic_Config_DeviceConfig_Role_SENSOR;
if ((isRouter || config.power.is_power_saving) && !isWifiAvailable() && !isTrackerOrSensor) { if ((isRouter || config.power.is_power_saving) && !isWifiAvailable() && !isTrackerOrSensor) {
powerFSM.add_timed_transition(&stateNB, &stateLS, powerFSM.add_timed_transition(&stateNB, &stateLS, Default::getConfiguredOrDefaultMs(config.power.min_wake_secs, default_min_wake_secs), NULL,
Default::getConfiguredOrDefaultMs(config.power.min_wake_secs, default_min_wake_secs), NULL, "Min wake timeout");
"Min wake timeout");
// If ESP32 and using power-saving, timer mover from DARK to light-sleep // If ESP32 and using power-saving, timer mover from DARK to light-sleep
// Also serves purpose of the old DARK to DARK transition(?) See https://github.com/meshtastic/firmware/issues/3517 // Also serves purpose of the old DARK to DARK transition(?) See https://github.com/meshtastic/firmware/issues/3517
powerFSM.add_timed_transition( powerFSM.add_timed_transition(&stateDARK, &stateLS,
&stateDARK, &stateLS, Default::getConfiguredOrDefaultMs(config.power.wait_bluetooth_secs, default_wait_bluetooth_secs), NULL,
Default::getConfiguredOrDefaultMs(config.power.wait_bluetooth_secs, default_wait_bluetooth_secs), NULL, "Bluetooth timeout");
"Bluetooth timeout"); } else {
} else { // If ESP32, but not using power-saving, check periodically if config has drifted out of stateDark
// If ESP32, but not using power-saving, check periodically if config has drifted out of stateDark powerFSM.add_timed_transition(&stateDARK, &stateDARK, Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
powerFSM.add_timed_transition(&stateDARK, &stateDARK, NULL, "Screen-on timeout");
Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs), }
NULL, "Screen-on timeout");
}
#endif // HAS_WIFI || !defined(MESHTASTIC_EXCLUDE_WIFI) #endif // HAS_WIFI || !defined(MESHTASTIC_EXCLUDE_WIFI)
#else // (not) ARCH_ESP32 #else // (not) ARCH_ESP32
// If not ESP32, light-sleep not used. Check periodically if config has drifted out of stateDark // If not ESP32, light-sleep not used. Check periodically if config has drifted out of stateDark
powerFSM.add_timed_transition(&stateDARK, &stateDARK, powerFSM.add_timed_transition(&stateDARK, &stateDARK, Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs), NULL, NULL, "Screen-on timeout");
"Screen-on timeout");
#endif #endif
powerFSM.run_machine(); // run one iteration of the state machine, so we run our on enter tasks for the initial DARK state powerFSM.run_machine(); // run one iteration of the state machine, so we run our on enter tasks for the initial DARK state
} }
#endif #endif
src/PowerFSM.h
+14 -15
View File
@@ -17,7 +17,8 @@
#define EVENT_RECEIVED_MSG 5 #define EVENT_RECEIVED_MSG 5
// #define EVENT_BOOT 6 // now done with a timed transition // #define EVENT_BOOT 6 // now done with a timed transition
#define EVENT_BLUETOOTH_PAIR 7 #define EVENT_BLUETOOTH_PAIR 7
// #define EVENT_NODEDB_UPDATED 8 // Now defunct: NodeDB has a big enough change that we think you should turn on the screen // #define EVENT_NODEDB_UPDATED 8 // Now defunct: NodeDB has a big enough change that we think you should turn on
// the screen
#define EVENT_CONTACT_FROM_PHONE 9 // the phone just talked to us over bluetooth #define EVENT_CONTACT_FROM_PHONE 9 // the phone just talked to us over bluetooth
#define EVENT_LOW_BATTERY 10 // Battery is critically low, go to sleep #define EVENT_LOW_BATTERY 10 // Battery is critically low, go to sleep
#define EVENT_SERIAL_CONNECTED 11 #define EVENT_SERIAL_CONNECTED 11
@@ -29,21 +30,19 @@
#define EVENT_INPUT 17 // input broker wants something, we need to wake up and enable screen #define EVENT_INPUT 17 // input broker wants something, we need to wake up and enable screen
#if MESHTASTIC_EXCLUDE_POWER_FSM #if MESHTASTIC_EXCLUDE_POWER_FSM
class FakeFsm class FakeFsm {
{ public:
public: void trigger(int event) {
void trigger(int event) if (event == EVENT_SERIAL_CONNECTED) {
{ serialConnected = true;
if (event == EVENT_SERIAL_CONNECTED) { } else if (event == EVENT_SERIAL_DISCONNECTED) {
serialConnected = true; serialConnected = false;
} else if (event == EVENT_SERIAL_DISCONNECTED) { }
serialConnected = false; };
} bool getState() { return serialConnected; };
};
bool getState() { return serialConnected; };
private: private:
bool serialConnected = false; bool serialConnected = false;
}; };
extern FakeFsm powerFSM; extern FakeFsm powerFSM;
void PowerFSM_setup(); void PowerFSM_setup();
src/PowerFSMThread.h
+24 -28
View File
@@ -6,40 +6,36 @@
#include "main.h" #include "main.h"
#include "power.h" #include "power.h"
namespace concurrency namespace concurrency {
{
/// Wrapper to convert our powerFSM stuff into a 'thread' /// Wrapper to convert our powerFSM stuff into a 'thread'
class PowerFSMThread : public OSThread class PowerFSMThread : public OSThread {
{ public:
public: // callback returns the period for the next callback invocation (or 0 if we should no longer be called)
// callback returns the period for the next callback invocation (or 0 if we should no longer be called) PowerFSMThread() : OSThread("PowerFSM") {}
PowerFSMThread() : OSThread("PowerFSM") {}
protected: protected:
int32_t runOnce() override int32_t runOnce() override {
{
#if !MESHTASTIC_EXCLUDE_POWER_FSM #if !MESHTASTIC_EXCLUDE_POWER_FSM
powerFSM.run_machine(); powerFSM.run_machine();
/// If we are in power state we force the CPU to wake every 10ms to check for serial characters (we don't yet wake /// If we are in power state we force the CPU to wake every 10ms to check for serial characters (we don't yet wake
/// cpu for serial rx - FIXME) /// cpu for serial rx - FIXME)
const State *state = powerFSM.getState(); const State *state = powerFSM.getState();
canSleep = (state != &statePOWER) && (state != &stateSERIAL); canSleep = (state != &statePOWER) && (state != &stateSERIAL);
if (powerStatus->getHasUSB()) { if (powerStatus->getHasUSB()) {
timeLastPowered = millis(); timeLastPowered = millis();
} else if (config.power.on_battery_shutdown_after_secs > 0 && config.power.on_battery_shutdown_after_secs != UINT32_MAX && } else if (config.power.on_battery_shutdown_after_secs > 0 && config.power.on_battery_shutdown_after_secs != UINT32_MAX &&
millis() > (timeLastPowered + millis() > (timeLastPowered +
Default::getConfiguredOrDefaultMs( Default::getConfiguredOrDefaultMs(config.power.on_battery_shutdown_after_secs))) { // shutdown after 30 minutes unpowered
config.power.on_battery_shutdown_after_secs))) { // shutdown after 30 minutes unpowered powerFSM.trigger(EVENT_SHUTDOWN);
powerFSM.trigger(EVENT_SHUTDOWN);
}
return 100;
#else
return INT32_MAX;
#endif
} }
return 100;
#else
return INT32_MAX;
#endif
}
}; };
} // namespace concurrency } // namespace concurrency
src/PowerMon.cpp
+20 -27
View File
@@ -2,46 +2,39 @@
#include "NodeDB.h" #include "NodeDB.h"
// Use the 'live' config flag to figure out if we should be showing this message // Use the 'live' config flag to figure out if we should be showing this message
bool PowerMon::is_power_enabled(uint64_t m) bool PowerMon::is_power_enabled(uint64_t m) {
{ // FIXME: VERY STRANGE BUG: if I or in "force_enabled || " the flashed image on a rak4631 is not accepted by the
// FIXME: VERY STRANGE BUG: if I or in "force_enabled || " the flashed image on a rak4631 is not accepted by the bootloader as // bootloader as valid!!! Possibly a linker/gcc/bootloader bug somewhere?
// valid!!! Possibly a linker/gcc/bootloader bug somewhere? return ((m & config.power.powermon_enables) ? true : false);
return ((m & config.power.powermon_enables) ? true : false);
} }
void PowerMon::setState(_meshtastic_PowerMon_State state, const char *reason) void PowerMon::setState(_meshtastic_PowerMon_State state, const char *reason) {
{
#ifdef USE_POWERMON #ifdef USE_POWERMON
auto oldstates = states; auto oldstates = states;
states |= state; states |= state;
if (oldstates != states && is_power_enabled(state)) { if (oldstates != states && is_power_enabled(state)) {
emitLog(reason); emitLog(reason);
} }
#endif #endif
} }
void PowerMon::clearState(_meshtastic_PowerMon_State state, const char *reason) void PowerMon::clearState(_meshtastic_PowerMon_State state, const char *reason) {
{
#ifdef USE_POWERMON #ifdef USE_POWERMON
auto oldstates = states; auto oldstates = states;
states &= ~state; states &= ~state;
if (oldstates != states && is_power_enabled(state)) { if (oldstates != states && is_power_enabled(state)) {
emitLog(reason); emitLog(reason);
} }
#endif #endif
} }
void PowerMon::emitLog(const char *reason) void PowerMon::emitLog(const char *reason) {
{
#ifdef USE_POWERMON #ifdef USE_POWERMON
// The nrf52 printf doesn't understand 64 bit ints, so if we ever reach that point this function will need to change. // The nrf52 printf doesn't understand 64 bit ints, so if we ever reach that point this function will need to change.
LOG_INFO("S:PM:0x%08lx,%s", (uint32_t)states, reason); LOG_INFO("S:PM:0x%08lx,%s", (uint32_t)states, reason);
#endif #endif
} }
PowerMon *powerMon; PowerMon *powerMon;
void powerMonInit() void powerMonInit() { powerMon = new PowerMon(); }
{
powerMon = new PowerMon();
}
src/PowerMon.h
+17 -18
View File
@@ -13,30 +13,29 @@
* *
* For more information see the PowerMon docs. * For more information see the PowerMon docs.
*/ */
class PowerMon class PowerMon {
{ uint64_t states = 0UL;
uint64_t states = 0UL;
friend class PowerStressModule; friend class PowerStressModule;
/** /**
* If stress testing we always want all events logged * If stress testing we always want all events logged
*/ */
bool force_enabled = false; bool force_enabled = false;
public: public:
PowerMon() {} PowerMon() {}
// Mark entry/exit of a power consuming state // Mark entry/exit of a power consuming state
void setState(_meshtastic_PowerMon_State state, const char *reason = ""); void setState(_meshtastic_PowerMon_State state, const char *reason = "");
void clearState(_meshtastic_PowerMon_State state, const char *reason = ""); void clearState(_meshtastic_PowerMon_State state, const char *reason = "");
private: private:
// Emit the coded log message // Emit the coded log message
void emitLog(const char *reason); void emitLog(const char *reason);
// Use the 'live' config flag to figure out if we should be showing this message // Use the 'live' config flag to figure out if we should be showing this message
bool is_power_enabled(uint64_t m); bool is_power_enabled(uint64_t m);
}; };
extern PowerMon *powerMon; extern PowerMon *powerMon;
src/PowerStatus.h
+60 -67
View File
@@ -3,8 +3,7 @@
#include "configuration.h" #include "configuration.h"
#include <Arduino.h> #include <Arduino.h>
namespace meshtastic namespace meshtastic {
{
/** /**
* A boolean where we have a third state of Unknown * A boolean where we have a third state of Unknown
@@ -12,90 +11,84 @@ namespace meshtastic
enum OptionalBool { OptFalse = 0, OptTrue = 1, OptUnknown = 2 }; enum OptionalBool { OptFalse = 0, OptTrue = 1, OptUnknown = 2 };
/// Describes the state of the Power system. /// Describes the state of the Power system.
class PowerStatus : public Status class PowerStatus : public Status {
{
private: private:
CallbackObserver<PowerStatus, const PowerStatus *> statusObserver = CallbackObserver<PowerStatus, const PowerStatus *> statusObserver =
CallbackObserver<PowerStatus, const PowerStatus *>(this, &PowerStatus::updateStatus); CallbackObserver<PowerStatus, const PowerStatus *>(this, &PowerStatus::updateStatus);
/// Whether we have a battery connected /// Whether we have a battery connected
OptionalBool hasBattery = OptUnknown; OptionalBool hasBattery = OptUnknown;
/// Battery voltage in mV, valid if haveBattery is true /// Battery voltage in mV, valid if haveBattery is true
int batteryVoltageMv = 0; int batteryVoltageMv = 0;
/// Battery charge percentage, either read directly or estimated /// Battery charge percentage, either read directly or estimated
int8_t batteryChargePercent = 0; int8_t batteryChargePercent = 0;
/// Whether USB is connected /// Whether USB is connected
OptionalBool hasUSB = OptUnknown; OptionalBool hasUSB = OptUnknown;
/// Whether we are charging the battery /// Whether we are charging the battery
OptionalBool isCharging = OptUnknown; OptionalBool isCharging = OptUnknown;
public: public:
PowerStatus() { statusType = STATUS_TYPE_POWER; } PowerStatus() { statusType = STATUS_TYPE_POWER; }
PowerStatus(OptionalBool hasBattery, OptionalBool hasUSB, OptionalBool isCharging, int batteryVoltageMv = -1, PowerStatus(OptionalBool hasBattery, OptionalBool hasUSB, OptionalBool isCharging, int batteryVoltageMv = -1, int8_t batteryChargePercent = 0)
int8_t batteryChargePercent = 0) : Status() {
: Status() this->hasBattery = hasBattery;
{ this->hasUSB = hasUSB;
this->hasBattery = hasBattery; this->isCharging = isCharging;
this->hasUSB = hasUSB; this->batteryVoltageMv = batteryVoltageMv;
this->isCharging = isCharging; this->batteryChargePercent = batteryChargePercent;
this->batteryVoltageMv = batteryVoltageMv; }
this->batteryChargePercent = batteryChargePercent; PowerStatus(const PowerStatus &);
} PowerStatus &operator=(const PowerStatus &);
PowerStatus(const PowerStatus &);
PowerStatus &operator=(const PowerStatus &);
void observe(Observable<const PowerStatus *> *source) { statusObserver.observe(source); } void observe(Observable<const PowerStatus *> *source) { statusObserver.observe(source); }
bool getHasBattery() const { return hasBattery == OptTrue; } bool getHasBattery() const { return hasBattery == OptTrue; }
bool getHasUSB() const { return hasUSB == OptTrue; } bool getHasUSB() const { return hasUSB == OptTrue; }
/// Can we even know if this board has USB power or not /// Can we even know if this board has USB power or not
bool knowsUSB() const { return hasUSB != OptUnknown; } bool knowsUSB() const { return hasUSB != OptUnknown; }
bool getIsCharging() const { return isCharging == OptTrue; } bool getIsCharging() const { return isCharging == OptTrue; }
int getBatteryVoltageMv() const { return batteryVoltageMv; } int getBatteryVoltageMv() const { return batteryVoltageMv; }
/** /**
* Note: for boards with battery pin or PMU, 0% battery means 'unknown/this board doesn't have a battery installed' * Note: for boards with battery pin or PMU, 0% battery means 'unknown/this board doesn't have a battery installed'
*/ */
#if defined(HAS_PMU) || defined(BATTERY_PIN) #if defined(HAS_PMU) || defined(BATTERY_PIN)
uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 0; } uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 0; }
#endif #endif
/** /**
* Note: for boards without battery pin and PMU, 101% battery means 'the board is using external power' * Note: for boards without battery pin and PMU, 101% battery means 'the board is using external power'
*/ */
#if !defined(HAS_PMU) && !defined(BATTERY_PIN) #if !defined(HAS_PMU) && !defined(BATTERY_PIN)
uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 101; } uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 101; }
#endif #endif
bool matches(const PowerStatus *newStatus) const bool matches(const PowerStatus *newStatus) const {
return (newStatus->getHasBattery() != hasBattery || newStatus->getHasUSB() != hasUSB || newStatus->getBatteryVoltageMv() != batteryVoltageMv);
}
int updateStatus(const PowerStatus *newStatus) {
// Only update the status if values have actually changed
bool isDirty;
{ {
return (newStatus->getHasBattery() != hasBattery || newStatus->getHasUSB() != hasUSB || isDirty = matches(newStatus);
newStatus->getBatteryVoltageMv() != batteryVoltageMv); initialized = true;
hasBattery = newStatus->hasBattery;
batteryVoltageMv = newStatus->getBatteryVoltageMv();
batteryChargePercent = newStatus->getBatteryChargePercent();
hasUSB = newStatus->hasUSB;
isCharging = newStatus->isCharging;
} }
int updateStatus(const PowerStatus *newStatus) if (isDirty) {
{ // LOG_DEBUG("Battery %dmV %d%%", batteryVoltageMv, batteryChargePercent);
// Only update the status if values have actually changed onNewStatus.notifyObservers(this);
bool isDirty;
{
isDirty = matches(newStatus);
initialized = true;
hasBattery = newStatus->hasBattery;
batteryVoltageMv = newStatus->getBatteryVoltageMv();
batteryChargePercent = newStatus->getBatteryChargePercent();
hasUSB = newStatus->hasUSB;
isCharging = newStatus->isCharging;
}
if (isDirty) {
// LOG_DEBUG("Battery %dmV %d%%", batteryVoltageMv, batteryChargePercent);
onNewStatus.notifyObservers(this);
}
return 0;
} }
return 0;
}
}; };
} // namespace meshtastic } // namespace meshtastic
src/RedirectablePrint.cpp
+307 -318
View File
@@ -20,387 +20,376 @@
#if HAS_NETWORKING #if HAS_NETWORKING
extern Syslog syslog; extern Syslog syslog;
#endif #endif
void RedirectablePrint::rpInit() void RedirectablePrint::rpInit() {
{
#ifdef HAS_FREE_RTOS #ifdef HAS_FREE_RTOS
inDebugPrint = xSemaphoreCreateMutexStatic(&this->_MutexStorageSpace); inDebugPrint = xSemaphoreCreateMutexStatic(&this->_MutexStorageSpace);
#endif #endif
} }
void RedirectablePrint::setDestination(Print *_dest) void RedirectablePrint::setDestination(Print *_dest) {
{ assert(_dest);
assert(_dest); dest = _dest;
dest = _dest;
} }
size_t RedirectablePrint::write(uint8_t c) size_t RedirectablePrint::write(uint8_t c) {
{ // Always send the characters to our segger JTAG debugger
// Always send the characters to our segger JTAG debugger
#ifdef USE_SEGGER #ifdef USE_SEGGER
SEGGER_RTT_PutChar(SEGGER_STDOUT_CH, c); SEGGER_RTT_PutChar(SEGGER_STDOUT_CH, c);
#endif #endif
// Account for legacy config transition // Account for legacy config transition
bool serialEnabled = config.has_security ? config.security.serial_enabled : config.device.serial_enabled; bool serialEnabled = config.has_security ? config.security.serial_enabled : config.device.serial_enabled;
if (!config.has_lora || serialEnabled) if (!config.has_lora || serialEnabled)
dest->write(c); dest->write(c);
return 1; // We always claim one was written, rather than trusting what the return 1; // We always claim one was written, rather than trusting what the
// serial port said (which could be zero) // serial port said (which could be zero)
} }
size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_list arg) size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_list arg) {
{ va_list copy;
va_list copy;
#if ENABLE_JSON_LOGGING || ARCH_PORTDUINO #if ENABLE_JSON_LOGGING || ARCH_PORTDUINO
static char printBuf[512]; static char printBuf[512];
#else #else
static char printBuf[160]; static char printBuf[160];
#endif #endif
#ifdef ARCH_PORTDUINO #ifdef ARCH_PORTDUINO
bool color = !portduino_config.ascii_logs; bool color = !portduino_config.ascii_logs;
#else #else
bool color = true; bool color = true;
#endif #endif
va_copy(copy, arg); va_copy(copy, arg);
size_t len = vsnprintf(printBuf, sizeof(printBuf), format, copy); size_t len = vsnprintf(printBuf, sizeof(printBuf), format, copy);
va_end(copy); va_end(copy);
// If the resulting string is longer than sizeof(printBuf)-1 characters, the remaining characters are still counted for the // If the resulting string is longer than sizeof(printBuf)-1 characters, the remaining characters are still counted
// return value // for the return value
if (len > sizeof(printBuf) - 1) { if (len > sizeof(printBuf) - 1) {
len = sizeof(printBuf) - 1; len = sizeof(printBuf) - 1;
printBuf[sizeof(printBuf) - 2] = '\n'; printBuf[sizeof(printBuf) - 2] = '\n';
} }
for (size_t f = 0; f < len; f++) { for (size_t f = 0; f < len; f++) {
if (!std::isprint(static_cast<unsigned char>(printBuf[f])) && printBuf[f] != '\n') if (!std::isprint(static_cast<unsigned char>(printBuf[f])) && printBuf[f] != '\n')
printBuf[f] = '#'; printBuf[f] = '#';
} }
if (color && logLevel != nullptr) { if (color && logLevel != nullptr) {
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
Print::write("\u001b[34m", 5); Print::write("\u001b[34m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0) if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
Print::write("\u001b[32m", 5); Print::write("\u001b[32m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0) if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
Print::write("\u001b[33m", 5); Print::write("\u001b[33m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0) if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
Print::write("\u001b[31m", 5); Print::write("\u001b[31m", 5);
} }
len = Print::write(printBuf, len); len = Print::write(printBuf, len);
if (color && logLevel != nullptr) { if (color && logLevel != nullptr) {
Print::write("\u001b[0m", 4); Print::write("\u001b[0m", 4);
} }
return len; return len;
} }
void RedirectablePrint::log_to_serial(const char *logLevel, const char *format, va_list arg) void RedirectablePrint::log_to_serial(const char *logLevel, const char *format, va_list arg) {
{ size_t r = 0;
size_t r = 0;
#ifdef ARCH_PORTDUINO #ifdef ARCH_PORTDUINO
bool color = !portduino_config.ascii_logs; bool color = !portduino_config.ascii_logs;
#else #else
bool color = true; bool color = true;
#endif #endif
// include the header // include the header
if (color) {
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0)
Print::write("\u001b[34m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
Print::write("\u001b[32m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
Print::write("\u001b[33m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
Print::write("\u001b[31m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0)
Print::write("\u001b[35m", 5);
}
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); // display local time on logfile
if (rtc_sec > 0) {
long hms = rtc_sec % SEC_PER_DAY;
// hms += tz.tz_dsttime * SEC_PER_HOUR;
// hms -= tz.tz_minuteswest * SEC_PER_MIN;
// mod `hms` to ensure in positive range of [0...SEC_PER_DAY)
hms = (hms + SEC_PER_DAY) % SEC_PER_DAY;
// Tear apart hms into h:m:s
int hour = hms / SEC_PER_HOUR;
int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN
#ifdef ARCH_PORTDUINO
::printf("%s ", logLevel);
if (color) { if (color) {
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) ::printf("\u001b[0m");
Print::write("\u001b[34m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0)
Print::write("\u001b[32m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0)
Print::write("\u001b[33m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_ERROR) == 0)
Print::write("\u001b[31m", 5);
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0)
Print::write("\u001b[35m", 5);
} }
::printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); // display local time on logfile
if (rtc_sec > 0) {
long hms = rtc_sec % SEC_PER_DAY;
// hms += tz.tz_dsttime * SEC_PER_HOUR;
// hms -= tz.tz_minuteswest * SEC_PER_MIN;
// mod `hms` to ensure in positive range of [0...SEC_PER_DAY)
hms = (hms + SEC_PER_DAY) % SEC_PER_DAY;
// Tear apart hms into h:m:s
int hour = hms / SEC_PER_HOUR;
int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN
#ifdef ARCH_PORTDUINO
::printf("%s ", logLevel);
if (color) {
::printf("\u001b[0m");
}
::printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
#else #else
printf("%s ", logLevel); printf("%s ", logLevel);
if (color) { if (color) {
printf("\u001b[0m"); printf("\u001b[0m");
} }
printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000); printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
#endif #endif
} else { } else {
#ifdef ARCH_PORTDUINO #ifdef ARCH_PORTDUINO
::printf("%s ", logLevel); ::printf("%s ", logLevel);
if (color) { if (color) {
::printf("\u001b[0m"); ::printf("\u001b[0m");
} }
::printf("| ??:??:?? %u ", millis() / 1000); ::printf("| ??:??:?? %u ", millis() / 1000);
#else #else
printf("%s ", logLevel); printf("%s ", logLevel);
if (color) { if (color) {
printf("\u001b[0m"); printf("\u001b[0m");
} }
printf("| ??:??:?? %u ", millis() / 1000); printf("| ??:??:?? %u ", millis() / 1000);
#endif #endif
} }
auto thread = concurrency::OSThread::currentThread; auto thread = concurrency::OSThread::currentThread;
if (thread) { if (thread) {
print("["); print("[");
// printf("%p ", thread); // printf("%p ", thread);
// assert(thread->ThreadName.length()); // assert(thread->ThreadName.length());
print(thread->ThreadName); print(thread->ThreadName);
print("] "); print("] ");
} }
#ifdef DEBUG_HEAP #ifdef DEBUG_HEAP
// Add heap free space bytes prefix before every log message // Add heap free space bytes prefix before every log message
#ifdef ARCH_PORTDUINO #ifdef ARCH_PORTDUINO
::printf("[heap %u] ", memGet.getFreeHeap()); ::printf("[heap %u] ", memGet.getFreeHeap());
#else #else
printf("[heap %u] ", memGet.getFreeHeap()); printf("[heap %u] ", memGet.getFreeHeap());
#endif #endif
#endif // DEBUG_HEAP #endif // DEBUG_HEAP
r += vprintf(logLevel, format, arg); r += vprintf(logLevel, format, arg);
} }
void RedirectablePrint::log_to_syslog(const char *logLevel, const char *format, va_list arg) void RedirectablePrint::log_to_syslog(const char *logLevel, const char *format, va_list arg) {
{
#if HAS_NETWORKING && !defined(ARCH_PORTDUINO) #if HAS_NETWORKING && !defined(ARCH_PORTDUINO)
// if syslog is in use, collect the log messages and send them to syslog // if syslog is in use, collect the log messages and send them to syslog
if (syslog.isEnabled()) { if (syslog.isEnabled()) {
int ll = 0; int ll = 0;
switch (logLevel[0]) {
case 'D':
ll = SYSLOG_DEBUG;
break;
case 'I':
ll = SYSLOG_INFO;
break;
case 'W':
ll = SYSLOG_WARN;
break;
case 'E':
ll = SYSLOG_ERR;
break;
case 'C':
ll = SYSLOG_CRIT;
break;
default:
ll = 0;
}
auto thread = concurrency::OSThread::currentThread;
if (thread) {
syslog.vlogf(ll, thread->ThreadName.c_str(), format, arg);
} else {
syslog.vlogf(ll, format, arg);
}
}
#endif
}
void RedirectablePrint::log_to_ble(const char *logLevel, const char *format, va_list arg)
{
#if !MESHTASTIC_EXCLUDE_BLUETOOTH
if (config.security.debug_log_api_enabled && !pauseBluetoothLogging) {
bool isBleConnected = false;
#ifdef ARCH_ESP32
isBleConnected = nimbleBluetooth && nimbleBluetooth->isActive() && nimbleBluetooth->isConnected();
#elif defined(ARCH_NRF52)
isBleConnected = nrf52Bluetooth != nullptr && nrf52Bluetooth->isConnected();
#endif
if (isBleConnected) {
char *message;
size_t initialLen;
size_t len;
initialLen = strlen(format);
message = new char[initialLen + 1];
len = vsnprintf(message, initialLen + 1, format, arg);
if (len > initialLen) {
delete[] message;
message = new char[len + 1];
vsnprintf(message, len + 1, format, arg);
}
auto thread = concurrency::OSThread::currentThread;
meshtastic_LogRecord logRecord = meshtastic_LogRecord_init_zero;
logRecord.level = getLogLevel(logLevel);
strcpy(logRecord.message, message);
if (thread)
strcpy(logRecord.source, thread->ThreadName.c_str());
logRecord.time = getValidTime(RTCQuality::RTCQualityDevice, true);
uint8_t *buffer = new uint8_t[meshtastic_LogRecord_size];
size_t size = pb_encode_to_bytes(buffer, meshtastic_LogRecord_size, meshtastic_LogRecord_fields, &logRecord);
#ifdef ARCH_ESP32
nimbleBluetooth->sendLog(buffer, size);
#elif defined(ARCH_NRF52)
nrf52Bluetooth->sendLog(buffer, size);
#endif
delete[] message;
delete[] buffer;
}
}
#else
(void)logLevel;
(void)format;
(void)arg;
#endif
}
meshtastic_LogRecord_Level RedirectablePrint::getLogLevel(const char *logLevel)
{
meshtastic_LogRecord_Level ll = meshtastic_LogRecord_Level_UNSET; // default to unset
switch (logLevel[0]) { switch (logLevel[0]) {
case 'D': case 'D':
ll = meshtastic_LogRecord_Level_DEBUG; ll = SYSLOG_DEBUG;
break; break;
case 'I': case 'I':
ll = meshtastic_LogRecord_Level_INFO; ll = SYSLOG_INFO;
break; break;
case 'W': case 'W':
ll = meshtastic_LogRecord_Level_WARNING; ll = SYSLOG_WARN;
break; break;
case 'E': case 'E':
ll = meshtastic_LogRecord_Level_ERROR; ll = SYSLOG_ERR;
break; break;
case 'C': case 'C':
ll = meshtastic_LogRecord_Level_CRITICAL; ll = SYSLOG_CRIT;
break; break;
default:
ll = 0;
} }
return ll; auto thread = concurrency::OSThread::currentThread;
if (thread) {
syslog.vlogf(ll, thread->ThreadName.c_str(), format, arg);
} else {
syslog.vlogf(ll, format, arg);
}
}
#endif
} }
void RedirectablePrint::log(const char *logLevel, const char *format, ...) void RedirectablePrint::log_to_ble(const char *logLevel, const char *format, va_list arg) {
{ #if !MESHTASTIC_EXCLUDE_BLUETOOTH
if (config.security.debug_log_api_enabled && !pauseBluetoothLogging) {
bool isBleConnected = false;
#ifdef ARCH_ESP32
isBleConnected = nimbleBluetooth && nimbleBluetooth->isActive() && nimbleBluetooth->isConnected();
#elif defined(ARCH_NRF52)
isBleConnected = nrf52Bluetooth != nullptr && nrf52Bluetooth->isConnected();
#endif
if (isBleConnected) {
char *message;
size_t initialLen;
size_t len;
initialLen = strlen(format);
message = new char[initialLen + 1];
len = vsnprintf(message, initialLen + 1, format, arg);
if (len > initialLen) {
delete[] message;
message = new char[len + 1];
vsnprintf(message, len + 1, format, arg);
}
auto thread = concurrency::OSThread::currentThread;
meshtastic_LogRecord logRecord = meshtastic_LogRecord_init_zero;
logRecord.level = getLogLevel(logLevel);
strcpy(logRecord.message, message);
if (thread)
strcpy(logRecord.source, thread->ThreadName.c_str());
logRecord.time = getValidTime(RTCQuality::RTCQualityDevice, true);
// append \n to format uint8_t *buffer = new uint8_t[meshtastic_LogRecord_size];
size_t len = strlen(format); size_t size = pb_encode_to_bytes(buffer, meshtastic_LogRecord_size, meshtastic_LogRecord_fields, &logRecord);
char *newFormat = new char[len + 2]; #ifdef ARCH_ESP32
strcpy(newFormat, format); nimbleBluetooth->sendLog(buffer, size);
newFormat[len] = '\n'; #elif defined(ARCH_NRF52)
newFormat[len + 1] = '\0'; nrf52Bluetooth->sendLog(buffer, size);
#endif
delete[] message;
delete[] buffer;
}
}
#else
(void)logLevel;
(void)format;
(void)arg;
#endif
}
meshtastic_LogRecord_Level RedirectablePrint::getLogLevel(const char *logLevel) {
meshtastic_LogRecord_Level ll = meshtastic_LogRecord_Level_UNSET; // default to unset
switch (logLevel[0]) {
case 'D':
ll = meshtastic_LogRecord_Level_DEBUG;
break;
case 'I':
ll = meshtastic_LogRecord_Level_INFO;
break;
case 'W':
ll = meshtastic_LogRecord_Level_WARNING;
break;
case 'E':
ll = meshtastic_LogRecord_Level_ERROR;
break;
case 'C':
ll = meshtastic_LogRecord_Level_CRITICAL;
break;
}
return ll;
}
void RedirectablePrint::log(const char *logLevel, const char *format, ...) {
// append \n to format
size_t len = strlen(format);
char *newFormat = new char[len + 2];
strcpy(newFormat, format);
newFormat[len] = '\n';
newFormat[len + 1] = '\0';
#if ARCH_PORTDUINO #if ARCH_PORTDUINO
// level trace is special, two possible ways to handle it. // level trace is special, two possible ways to handle it.
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) { if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) {
if (portduino_config.traceFilename != "") { if (portduino_config.traceFilename != "") {
va_list arg; va_list arg;
va_start(arg, format); va_start(arg, format);
try { try {
traceFile << va_arg(arg, char *) << std::endl; traceFile << va_arg(arg, char *) << std::endl;
} catch (const std::ios_base::failure &e) { } catch (const std::ios_base::failure &e) {
} }
va_end(arg); va_end(arg);
}
if (portduino_config.logoutputlevel < level_trace && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) {
delete[] newFormat;
return;
}
} }
if (portduino_config.logoutputlevel < level_debug && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) { if (portduino_config.logoutputlevel < level_trace && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) {
delete[] newFormat; delete[] newFormat;
return; return;
} else if (portduino_config.logoutputlevel < level_info && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0) {
delete[] newFormat;
return;
} else if (portduino_config.logoutputlevel < level_warn && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0) {
delete[] newFormat;
return;
} }
#endif }
if (moduleConfig.serial.override_console_serial_port && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) { if (portduino_config.logoutputlevel < level_debug && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) {
delete[] newFormat;
return;
}
#ifdef HAS_FREE_RTOS
if (inDebugPrint != nullptr && xSemaphoreTake(inDebugPrint, portMAX_DELAY) == pdTRUE) {
#else
if (!inDebugPrint) {
inDebugPrint = true;
#endif
va_list arg;
va_start(arg, format);
log_to_serial(logLevel, newFormat, arg);
log_to_syslog(logLevel, newFormat, arg);
log_to_ble(logLevel, newFormat, arg);
va_end(arg);
#ifdef HAS_FREE_RTOS
xSemaphoreGive(inDebugPrint);
#else
inDebugPrint = false;
#endif
}
delete[] newFormat; delete[] newFormat;
return; return;
} else if (portduino_config.logoutputlevel < level_info && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_INFO) == 0) {
delete[] newFormat;
return;
} else if (portduino_config.logoutputlevel < level_warn && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_WARN) == 0) {
delete[] newFormat;
return;
}
#endif
if (moduleConfig.serial.override_console_serial_port && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) {
delete[] newFormat;
return;
}
#ifdef HAS_FREE_RTOS
if (inDebugPrint != nullptr && xSemaphoreTake(inDebugPrint, portMAX_DELAY) == pdTRUE) {
#else
if (!inDebugPrint) {
inDebugPrint = true;
#endif
va_list arg;
va_start(arg, format);
log_to_serial(logLevel, newFormat, arg);
log_to_syslog(logLevel, newFormat, arg);
log_to_ble(logLevel, newFormat, arg);
va_end(arg);
#ifdef HAS_FREE_RTOS
xSemaphoreGive(inDebugPrint);
#else
inDebugPrint = false;
#endif
}
delete[] newFormat;
return;
} }
void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len) void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len) {
{ const char alphabet[17] = "0123456789abcdef";
const char alphabet[17] = "0123456789abcdef"; log(logLevel, " +------------------------------------------------+ +----------------+");
log(logLevel, " +------------------------------------------------+ +----------------+"); log(logLevel, " |.0 .1 .2 .3 .4 .5 .6 .7 .8 .9 .a .b .c .d .e .f | | ASCII |");
log(logLevel, " |.0 .1 .2 .3 .4 .5 .6 .7 .8 .9 .a .b .c .d .e .f | | ASCII |"); for (uint16_t i = 0; i < len; i += 16) {
for (uint16_t i = 0; i < len; i += 16) { if (i % 128 == 0)
if (i % 128 == 0) log(logLevel, " +------------------------------------------------+ +----------------+");
log(logLevel, " +------------------------------------------------+ +----------------+"); char s[] = " | | | |\n";
char s[] = " | | | |\n"; uint8_t ix = 5, iy = 56;
uint8_t ix = 5, iy = 56; for (uint8_t j = 0; j < 16; j++) {
for (uint8_t j = 0; j < 16; j++) { if (i + j < len) {
if (i + j < len) { uint8_t c = buf[i + j];
uint8_t c = buf[i + j]; s[ix++] = alphabet[(c >> 4) & 0x0F];
s[ix++] = alphabet[(c >> 4) & 0x0F]; s[ix++] = alphabet[c & 0x0F];
s[ix++] = alphabet[c & 0x0F]; ix++;
ix++; if (c > 31 && c < 128)
if (c > 31 && c < 128) s[iy++] = c;
s[iy++] = c;
else
s[iy++] = '.';
}
}
uint8_t index = i / 16;
sprintf(s, "%03x", index);
s[3] = '.';
log(logLevel, s);
}
log(logLevel, " +------------------------------------------------+ +----------------+");
}
std::string RedirectablePrint::mt_sprintf(const std::string fmt_str, ...)
{
int n = ((int)fmt_str.size()) * 2; /* Reserve two times as much as the length of the fmt_str */
std::unique_ptr<char[]> formatted;
va_list ap;
while (1) {
formatted.reset(new char[n]); /* Wrap the plain char array into the unique_ptr */
strcpy(&formatted[0], fmt_str.c_str());
va_start(ap, fmt_str);
int final_n = vsnprintf(&formatted[0], n, fmt_str.c_str(), ap);
va_end(ap);
if (final_n < 0 || final_n >= n)
n += abs(final_n - n + 1);
else else
break; s[iy++] = '.';
}
} }
return std::string(formatted.get()); uint8_t index = i / 16;
sprintf(s, "%03x", index);
s[3] = '.';
log(logLevel, s);
}
log(logLevel, " +------------------------------------------------+ +----------------+");
}
std::string RedirectablePrint::mt_sprintf(const std::string fmt_str, ...) {
int n = ((int)fmt_str.size()) * 2; /* Reserve two times as much as the length of the fmt_str */
std::unique_ptr<char[]> formatted;
va_list ap;
while (1) {
formatted.reset(new char[n]); /* Wrap the plain char array into the unique_ptr */
strcpy(&formatted[0], fmt_str.c_str());
va_start(ap, fmt_str);
int final_n = vsnprintf(&formatted[0], n, fmt_str.c_str(), ap);
va_end(ap);
if (final_n < 0 || final_n >= n)
n += abs(final_n - n + 1);
else
break;
}
return std::string(formatted.get());
} }
src/RedirectablePrint.h
+32 -33
View File
@@ -11,49 +11,48 @@
* This class is mostly useful to allow debug printing to be redirected away from Serial * This class is mostly useful to allow debug printing to be redirected away from Serial
* to some other transport if we switch Serial usage (on the fly) to some other purpose. * to some other transport if we switch Serial usage (on the fly) to some other purpose.
*/ */
class RedirectablePrint : public Print class RedirectablePrint : public Print {
{ Print *dest;
Print *dest;
#ifdef HAS_FREE_RTOS #ifdef HAS_FREE_RTOS
SemaphoreHandle_t inDebugPrint = nullptr; SemaphoreHandle_t inDebugPrint = nullptr;
StaticSemaphore_t _MutexStorageSpace; StaticSemaphore_t _MutexStorageSpace;
#else #else
volatile bool inDebugPrint = false; volatile bool inDebugPrint = false;
#endif #endif
public: public:
explicit RedirectablePrint(Print *_dest) : dest(_dest) {} explicit RedirectablePrint(Print *_dest) : dest(_dest) {}
/** /**
* Set a new destination * Set a new destination
*/ */
void rpInit(); void rpInit();
void setDestination(Print *dest); void setDestination(Print *dest);
virtual size_t write(uint8_t c); virtual size_t write(uint8_t c);
/** /**
* Debug logging print message * Debug logging print message
* *
* If the provide format string ends with a newline we assume it is the final print of a single * If the provide format string ends with a newline we assume it is the final print of a single
* log message. Otherwise we assume more prints will come before the log message ends. This * log message. Otherwise we assume more prints will come before the log message ends. This
* allows you to call logDebug a few times to build up a single log message line if you wish. * allows you to call logDebug a few times to build up a single log message line if you wish.
*/ */
void log(const char *logLevel, const char *format, ...) __attribute__((format(printf, 3, 4))); void log(const char *logLevel, const char *format, ...) __attribute__((format(printf, 3, 4)));
/** like printf but va_list based */ /** like printf but va_list based */
size_t vprintf(const char *logLevel, const char *format, va_list arg); size_t vprintf(const char *logLevel, const char *format, va_list arg);
void hexDump(const char *logLevel, unsigned char *buf, uint16_t len); void hexDump(const char *logLevel, unsigned char *buf, uint16_t len);
std::string mt_sprintf(const std::string fmt_str, ...); std::string mt_sprintf(const std::string fmt_str, ...);
protected: protected:
/// Subclasses can override if they need to change how we format over the serial port /// Subclasses can override if they need to change how we format over the serial port
virtual void log_to_serial(const char *logLevel, const char *format, va_list arg); virtual void log_to_serial(const char *logLevel, const char *format, va_list arg);
meshtastic_LogRecord_Level getLogLevel(const char *logLevel); meshtastic_LogRecord_Level getLogLevel(const char *logLevel);
private: private:
void log_to_syslog(const char *logLevel, const char *format, va_list arg); void log_to_syslog(const char *logLevel, const char *format, va_list arg);
void log_to_ble(const char *logLevel, const char *format, va_list arg); void log_to_ble(const char *logLevel, const char *format, va_list arg);
}; };
src/SPILock.cpp
+3 -4
View File
@@ -5,8 +5,7 @@
concurrency::Lock *spiLock; concurrency::Lock *spiLock;
void initSPI() void initSPI() {
{ assert(!spiLock);
assert(!spiLock); spiLock = new concurrency::Lock();
spiLock = new concurrency::Lock();
} }
src/SafeFile.cpp
+75 -83
View File
@@ -3,54 +3,48 @@
#ifdef FSCom #ifdef FSCom
// Only way to work on both esp32 and nrf52 // Only way to work on both esp32 and nrf52
static File openFile(const char *filename, bool fullAtomic) static File openFile(const char *filename, bool fullAtomic) {
{ concurrency::LockGuard g(spiLock);
concurrency::LockGuard g(spiLock); LOG_DEBUG("Opening %s, fullAtomic=%d", filename, fullAtomic);
LOG_DEBUG("Opening %s, fullAtomic=%d", filename, fullAtomic);
#ifdef ARCH_NRF52 #ifdef ARCH_NRF52
FSCom.remove(filename); FSCom.remove(filename);
return FSCom.open(filename, FILE_O_WRITE); return FSCom.open(filename, FILE_O_WRITE);
#endif #endif
if (!fullAtomic) { if (!fullAtomic) {
FSCom.remove(filename); // Nuke the old file to make space (ignore if it !exists) FSCom.remove(filename); // Nuke the old file to make space (ignore if it !exists)
} }
String filenameTmp = filename; String filenameTmp = filename;
filenameTmp += ".tmp"; filenameTmp += ".tmp";
// FIXME: If we are doing a full atomic write, we may need to remove the old tmp file now // FIXME: If we are doing a full atomic write, we may need to remove the old tmp file now
// if (fullAtomic) { // if (fullAtomic) {
// FSCom.remove(filename); // FSCom.remove(filename);
// } // }
// clear any previous LFS errors // clear any previous LFS errors
return FSCom.open(filenameTmp.c_str(), FILE_O_WRITE); return FSCom.open(filenameTmp.c_str(), FILE_O_WRITE);
} }
SafeFile::SafeFile(const char *_filename, bool fullAtomic) SafeFile::SafeFile(const char *_filename, bool fullAtomic) : filename(_filename), f(openFile(_filename, fullAtomic)), fullAtomic(fullAtomic) {}
: filename(_filename), f(openFile(_filename, fullAtomic)), fullAtomic(fullAtomic)
{ size_t SafeFile::write(uint8_t ch) {
if (!f)
return 0;
hash ^= ch;
return f.write(ch);
} }
size_t SafeFile::write(uint8_t ch) size_t SafeFile::write(const uint8_t *buffer, size_t size) {
{ if (!f)
if (!f) return 0;
return 0;
hash ^= ch; for (size_t i = 0; i < size; i++) {
return f.write(ch); hash ^= buffer[i];
} }
return f.write((uint8_t const *)buffer, size); // This nasty cast is _IMPORTANT_ otherwise the correct adafruit method
size_t SafeFile::write(const uint8_t *buffer, size_t size) // does not get used (they made a mistake in their typing)
{
if (!f)
return 0;
for (size_t i = 0; i < size; i++) {
hash ^= buffer[i];
}
return f.write((uint8_t const *)buffer, size); // This nasty cast is _IMPORTANT_ otherwise the correct adafruit method does
// not get used (they made a mistake in their typing)
} }
/** /**
@@ -58,66 +52,64 @@ size_t SafeFile::write(const uint8_t *buffer, size_t size)
* *
* @return false for failure * @return false for failure
*/ */
bool SafeFile::close() bool SafeFile::close() {
{ if (!f)
if (!f) return false;
return false;
spiLock->lock(); spiLock->lock();
f.close(); f.close();
spiLock->unlock(); spiLock->unlock();
#ifdef ARCH_NRF52 #ifdef ARCH_NRF52
return true; return true;
#endif #endif
if (!testReadback()) if (!testReadback())
return false; return false;
{ // Scope for lock { // Scope for lock
concurrency::LockGuard g(spiLock); concurrency::LockGuard g(spiLock);
// brief window of risk here ;-) // brief window of risk here ;-)
if (fullAtomic && FSCom.exists(filename.c_str()) && !FSCom.remove(filename.c_str())) { if (fullAtomic && FSCom.exists(filename.c_str()) && !FSCom.remove(filename.c_str())) {
LOG_ERROR("Can't remove old pref file"); LOG_ERROR("Can't remove old pref file");
return false; return false;
}
} }
}
String filenameTmp = filename; String filenameTmp = filename;
filenameTmp += ".tmp"; filenameTmp += ".tmp";
if (!renameFile(filenameTmp.c_str(), filename.c_str())) { if (!renameFile(filenameTmp.c_str(), filename.c_str())) {
LOG_ERROR("Error: can't rename new pref file"); LOG_ERROR("Error: can't rename new pref file");
return false; return false;
} }
return true; return true;
} }
/// Read our (closed) tempfile back in and compare the hash /// Read our (closed) tempfile back in and compare the hash
bool SafeFile::testReadback() bool SafeFile::testReadback() {
{ concurrency::LockGuard g(spiLock);
concurrency::LockGuard g(spiLock);
String filenameTmp = filename; String filenameTmp = filename;
filenameTmp += ".tmp"; filenameTmp += ".tmp";
auto f2 = FSCom.open(filenameTmp.c_str(), FILE_O_READ); auto f2 = FSCom.open(filenameTmp.c_str(), FILE_O_READ);
if (!f2) { if (!f2) {
LOG_ERROR("Can't open tmp file for readback"); LOG_ERROR("Can't open tmp file for readback");
return false; return false;
} }
int c = 0; int c = 0;
uint8_t test_hash = 0; uint8_t test_hash = 0;
while ((c = f2.read()) >= 0) { while ((c = f2.read()) >= 0) {
test_hash ^= (uint8_t)c; test_hash ^= (uint8_t)c;
} }
f2.close(); f2.close();
if (test_hash != hash) { if (test_hash != hash) {
LOG_ERROR("Readback failed hash mismatch"); LOG_ERROR("Readback failed hash mismatch");
return false; return false;
} }
return true; return true;
} }
#endif #endif
src/SafeFile.h
+26 -26
View File
@@ -10,41 +10,41 @@
* This class provides 'safe'/paranoid file writing. * This class provides 'safe'/paranoid file writing.
* *
* Some of our filesystems (in particular the nrf52) may have bugs beneath our layer. Therefore we want to * Some of our filesystems (in particular the nrf52) may have bugs beneath our layer. Therefore we want to
* be very careful about how we write files. This class provides a restricted (Stream only) writing API for writing to files. * be very careful about how we write files. This class provides a restricted (Stream only) writing API for writing to
* files.
* *
* Notably: * Notably:
* - we keep a simple xor hash of all characters that were written. * - we keep a simple xor hash of all characters that were written.
* - We do not allow seeking (because we want to maintain our hash) * - We do not allow seeking (because we want to maintain our hash)
* - we provide an close() method which is similar to close but returns false if we were unable to successfully write the * - we provide an close() method which is similar to close but returns false if we were unable to successfully write
* file. Also this method * the file. Also this method
* - atomically replaces any old version of the file on the disk with our new file (after first rereading the file from the disk * - atomically replaces any old version of the file on the disk with our new file (after first rereading the file from
* to confirm the hash matches) * the disk to confirm the hash matches)
* - Some files are super huge so we can't do the full atomic rename/copy (because of filesystem size limits). If !fullAtomic * - Some files are super huge so we can't do the full atomic rename/copy (because of filesystem size limits). If
* then we still do the readback to verify file is valid so higher level code can handle failures. * !fullAtomic then we still do the readback to verify file is valid so higher level code can handle failures.
*/ */
class SafeFile : public Print class SafeFile : public Print {
{ public:
public: explicit SafeFile(char const *filepath, bool fullAtomic = false);
explicit SafeFile(char const *filepath, bool fullAtomic = false);
virtual size_t write(uint8_t); virtual size_t write(uint8_t);
virtual size_t write(const uint8_t *buffer, size_t size); virtual size_t write(const uint8_t *buffer, size_t size);
/** /**
* Atomically close the file (deleting any old versions) and readback the contents to confirm the hash matches * Atomically close the file (deleting any old versions) and readback the contents to confirm the hash matches
* *
* @return false for failure * @return false for failure
*/ */
bool close(); bool close();
private: private:
/// Read our (closed) tempfile back in and compare the hash /// Read our (closed) tempfile back in and compare the hash
bool testReadback(); bool testReadback();
String filename; String filename;
File f; File f;
bool fullAtomic; bool fullAtomic;
uint8_t hash = 0; uint8_t hash = 0;
}; };
#endif #endif
src/SerialConsole.cpp
+62 -78
View File
@@ -28,121 +28,105 @@
SerialConsole *console; SerialConsole *console;
void consoleInit() void consoleInit() {
{ auto sc = new SerialConsole(); // Must be dynamically allocated because we are now inheriting from thread
auto sc = new SerialConsole(); // Must be dynamically allocated because we are now inheriting from thread
#if defined(SERIAL_HAS_ON_RECEIVE) #if defined(SERIAL_HAS_ON_RECEIVE)
// onReceive does only exist for HardwareSerial not for USB CDC serial // onReceive does only exist for HardwareSerial not for USB CDC serial
Port.onReceive([sc]() { sc->rxInt(); }); Port.onReceive([sc]() { sc->rxInt(); });
#endif #endif
DEBUG_PORT.rpInit(); // Simply sets up semaphore DEBUG_PORT.rpInit(); // Simply sets up semaphore
} }
void consolePrintf(const char *format, ...) void consolePrintf(const char *format, ...) {
{ va_list arg;
va_list arg; va_start(arg, format);
va_start(arg, format); console->vprintf(nullptr, format, arg);
console->vprintf(nullptr, format, arg); va_end(arg);
va_end(arg); console->flush();
console->flush();
} }
SerialConsole::SerialConsole() : StreamAPI(&Port), RedirectablePrint(&Port), concurrency::OSThread("SerialConsole") SerialConsole::SerialConsole() : StreamAPI(&Port), RedirectablePrint(&Port), concurrency::OSThread("SerialConsole") {
{ api_type = TYPE_SERIAL;
api_type = TYPE_SERIAL; assert(!console);
assert(!console); console = this;
console = this; canWrite = false; // We don't send packets to our port until it has talked to us first
canWrite = false; // We don't send packets to our port until it has talked to us first
#ifdef RP2040_SLOW_CLOCK #ifdef RP2040_SLOW_CLOCK
Port.setTX(SERIAL2_TX); Port.setTX(SERIAL2_TX);
Port.setRX(SERIAL2_RX); Port.setRX(SERIAL2_RX);
#endif #endif
Port.begin(SERIAL_BAUD); Port.begin(SERIAL_BAUD);
#if defined(ARCH_NRF52) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) || defined(ARCH_RP2040) || \ #if defined(ARCH_NRF52) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) || defined(ARCH_RP2040) || \
defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32C6) defined(CONFIG_IDF_TARGET_ESP32C3) || defined(CONFIG_IDF_TARGET_ESP32C6)
time_t timeout = millis(); time_t timeout = millis();
while (!Port) { while (!Port) {
if (Throttle::isWithinTimespanMs(timeout, FIVE_SECONDS_MS)) { if (Throttle::isWithinTimespanMs(timeout, FIVE_SECONDS_MS)) {
delay(100); delay(100);
} else { } else {
break; break;
}
} }
}
#endif #endif
#if !ARCH_PORTDUINO #if !ARCH_PORTDUINO
emitRebooted(); emitRebooted();
#endif #endif
} }
int32_t SerialConsole::runOnce() int32_t SerialConsole::runOnce() {
{
#ifdef HELTEC_MESH_SOLAR #ifdef HELTEC_MESH_SOLAR
// After enabling the mesh solar serial port module configuration, command processing is handled by the serial port module. // After enabling the mesh solar serial port module configuration, command processing is handled by the serial port
if (moduleConfig.serial.enabled && moduleConfig.serial.override_console_serial_port && // module.
moduleConfig.serial.mode == meshtastic_ModuleConfig_SerialConfig_Serial_Mode_MS_CONFIG) { if (moduleConfig.serial.enabled && moduleConfig.serial.override_console_serial_port &&
return 250; moduleConfig.serial.mode == meshtastic_ModuleConfig_SerialConfig_Serial_Mode_MS_CONFIG) {
} return 250;
}
#endif #endif
int32_t delay = runOncePart(); int32_t delay = runOncePart();
#if defined(SERIAL_HAS_ON_RECEIVE) || defined(CONFIG_IDF_TARGET_ESP32S2) #if defined(SERIAL_HAS_ON_RECEIVE) || defined(CONFIG_IDF_TARGET_ESP32S2)
return Port.available() ? delay : INT32_MAX; return Port.available() ? delay : INT32_MAX;
#elif defined(IS_USB_SERIAL) #elif defined(IS_USB_SERIAL)
return HWCDC::isPlugged() ? delay : (1000 * 20); return HWCDC::isPlugged() ? delay : (1000 * 20);
#else #else
return delay; return delay;
#endif #endif
} }
void SerialConsole::flush() void SerialConsole::flush() { Port.flush(); }
{
Port.flush();
}
// trigger tx of serial data // trigger tx of serial data
void SerialConsole::onNowHasData(uint32_t fromRadioNum) void SerialConsole::onNowHasData(uint32_t fromRadioNum) { setIntervalFromNow(0); }
{
setIntervalFromNow(0);
}
// trigger rx of serial data // trigger rx of serial data
void SerialConsole::rxInt() void SerialConsole::rxInt() { setIntervalFromNow(0); }
{
setIntervalFromNow(0);
}
// For the serial port we can't really detect if any client is on the other side, so instead just look for recent messages // For the serial port we can't really detect if any client is on the other side, so instead just look for recent
bool SerialConsole::checkIsConnected() // messages
{ bool SerialConsole::checkIsConnected() { return Throttle::isWithinTimespanMs(lastContactMsec, SERIAL_CONNECTION_TIMEOUT); }
return Throttle::isWithinTimespanMs(lastContactMsec, SERIAL_CONNECTION_TIMEOUT);
}
/** /**
* we override this to notice when we've received a protobuf over the serial * we override this to notice when we've received a protobuf over the serial
* stream. Then we shut off debug serial output. * stream. Then we shut off debug serial output.
*/ */
bool SerialConsole::handleToRadio(const uint8_t *buf, size_t len) bool SerialConsole::handleToRadio(const uint8_t *buf, size_t len) {
{ // only talk to the API once the configuration has been loaded and we're sure the serial port is not disabled.
// only talk to the API once the configuration has been loaded and we're sure the serial port is not disabled. if (config.has_lora && config.security.serial_enabled) {
if (config.has_lora && config.security.serial_enabled) { // Switch to protobufs for log messages
// Switch to protobufs for log messages usingProtobufs = true;
usingProtobufs = true; canWrite = true;
canWrite = true;
return StreamAPI::handleToRadio(buf, len); return StreamAPI::handleToRadio(buf, len);
} else { } else {
return false; return false;
} }
} }
void SerialConsole::log_to_serial(const char *logLevel, const char *format, va_list arg) void SerialConsole::log_to_serial(const char *logLevel, const char *format, va_list arg) {
{ if (usingProtobufs && config.security.debug_log_api_enabled) {
if (usingProtobufs && config.security.debug_log_api_enabled) { meshtastic_LogRecord_Level ll = RedirectablePrint::getLogLevel(logLevel);
meshtastic_LogRecord_Level ll = RedirectablePrint::getLogLevel(logLevel); auto thread = concurrency::OSThread::currentThread;
auto thread = concurrency::OSThread::currentThread; emitLogRecord(ll, thread ? thread->ThreadName.c_str() : "", format, arg);
emitLogRecord(ll, thread ? thread->ThreadName.c_str() : "", format, arg); } else
} else RedirectablePrint::log_to_serial(logLevel, format, arg);
RedirectablePrint::log_to_serial(logLevel, format, arg);
} }
src/SerialConsole.h
+26 -28
View File
@@ -6,42 +6,40 @@
* Provides both debug printing and, if the client starts sending protobufs to us, switches to send/receive protobufs * Provides both debug printing and, if the client starts sending protobufs to us, switches to send/receive protobufs
* (and starts dropping debug printing - FIXME, eventually those prints should be encapsulated in protobufs). * (and starts dropping debug printing - FIXME, eventually those prints should be encapsulated in protobufs).
*/ */
class SerialConsole : public StreamAPI, public RedirectablePrint, private concurrency::OSThread class SerialConsole : public StreamAPI, public RedirectablePrint, private concurrency::OSThread {
{ /**
/** * If true we are talking to a smart host and all messages (including log messages) must be framed as protobufs.
* If true we are talking to a smart host and all messages (including log messages) must be framed as protobufs. */
*/ bool usingProtobufs = false;
bool usingProtobufs = false;
public: public:
SerialConsole(); SerialConsole();
/** /**
* we override this to notice when we've received a protobuf over the serial stream. Then we shunt off * we override this to notice when we've received a protobuf over the serial stream. Then we shunt off
* debug serial output. * debug serial output.
*/ */
virtual bool handleToRadio(const uint8_t *buf, size_t len) override; virtual bool handleToRadio(const uint8_t *buf, size_t len) override;
virtual size_t write(uint8_t c) override virtual size_t write(uint8_t c) override {
{ if (c == '\n') // prefix any newlines with carriage return
if (c == '\n') // prefix any newlines with carriage return RedirectablePrint::write('\r');
RedirectablePrint::write('\r'); return RedirectablePrint::write(c);
return RedirectablePrint::write(c); }
}
virtual int32_t runOnce() override; virtual int32_t runOnce() override;
void flush(); void flush();
void rxInt(); void rxInt();
protected: protected:
/// Check the current underlying physical link to see if the client is currently connected /// Check the current underlying physical link to see if the client is currently connected
virtual bool checkIsConnected() override; virtual bool checkIsConnected() override;
virtual void onNowHasData(uint32_t fromRadioNum) override; virtual void onNowHasData(uint32_t fromRadioNum) override;
/// Possibly switch to protobufs if we see a valid protobuf message /// Possibly switch to protobufs if we see a valid protobuf message
virtual void log_to_serial(const char *logLevel, const char *format, va_list arg); virtual void log_to_serial(const char *logLevel, const char *format, va_list arg);
}; };
// A simple wrapper to allow non class aware code write to the console // A simple wrapper to allow non class aware code write to the console
src/Status.h
+28 -32
View File
@@ -9,49 +9,45 @@
#define STATUS_TYPE_NODE 3 #define STATUS_TYPE_NODE 3
#define STATUS_TYPE_BLUETOOTH 4 #define STATUS_TYPE_BLUETOOTH 4
namespace meshtastic namespace meshtastic {
{
// A base class for observable status // A base class for observable status
class Status class Status {
{ protected:
protected: // Allows us to observe an Observable
// Allows us to observe an Observable CallbackObserver<Status, const Status *> statusObserver = CallbackObserver<Status, const Status *>(this, &Status::updateStatus);
CallbackObserver<Status, const Status *> statusObserver = bool initialized = false;
CallbackObserver<Status, const Status *>(this, &Status::updateStatus); // Workaround for no typeid support
bool initialized = false; int statusType = 0;
// Workaround for no typeid support
int statusType = 0;
public: public:
// Allows us to generate observable events // Allows us to generate observable events
Observable<const Status *> onNewStatus; Observable<const Status *> onNewStatus;
// Enable polymorphism ? // Enable polymorphism ?
virtual ~Status() = default; virtual ~Status() = default;
Status() Status() {
{ if (!statusType) {
if (!statusType) { statusType = STATUS_TYPE_BASE;
statusType = STATUS_TYPE_BASE;
}
} }
}
// Prevent object copy/move // Prevent object copy/move
Status(const Status &) = delete; Status(const Status &) = delete;
Status &operator=(const Status &) = delete; Status &operator=(const Status &) = delete;
// Start observing a source of data // Start observing a source of data
void observe(Observable<const Status *> *source) { statusObserver.observe(source); } void observe(Observable<const Status *> *source) { statusObserver.observe(source); }
// Determines whether or not existing data matches the data in another Status instance // Determines whether or not existing data matches the data in another Status instance
bool matches(const Status *otherStatus) const { return true; } bool matches(const Status *otherStatus) const { return true; }
bool isInitialized() const { return initialized; } bool isInitialized() const { return initialized; }
int getStatusType() const { return statusType; } int getStatusType() const { return statusType; }
// Called when the Observable we're observing generates a new notification // Called when the Observable we're observing generates a new notification
int updateStatus(const Status *newStatus) { return 0; } int updateStatus(const Status *newStatus) { return 0; }
}; };
}; // namespace meshtastic }; // namespace meshtastic
src/airtime.cpp
+146 -173
View File
@@ -9,202 +9,175 @@ AirTime *airTime = NULL;
uint32_t air_period_tx[PERIODS_TO_LOG]; uint32_t air_period_tx[PERIODS_TO_LOG];
uint32_t air_period_rx[PERIODS_TO_LOG]; uint32_t air_period_rx[PERIODS_TO_LOG];
void AirTime::logAirtime(reportTypes reportType, uint32_t airtime_ms) void AirTime::logAirtime(reportTypes reportType, uint32_t airtime_ms) {
{
if (reportType == TX_LOG) { if (reportType == TX_LOG) {
LOG_DEBUG("Packet TX: %ums", airtime_ms); LOG_DEBUG("Packet TX: %ums", airtime_ms);
this->airtimes.periodTX[0] = this->airtimes.periodTX[0] + airtime_ms; this->airtimes.periodTX[0] = this->airtimes.periodTX[0] + airtime_ms;
air_period_tx[0] = air_period_tx[0] + airtime_ms; air_period_tx[0] = air_period_tx[0] + airtime_ms;
this->utilizationTX[this->getPeriodUtilHour()] = this->utilizationTX[this->getPeriodUtilHour()] + airtime_ms; this->utilizationTX[this->getPeriodUtilHour()] = this->utilizationTX[this->getPeriodUtilHour()] + airtime_ms;
} else if (reportType == RX_LOG) { } else if (reportType == RX_LOG) {
LOG_DEBUG("Packet RX: %ums", airtime_ms); LOG_DEBUG("Packet RX: %ums", airtime_ms);
this->airtimes.periodRX[0] = this->airtimes.periodRX[0] + airtime_ms; this->airtimes.periodRX[0] = this->airtimes.periodRX[0] + airtime_ms;
air_period_rx[0] = air_period_rx[0] + airtime_ms; air_period_rx[0] = air_period_rx[0] + airtime_ms;
} else if (reportType == RX_ALL_LOG) { } else if (reportType == RX_ALL_LOG) {
LOG_DEBUG("Packet RX (noise?) : %ums", airtime_ms); LOG_DEBUG("Packet RX (noise?) : %ums", airtime_ms);
this->airtimes.periodRX_ALL[0] = this->airtimes.periodRX_ALL[0] + airtime_ms; this->airtimes.periodRX_ALL[0] = this->airtimes.periodRX_ALL[0] + airtime_ms;
}
// Log all airtime type for channel utilization
this->channelUtilization[this->getPeriodUtilMinute()] = channelUtilization[this->getPeriodUtilMinute()] + airtime_ms;
}
uint8_t AirTime::currentPeriodIndex() { return ((getSecondsSinceBoot() / SECONDS_PER_PERIOD) % PERIODS_TO_LOG); }
uint8_t AirTime::getPeriodUtilMinute() { return (getSecondsSinceBoot() / 10) % CHANNEL_UTILIZATION_PERIODS; }
uint8_t AirTime::getPeriodUtilHour() { return (getSecondsSinceBoot() / 60) % MINUTES_IN_HOUR; }
void AirTime::airtimeRotatePeriod() {
if (this->airtimes.lastPeriodIndex != this->currentPeriodIndex()) {
LOG_DEBUG("Rotate airtimes to a new period = %u", this->currentPeriodIndex());
for (int i = PERIODS_TO_LOG - 2; i >= 0; --i) {
this->airtimes.periodTX[i + 1] = this->airtimes.periodTX[i];
this->airtimes.periodRX[i + 1] = this->airtimes.periodRX[i];
this->airtimes.periodRX_ALL[i + 1] = this->airtimes.periodRX_ALL[i];
air_period_tx[i + 1] = this->airtimes.periodTX[i];
air_period_rx[i + 1] = this->airtimes.periodRX[i];
} }
// Log all airtime type for channel utilization this->airtimes.periodTX[0] = 0;
this->channelUtilization[this->getPeriodUtilMinute()] = channelUtilization[this->getPeriodUtilMinute()] + airtime_ms; this->airtimes.periodRX[0] = 0;
this->airtimes.periodRX_ALL[0] = 0;
air_period_tx[0] = 0;
air_period_rx[0] = 0;
this->airtimes.lastPeriodIndex = this->currentPeriodIndex();
}
} }
uint8_t AirTime::currentPeriodIndex() uint32_t *AirTime::airtimeReport(reportTypes reportType) {
{
return ((getSecondsSinceBoot() / SECONDS_PER_PERIOD) % PERIODS_TO_LOG); if (reportType == TX_LOG) {
return this->airtimes.periodTX;
} else if (reportType == RX_LOG) {
return this->airtimes.periodRX;
} else if (reportType == RX_ALL_LOG) {
return this->airtimes.periodRX_ALL;
}
return 0;
} }
uint8_t AirTime::getPeriodUtilMinute() uint8_t AirTime::getPeriodsToLog() { return PERIODS_TO_LOG; }
{
return (getSecondsSinceBoot() / 10) % CHANNEL_UTILIZATION_PERIODS; uint32_t AirTime::getSecondsPerPeriod() { return SECONDS_PER_PERIOD; }
uint32_t AirTime::getSecondsSinceBoot() { return this->secSinceBoot; }
float AirTime::channelUtilizationPercent() {
uint32_t sum = 0;
for (uint32_t i = 0; i < CHANNEL_UTILIZATION_PERIODS; i++) {
sum += this->channelUtilization[i];
}
return (float(sum) / float(CHANNEL_UTILIZATION_PERIODS * 10 * 1000)) * 100;
} }
uint8_t AirTime::getPeriodUtilHour() float AirTime::utilizationTXPercent() {
{ uint32_t sum = 0;
return (getSecondsSinceBoot() / 60) % MINUTES_IN_HOUR; for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) {
sum += this->utilizationTX[i];
}
return (float(sum) / float(MS_IN_HOUR)) * 100;
} }
void AirTime::airtimeRotatePeriod() bool AirTime::isTxAllowedChannelUtil(bool polite) {
{ uint8_t percentage = (polite ? polite_channel_util_percent : max_channel_util_percent);
if (channelUtilizationPercent() < percentage) {
if (this->airtimes.lastPeriodIndex != this->currentPeriodIndex()) {
LOG_DEBUG("Rotate airtimes to a new period = %u", this->currentPeriodIndex());
for (int i = PERIODS_TO_LOG - 2; i >= 0; --i) {
this->airtimes.periodTX[i + 1] = this->airtimes.periodTX[i];
this->airtimes.periodRX[i + 1] = this->airtimes.periodRX[i];
this->airtimes.periodRX_ALL[i + 1] = this->airtimes.periodRX_ALL[i];
air_period_tx[i + 1] = this->airtimes.periodTX[i];
air_period_rx[i + 1] = this->airtimes.periodRX[i];
}
this->airtimes.periodTX[0] = 0;
this->airtimes.periodRX[0] = 0;
this->airtimes.periodRX_ALL[0] = 0;
air_period_tx[0] = 0;
air_period_rx[0] = 0;
this->airtimes.lastPeriodIndex = this->currentPeriodIndex();
}
}
uint32_t *AirTime::airtimeReport(reportTypes reportType)
{
if (reportType == TX_LOG) {
return this->airtimes.periodTX;
} else if (reportType == RX_LOG) {
return this->airtimes.periodRX;
} else if (reportType == RX_ALL_LOG) {
return this->airtimes.periodRX_ALL;
}
return 0;
}
uint8_t AirTime::getPeriodsToLog()
{
return PERIODS_TO_LOG;
}
uint32_t AirTime::getSecondsPerPeriod()
{
return SECONDS_PER_PERIOD;
}
uint32_t AirTime::getSecondsSinceBoot()
{
return this->secSinceBoot;
}
float AirTime::channelUtilizationPercent()
{
uint32_t sum = 0;
for (uint32_t i = 0; i < CHANNEL_UTILIZATION_PERIODS; i++) {
sum += this->channelUtilization[i];
}
return (float(sum) / float(CHANNEL_UTILIZATION_PERIODS * 10 * 1000)) * 100;
}
float AirTime::utilizationTXPercent()
{
uint32_t sum = 0;
for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) {
sum += this->utilizationTX[i];
}
return (float(sum) / float(MS_IN_HOUR)) * 100;
}
bool AirTime::isTxAllowedChannelUtil(bool polite)
{
uint8_t percentage = (polite ? polite_channel_util_percent : max_channel_util_percent);
if (channelUtilizationPercent() < percentage) {
return true;
} else {
LOG_WARN("Ch. util >%d%%. Skip send", percentage);
return false;
}
}
bool AirTime::isTxAllowedAirUtil()
{
if (!config.lora.override_duty_cycle && myRegion->dutyCycle < 100) {
if (utilizationTXPercent() < myRegion->dutyCycle * polite_duty_cycle_percent / 100) {
return true;
} else {
LOG_WARN("TX air util. >%f%%. Skip send", myRegion->dutyCycle * polite_duty_cycle_percent / 100);
return false;
}
}
return true; return true;
} else {
LOG_WARN("Ch. util >%d%%. Skip send", percentage);
return false;
}
}
bool AirTime::isTxAllowedAirUtil() {
if (!config.lora.override_duty_cycle && myRegion->dutyCycle < 100) {
if (utilizationTXPercent() < myRegion->dutyCycle * polite_duty_cycle_percent / 100) {
return true;
} else {
LOG_WARN("TX air util. >%f%%. Skip send", myRegion->dutyCycle * polite_duty_cycle_percent / 100);
return false;
}
}
return true;
} }
// Get the amount of minutes we have to be silent before we can send again // Get the amount of minutes we have to be silent before we can send again
uint8_t AirTime::getSilentMinutes(float txPercent, float dutyCycle) uint8_t AirTime::getSilentMinutes(float txPercent, float dutyCycle) {
{ float newTxPercent = txPercent;
float newTxPercent = txPercent; for (int8_t i = MINUTES_IN_HOUR - 1; i >= 0; --i) {
for (int8_t i = MINUTES_IN_HOUR - 1; i >= 0; --i) { newTxPercent -= ((float)this->utilizationTX[i] / (MS_IN_MINUTE * MINUTES_IN_HOUR / 100));
newTxPercent -= ((float)this->utilizationTX[i] / (MS_IN_MINUTE * MINUTES_IN_HOUR / 100)); if (newTxPercent < dutyCycle)
if (newTxPercent < dutyCycle) return MINUTES_IN_HOUR - 1 - i;
return MINUTES_IN_HOUR - 1 - i; }
}
return MINUTES_IN_HOUR; return MINUTES_IN_HOUR;
} }
AirTime::AirTime() : concurrency::OSThread("AirTime"), airtimes({}) {} AirTime::AirTime() : concurrency::OSThread("AirTime"), airtimes({}) {}
int32_t AirTime::runOnce() int32_t AirTime::runOnce() {
{ secSinceBoot++;
secSinceBoot++;
uint8_t utilPeriod = this->getPeriodUtilMinute(); uint8_t utilPeriod = this->getPeriodUtilMinute();
uint8_t utilPeriodTX = this->getPeriodUtilHour(); uint8_t utilPeriodTX = this->getPeriodUtilHour();
if (firstTime) { if (firstTime) {
// Init utilizationTX window to all 0 // Init utilizationTX window to all 0
for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) { for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) {
this->utilizationTX[i] = 0; this->utilizationTX[i] = 0;
}
// Init channelUtilization window to all 0
for (uint32_t i = 0; i < CHANNEL_UTILIZATION_PERIODS; i++) {
this->channelUtilization[i] = 0;
}
// Init airtime windows to all 0
for (int i = 0; i < PERIODS_TO_LOG; i++) {
this->airtimes.periodTX[i] = 0;
this->airtimes.periodRX[i] = 0;
this->airtimes.periodRX_ALL[i] = 0;
// air_period_tx[i] = 0;
// air_period_rx[i] = 0;
}
firstTime = false;
lastUtilPeriod = utilPeriod;
} else {
this->airtimeRotatePeriod();
// Reset the channelUtilization window when we roll over
if (lastUtilPeriod != utilPeriod) {
lastUtilPeriod = utilPeriod;
this->channelUtilization[utilPeriod] = 0;
}
if (lastUtilPeriodTX != utilPeriodTX) {
lastUtilPeriodTX = utilPeriodTX;
this->utilizationTX[utilPeriodTX] = 0;
}
} }
return (1000 * 1);
// Init channelUtilization window to all 0
for (uint32_t i = 0; i < CHANNEL_UTILIZATION_PERIODS; i++) {
this->channelUtilization[i] = 0;
}
// Init airtime windows to all 0
for (int i = 0; i < PERIODS_TO_LOG; i++) {
this->airtimes.periodTX[i] = 0;
this->airtimes.periodRX[i] = 0;
this->airtimes.periodRX_ALL[i] = 0;
// air_period_tx[i] = 0;
// air_period_rx[i] = 0;
}
firstTime = false;
lastUtilPeriod = utilPeriod;
} else {
this->airtimeRotatePeriod();
// Reset the channelUtilization window when we roll over
if (lastUtilPeriod != utilPeriod) {
lastUtilPeriod = utilPeriod;
this->channelUtilization[utilPeriod] = 0;
}
if (lastUtilPeriodTX != utilPeriodTX) {
lastUtilPeriodTX = utilPeriodTX;
this->utilizationTX[utilPeriodTX] = 0;
}
}
return (1000 * 1);
} }
src/airtime.h
+36 -37
View File
@@ -39,51 +39,50 @@ void logAirtime(reportTypes reportType, uint32_t airtime_ms);
uint32_t *airtimeReport(reportTypes reportType); uint32_t *airtimeReport(reportTypes reportType);
class AirTime : private concurrency::OSThread class AirTime : private concurrency::OSThread {
{
public: public:
AirTime(); AirTime();
void logAirtime(reportTypes reportType, uint32_t airtime_ms); void logAirtime(reportTypes reportType, uint32_t airtime_ms);
float channelUtilizationPercent(); float channelUtilizationPercent();
float utilizationTXPercent(); float utilizationTXPercent();
float UtilizationPercentTX(); float UtilizationPercentTX();
uint32_t channelUtilization[CHANNEL_UTILIZATION_PERIODS] = {0}; uint32_t channelUtilization[CHANNEL_UTILIZATION_PERIODS] = {0};
uint32_t utilizationTX[MINUTES_IN_HOUR] = {0}; uint32_t utilizationTX[MINUTES_IN_HOUR] = {0};
void airtimeRotatePeriod(); void airtimeRotatePeriod();
uint8_t getPeriodsToLog(); uint8_t getPeriodsToLog();
uint32_t getSecondsPerPeriod(); uint32_t getSecondsPerPeriod();
uint32_t getSecondsSinceBoot(); uint32_t getSecondsSinceBoot();
uint32_t *airtimeReport(reportTypes reportType); uint32_t *airtimeReport(reportTypes reportType);
uint8_t getSilentMinutes(float txPercent, float dutyCycle); uint8_t getSilentMinutes(float txPercent, float dutyCycle);
bool isTxAllowedChannelUtil(bool polite = false); bool isTxAllowedChannelUtil(bool polite = false);
bool isTxAllowedAirUtil(); bool isTxAllowedAirUtil();
private: private:
bool firstTime = true; bool firstTime = true;
uint8_t lastUtilPeriod = 0; uint8_t lastUtilPeriod = 0;
uint8_t lastUtilPeriodTX = 0; uint8_t lastUtilPeriodTX = 0;
uint32_t secSinceBoot = 0; uint32_t secSinceBoot = 0;
uint8_t max_channel_util_percent = 40; uint8_t max_channel_util_percent = 40;
uint8_t polite_channel_util_percent = 25; uint8_t polite_channel_util_percent = 25;
uint8_t polite_duty_cycle_percent = 50; // half of Duty Cycle allowance is ok for metadata uint8_t polite_duty_cycle_percent = 50; // half of Duty Cycle allowance is ok for metadata
struct airtimeStruct { struct airtimeStruct {
uint32_t periodTX[PERIODS_TO_LOG]; // AirTime transmitted uint32_t periodTX[PERIODS_TO_LOG]; // AirTime transmitted
uint32_t periodRX[PERIODS_TO_LOG]; // AirTime received and repeated (Only valid mesh packets) uint32_t periodRX[PERIODS_TO_LOG]; // AirTime received and repeated (Only valid mesh packets)
uint32_t periodRX_ALL[PERIODS_TO_LOG]; // AirTime received regardless of valid mesh packet. Could include noise. uint32_t periodRX_ALL[PERIODS_TO_LOG]; // AirTime received regardless of valid mesh packet. Could include noise.
uint8_t lastPeriodIndex; uint8_t lastPeriodIndex;
} airtimes; } airtimes;
uint8_t getPeriodUtilMinute(); uint8_t getPeriodUtilMinute();
uint8_t getPeriodUtilHour(); uint8_t getPeriodUtilHour();
uint8_t currentPeriodIndex(); uint8_t currentPeriodIndex();
protected: protected:
virtual int32_t runOnce() override; virtual int32_t runOnce() override;
}; };
extern AirTime *airTime; extern AirTime *airTime;
src/buzz/BuzzerFeedbackThread.cpp
+50 -52
View File
@@ -5,60 +5,58 @@
BuzzerFeedbackThread *buzzerFeedbackThread; BuzzerFeedbackThread *buzzerFeedbackThread;
BuzzerFeedbackThread::BuzzerFeedbackThread() BuzzerFeedbackThread::BuzzerFeedbackThread() {
{ if (inputBroker)
if (inputBroker) inputObserver.observe(inputBroker);
inputObserver.observe(inputBroker);
} }
int BuzzerFeedbackThread::handleInputEvent(const InputEvent *event) int BuzzerFeedbackThread::handleInputEvent(const InputEvent *event) {
{ // Only provide feedback if buzzer is enabled for notifications
// Only provide feedback if buzzer is enabled for notifications if (config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DISABLED ||
if (config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DISABLED || config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_NOTIFICATIONS_ONLY ||
config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_NOTIFICATIONS_ONLY || config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DIRECT_MSG_ONLY) {
config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DIRECT_MSG_ONLY) { return 0; // Let other handlers process the event
return 0; // Let other handlers process the event }
// Handle different input events with appropriate buzzer feedback
switch (event->inputEvent) {
case INPUT_BROKER_USER_PRESS:
case INPUT_BROKER_ALT_PRESS:
playClick(); // Low delay feedback
break;
case INPUT_BROKER_SELECT:
case INPUT_BROKER_SELECT_LONG:
playBeep(); // Confirmation feedback
break;
case INPUT_BROKER_UP:
case INPUT_BROKER_UP_LONG:
case INPUT_BROKER_DOWN:
case INPUT_BROKER_DOWN_LONG:
case INPUT_BROKER_LEFT:
case INPUT_BROKER_RIGHT:
playChirp(); // Navigation feedback
break;
case INPUT_BROKER_CANCEL:
case INPUT_BROKER_BACK:
playBoop(); // Cancel/back feedback
break;
case INPUT_BROKER_SEND_PING:
playComboTune(); // Ping sent feedback
break;
default:
// For other events, check if it's a printable character
if (event->kbchar >= 32 && event->kbchar <= 126) {
// Typing feedback - very short boop
// Removing this for now, too chatty
// playChirp();
} }
break;
}
// Handle different input events with appropriate buzzer feedback return 0; // Allow other handlers to process the event
switch (event->inputEvent) {
case INPUT_BROKER_USER_PRESS:
case INPUT_BROKER_ALT_PRESS:
playClick(); // Low delay feedback
break;
case INPUT_BROKER_SELECT:
case INPUT_BROKER_SELECT_LONG:
playBeep(); // Confirmation feedback
break;
case INPUT_BROKER_UP:
case INPUT_BROKER_UP_LONG:
case INPUT_BROKER_DOWN:
case INPUT_BROKER_DOWN_LONG:
case INPUT_BROKER_LEFT:
case INPUT_BROKER_RIGHT:
playChirp(); // Navigation feedback
break;
case INPUT_BROKER_CANCEL:
case INPUT_BROKER_BACK:
playBoop(); // Cancel/back feedback
break;
case INPUT_BROKER_SEND_PING:
playComboTune(); // Ping sent feedback
break;
default:
// For other events, check if it's a printable character
if (event->kbchar >= 32 && event->kbchar <= 126) {
// Typing feedback - very short boop
// Removing this for now, too chatty
// playChirp();
}
break;
}
return 0; // Allow other handlers to process the event
} }
src/buzz/BuzzerFeedbackThread.h
+6 -7
View File
@@ -4,14 +4,13 @@
#include "concurrency/OSThread.h" #include "concurrency/OSThread.h"
#include "input/InputBroker.h" #include "input/InputBroker.h"
class BuzzerFeedbackThread class BuzzerFeedbackThread {
{ CallbackObserver<BuzzerFeedbackThread, const InputEvent *> inputObserver =
CallbackObserver<BuzzerFeedbackThread, const InputEvent *> inputObserver = CallbackObserver<BuzzerFeedbackThread, const InputEvent *>(this, &BuzzerFeedbackThread::handleInputEvent);
CallbackObserver<BuzzerFeedbackThread, const InputEvent *>(this, &BuzzerFeedbackThread::handleInputEvent);
public: public:
BuzzerFeedbackThread(); BuzzerFeedbackThread();
int handleInputEvent(const InputEvent *event); int handleInputEvent(const InputEvent *event);
}; };
extern BuzzerFeedbackThread *buzzerFeedbackThread; extern BuzzerFeedbackThread *buzzerFeedbackThread;
src/buzz/buzz.cpp
+83 -101
View File
@@ -11,8 +11,8 @@ extern "C" void delay(uint32_t dwMs);
#endif #endif
struct ToneDuration { struct ToneDuration {
int frequency_khz; int frequency_khz;
int duration_ms; int duration_ms;
}; };
// Some common frequencies. // Some common frequencies.
@@ -42,105 +42,92 @@ const int DURATION_1_2 = 500; // 1/2 note
const int DURATION_3_4 = 750; // 3/4 note const int DURATION_3_4 = 750; // 3/4 note
const int DURATION_1_1 = 1000; // 1/1 note const int DURATION_1_1 = 1000; // 1/1 note
void playTones(const ToneDuration *tone_durations, int size) void playTones(const ToneDuration *tone_durations, int size) {
{ if (config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DISABLED ||
if (config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DISABLED || config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_NOTIFICATIONS_ONLY) {
config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_NOTIFICATIONS_ONLY) { // Buzzer is disabled or not set to system tones
// Buzzer is disabled or not set to system tones return;
return; }
}
#ifdef PIN_BUZZER #ifdef PIN_BUZZER
if (!config.device.buzzer_gpio) if (!config.device.buzzer_gpio)
config.device.buzzer_gpio = PIN_BUZZER; config.device.buzzer_gpio = PIN_BUZZER;
#endif #endif
if (config.device.buzzer_gpio) { if (config.device.buzzer_gpio) {
for (int i = 0; i < size; i++) { for (int i = 0; i < size; i++) {
const auto &tone_duration = tone_durations[i]; const auto &tone_duration = tone_durations[i];
tone(config.device.buzzer_gpio, tone_duration.frequency_khz, tone_duration.duration_ms); tone(config.device.buzzer_gpio, tone_duration.frequency_khz, tone_duration.duration_ms);
// to distinguish the notes, set a minimum time between them. // to distinguish the notes, set a minimum time between them.
delay(1.3 * tone_duration.duration_ms); delay(1.3 * tone_duration.duration_ms);
}
} }
}
} }
void playBeep() void playBeep() {
{ ToneDuration melody[] = {{NOTE_B3, DURATION_1_8}};
ToneDuration melody[] = {{NOTE_B3, DURATION_1_8}}; playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playLongBeep() void playLongBeep() {
{ ToneDuration melody[] = {{NOTE_B3, DURATION_1_1}};
ToneDuration melody[] = {{NOTE_B3, DURATION_1_1}}; playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playGPSEnableBeep() void playGPSEnableBeep() {
{
#if defined(R1_NEO) || defined(MUZI_BASE) #if defined(R1_NEO) || defined(MUZI_BASE)
ToneDuration melody[] = { ToneDuration melody[] = {{NOTE_F5, DURATION_1_2}, {NOTE_G6, DURATION_1_8}, {NOTE_E7, DURATION_1_4}, {NOTE_SILENT, DURATION_1_2}};
{NOTE_F5, DURATION_1_2}, {NOTE_G6, DURATION_1_8}, {NOTE_E7, DURATION_1_4}, {NOTE_SILENT, DURATION_1_2}};
#else #else
ToneDuration melody[] = {{NOTE_C3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_CS4, DURATION_1_4}}; ToneDuration melody[] = {{NOTE_C3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_CS4, DURATION_1_4}};
#endif #endif
playTones(melody, sizeof(melody) / sizeof(ToneDuration)); playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playGPSDisableBeep() void playGPSDisableBeep() {
{
#if defined(R1_NEO) || defined(MUZI_BASE) #if defined(R1_NEO) || defined(MUZI_BASE)
ToneDuration melody[] = {{NOTE_B4, DURATION_1_16}, {NOTE_B4, DURATION_1_16}, {NOTE_SILENT, DURATION_1_8}, ToneDuration melody[] = {{NOTE_B4, DURATION_1_16}, {NOTE_B4, DURATION_1_16}, {NOTE_SILENT, DURATION_1_8}, {NOTE_F3, DURATION_1_16},
{NOTE_F3, DURATION_1_16}, {NOTE_F3, DURATION_1_16}, {NOTE_SILENT, DURATION_1_8}, {NOTE_F3, DURATION_1_16}, {NOTE_SILENT, DURATION_1_8}, {NOTE_C3, DURATION_1_1}, {NOTE_SILENT, DURATION_1_1}};
{NOTE_C3, DURATION_1_1}, {NOTE_SILENT, DURATION_1_1}};
#else #else
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_C3, DURATION_1_4}}; ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_C3, DURATION_1_4}};
#endif #endif
playTones(melody, sizeof(melody) / sizeof(ToneDuration)); playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playStartMelody() void playStartMelody() {
{ ToneDuration melody[] = {{NOTE_FS3, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_CS4, DURATION_1_4}};
ToneDuration melody[] = {{NOTE_FS3, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_CS4, DURATION_1_4}}; playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playShutdownMelody() void playShutdownMelody() {
{ ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}};
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}}; playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playChirp() void playChirp() {
{ // A short, friendly "chirp" sound for key presses
// A short, friendly "chirp" sound for key presses ToneDuration melody[] = {{NOTE_AS3, 20}}; // Short AS3 note
ToneDuration melody[] = {{NOTE_AS3, 20}}; // Short AS3 note playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playClick() void playClick() {
{ // A very short "click" sound with minimum delay; ideal for rotary encoder events
// A very short "click" sound with minimum delay; ideal for rotary encoder events ToneDuration melody[] = {{NOTE_AS3, 1}}; // Very Short AS3
ToneDuration melody[] = {{NOTE_AS3, 1}}; // Very Short AS3 playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playBoop() void playBoop() {
{ // A short, friendly "boop" sound for button presses
// A short, friendly "boop" sound for button presses ToneDuration melody[] = {{NOTE_A3, 50}}; // Very short A3 note
ToneDuration melody[] = {{NOTE_A3, 50}}; // Very short A3 note playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
void playLongPressLeadUp() void playLongPressLeadUp() {
{ // An ascending lead-up sequence for long press - builds anticipation
// An ascending lead-up sequence for long press - builds anticipation ToneDuration melody[] = {
ToneDuration melody[] = { {NOTE_C3, 100}, // Start low
{NOTE_C3, 100}, // Start low {NOTE_E3, 100}, // Step up
{NOTE_E3, 100}, // Step up {NOTE_G3, 100}, // Keep climbing
{NOTE_G3, 100}, // Keep climbing {NOTE_B3, 150} // Peak with longer note for emphasis
{NOTE_B3, 150} // Peak with longer note for emphasis };
}; playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
// Static state for progressive lead-up notes // Static state for progressive lead-up notes
@@ -153,39 +140,34 @@ static const ToneDuration leadUpNotes[] = {
}; };
static const int leadUpNotesCount = sizeof(leadUpNotes) / sizeof(ToneDuration); static const int leadUpNotesCount = sizeof(leadUpNotes) / sizeof(ToneDuration);
bool playNextLeadUpNote() bool playNextLeadUpNote() {
{ if (leadUpNoteIndex >= leadUpNotesCount) {
if (leadUpNoteIndex >= leadUpNotesCount) { return false; // All notes have been played
return false; // All notes have been played }
}
// Use playTones to handle buzzer logic consistently // Use playTones to handle buzzer logic consistently
const auto &note = leadUpNotes[leadUpNoteIndex]; const auto &note = leadUpNotes[leadUpNoteIndex];
playTones(&note, 1); // Play single note using existing playTones function playTones(&note, 1); // Play single note using existing playTones function
leadUpNoteIndex++; leadUpNoteIndex++;
if (leadUpNoteIndex >= leadUpNotesCount) { if (leadUpNoteIndex >= leadUpNotesCount) {
return false; // this was the final note return false; // this was the final note
} }
return true; // Note was played (playTones handles buzzer availability internally) return true; // Note was played (playTones handles buzzer availability internally)
} }
void resetLeadUpSequence() void resetLeadUpSequence() { leadUpNoteIndex = 0; }
{
leadUpNoteIndex = 0;
}
void playComboTune() void playComboTune() {
{ // Quick high-pitched notes with trills
// Quick high-pitched notes with trills ToneDuration melody[] = {
ToneDuration melody[] = { {NOTE_G3, 80}, // Quick chirp
{NOTE_G3, 80}, // Quick chirp {NOTE_B3, 60}, // Higher chirp
{NOTE_B3, 60}, // Higher chirp {NOTE_CS4, 80}, // Even higher
{NOTE_CS4, 80}, // Even higher {NOTE_G3, 60}, // Quick trill down
{NOTE_G3, 60}, // Quick trill down {NOTE_CS4, 60}, // Quick trill up
{NOTE_CS4, 60}, // Quick trill up {NOTE_B3, 120} // Ending chirp
{NOTE_B3, 120} // Ending chirp };
}; playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
} }
src/commands.h
+11 -11
View File
@@ -4,15 +4,15 @@
*/ */
enum class Cmd { enum class Cmd {
INVALID, INVALID,
SET_ON, SET_ON,
SET_OFF, SET_OFF,
ON_PRESS, ON_PRESS,
START_ALERT_FRAME, START_ALERT_FRAME,
STOP_ALERT_FRAME, STOP_ALERT_FRAME,
START_FIRMWARE_UPDATE_SCREEN, START_FIRMWARE_UPDATE_SCREEN,
STOP_BOOT_SCREEN, STOP_BOOT_SCREEN,
SHOW_PREV_FRAME, SHOW_PREV_FRAME,
SHOW_NEXT_FRAME, SHOW_NEXT_FRAME,
NOOP NOOP
}; };
src/concurrency/BinarySemaphoreFreeRTOS.cpp
+7 -21
View File
@@ -4,35 +4,21 @@
#ifdef HAS_FREE_RTOS #ifdef HAS_FREE_RTOS
namespace concurrency namespace concurrency {
{
BinarySemaphoreFreeRTOS::BinarySemaphoreFreeRTOS() : semaphore(xSemaphoreCreateBinary()) BinarySemaphoreFreeRTOS::BinarySemaphoreFreeRTOS() : semaphore(xSemaphoreCreateBinary()) { assert(semaphore); }
{
assert(semaphore);
}
BinarySemaphoreFreeRTOS::~BinarySemaphoreFreeRTOS() BinarySemaphoreFreeRTOS::~BinarySemaphoreFreeRTOS() { vSemaphoreDelete(semaphore); }
{
vSemaphoreDelete(semaphore);
}
/** /**
* Returns false if we were interrupted * Returns false if we were interrupted
*/ */
bool BinarySemaphoreFreeRTOS::take(uint32_t msec) bool BinarySemaphoreFreeRTOS::take(uint32_t msec) { return xSemaphoreTake(semaphore, pdMS_TO_TICKS(msec)); }
{
return xSemaphoreTake(semaphore, pdMS_TO_TICKS(msec));
}
void BinarySemaphoreFreeRTOS::give() void BinarySemaphoreFreeRTOS::give() { xSemaphoreGive(semaphore); }
{
xSemaphoreGive(semaphore);
}
IRAM_ATTR void BinarySemaphoreFreeRTOS::giveFromISR(BaseType_t *pxHigherPriorityTaskWoken) IRAM_ATTR void BinarySemaphoreFreeRTOS::giveFromISR(BaseType_t *pxHigherPriorityTaskWoken) {
{ xSemaphoreGiveFromISR(semaphore, pxHigherPriorityTaskWoken);
xSemaphoreGiveFromISR(semaphore, pxHigherPriorityTaskWoken);
} }
} // namespace concurrency } // namespace concurrency
src/concurrency/BinarySemaphoreFreeRTOS.h
+12 -14
View File
@@ -2,27 +2,25 @@
#include "../freertosinc.h" #include "../freertosinc.h"
namespace concurrency namespace concurrency {
{
#ifdef HAS_FREE_RTOS #ifdef HAS_FREE_RTOS
class BinarySemaphoreFreeRTOS class BinarySemaphoreFreeRTOS {
{ SemaphoreHandle_t semaphore;
SemaphoreHandle_t semaphore;
public: public:
BinarySemaphoreFreeRTOS(); BinarySemaphoreFreeRTOS();
~BinarySemaphoreFreeRTOS(); ~BinarySemaphoreFreeRTOS();
/** /**
* Returns false if we timed out * Returns false if we timed out
*/ */
bool take(uint32_t msec); bool take(uint32_t msec);
void give(); void give();
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken); void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
}; };
#endif #endif
src/concurrency/BinarySemaphorePosix.cpp
+4 -6
View File
@@ -3,8 +3,7 @@
#ifndef HAS_FREE_RTOS #ifndef HAS_FREE_RTOS
namespace concurrency namespace concurrency {
{
BinarySemaphorePosix::BinarySemaphorePosix() {} BinarySemaphorePosix::BinarySemaphorePosix() {}
@@ -13,10 +12,9 @@ BinarySemaphorePosix::~BinarySemaphorePosix() {}
/** /**
* Returns false if we timed out * Returns false if we timed out
*/ */
bool BinarySemaphorePosix::take(uint32_t msec) bool BinarySemaphorePosix::take(uint32_t msec) {
{ delay(msec); // FIXME
delay(msec); // FIXME return false;
return false;
} }
void BinarySemaphorePosix::give() {} void BinarySemaphorePosix::give() {}
src/concurrency/BinarySemaphorePosix.h
+12 -14
View File
@@ -2,27 +2,25 @@
#include "../freertosinc.h" #include "../freertosinc.h"
namespace concurrency namespace concurrency {
{
#ifndef HAS_FREE_RTOS #ifndef HAS_FREE_RTOS
class BinarySemaphorePosix class BinarySemaphorePosix {
{ // SemaphoreHandle_t semaphore;
// SemaphoreHandle_t semaphore;
public: public:
BinarySemaphorePosix(); BinarySemaphorePosix();
~BinarySemaphorePosix(); ~BinarySemaphorePosix();
/** /**
* Returns false if we timed out * Returns false if we timed out
*/ */
bool take(uint32_t msec); bool take(uint32_t msec);
void give(); void give();
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken); void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
}; };
#endif #endif
src/concurrency/InterruptableDelay.cpp
+9 -17
View File
@@ -1,8 +1,7 @@
#include "concurrency/InterruptableDelay.h" #include "concurrency/InterruptableDelay.h"
#include "configuration.h" #include "configuration.h"
namespace concurrency namespace concurrency {
{
InterruptableDelay::InterruptableDelay() {} InterruptableDelay::InterruptableDelay() {}
@@ -11,25 +10,18 @@ InterruptableDelay::~InterruptableDelay() {}
/** /**
* Returns false if we were interrupted * Returns false if we were interrupted
*/ */
bool InterruptableDelay::delay(uint32_t msec) bool InterruptableDelay::delay(uint32_t msec) {
{ // LOG_DEBUG("delay %u ", msec);
// LOG_DEBUG("delay %u ", msec);
// sem take will return false if we timed out (i.e. were not interrupted) // sem take will return false if we timed out (i.e. were not interrupted)
bool r = semaphore.take(msec); bool r = semaphore.take(msec);
// LOG_DEBUG("interrupt=%d", r); // LOG_DEBUG("interrupt=%d", r);
return !r; return !r;
} }
void InterruptableDelay::interrupt() void InterruptableDelay::interrupt() { semaphore.give(); }
{
semaphore.give();
}
IRAM_ATTR void InterruptableDelay::interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken) IRAM_ATTR void InterruptableDelay::interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken) { semaphore.giveFromISR(pxHigherPriorityTaskWoken); }
{
semaphore.giveFromISR(pxHigherPriorityTaskWoken);
}
} // namespace concurrency } // namespace concurrency
src/concurrency/InterruptableDelay.h
+14 -15
View File
@@ -10,32 +10,31 @@
#define BinarySemaphore BinarySemaphorePosix #define BinarySemaphore BinarySemaphorePosix
#endif #endif
namespace concurrency namespace concurrency {
{
/** /**
* An object that provides delay(msec) like functionality, but can be interrupted by calling interrupt(). * An object that provides delay(msec) like functionality, but can be interrupted by calling interrupt().
* *
* Useful for they top level loop() delay call to keep the CPU powered down until our next scheduled event or some external event. * Useful for they top level loop() delay call to keep the CPU powered down until our next scheduled event or some
* external event.
* *
* This is implemented for FreeRTOS but should be easy to port to other operating systems. * This is implemented for FreeRTOS but should be easy to port to other operating systems.
*/ */
class InterruptableDelay class InterruptableDelay {
{ BinarySemaphore semaphore;
BinarySemaphore semaphore;
public: public:
InterruptableDelay(); InterruptableDelay();
~InterruptableDelay(); ~InterruptableDelay();
/** /**
* Returns false if we were interrupted * Returns false if we were interrupted
*/ */
bool delay(uint32_t msec); bool delay(uint32_t msec);
void interrupt(); void interrupt();
void interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken); void interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken);
}; };
} // namespace concurrency } // namespace concurrency
src/concurrency/Lock.cpp
+14 -18
View File
@@ -2,30 +2,26 @@
#include "configuration.h" #include "configuration.h"
#include <cassert> #include <cassert>
namespace concurrency namespace concurrency {
{
#ifdef HAS_FREE_RTOS #ifdef HAS_FREE_RTOS
Lock::Lock() : handle(xSemaphoreCreateBinary()) Lock::Lock() : handle(xSemaphoreCreateBinary()) {
{ assert(handle);
assert(handle); if (xSemaphoreGive(handle) == false) {
if (xSemaphoreGive(handle) == false) { abort();
abort(); }
}
} }
void Lock::lock() void Lock::lock() {
{ if (xSemaphoreTake(handle, portMAX_DELAY) == false) {
if (xSemaphoreTake(handle, portMAX_DELAY) == false) { abort();
abort(); }
}
} }
void Lock::unlock() void Lock::unlock() {
{ if (xSemaphoreGive(handle) == false) {
if (xSemaphoreGive(handle) == false) { abort();
abort(); }
}
} }
#else #else
Lock::Lock() {} Lock::Lock() {}
src/concurrency/Lock.h
+16 -18
View File
@@ -2,33 +2,31 @@
#include "../freertosinc.h" #include "../freertosinc.h"
namespace concurrency namespace concurrency {
{
/** /**
* @brief Simple wrapper around FreeRTOS API for implementing a mutex lock * @brief Simple wrapper around FreeRTOS API for implementing a mutex lock
*/ */
class Lock class Lock {
{ public:
public: Lock();
Lock();
Lock(const Lock &) = delete; Lock(const Lock &) = delete;
Lock &operator=(const Lock &) = delete; Lock &operator=(const Lock &) = delete;
/// Locks the lock. /// Locks the lock.
// //
// Must not be called from an ISR. // Must not be called from an ISR.
void lock(); void lock();
// Unlocks the lock. // Unlocks the lock.
// //
// Must not be called from an ISR. // Must not be called from an ISR.
void unlock(); void unlock();
private: private:
#ifdef HAS_FREE_RTOS #ifdef HAS_FREE_RTOS
SemaphoreHandle_t handle; SemaphoreHandle_t handle;
#endif #endif
}; };
src/concurrency/LockGuard.cpp
+3 -10
View File
@@ -1,17 +1,10 @@
#include "LockGuard.h" #include "LockGuard.h"
#include "configuration.h" #include "configuration.h"
namespace concurrency namespace concurrency {
{
LockGuard::LockGuard(Lock *lock) : lock(lock) LockGuard::LockGuard(Lock *lock) : lock(lock) { lock->lock(); }
{
lock->lock();
}
LockGuard::~LockGuard() LockGuard::~LockGuard() { lock->unlock(); }
{
lock->unlock();
}
} // namespace concurrency } // namespace concurrency
src/concurrency/LockGuard.h
+9 -11
View File
@@ -2,23 +2,21 @@
#include "Lock.h" #include "Lock.h"
namespace concurrency namespace concurrency {
{
/** /**
* @brief RAII lock guard * @brief RAII lock guard
*/ */
class LockGuard class LockGuard {
{ public:
public: explicit LockGuard(Lock *lock);
explicit LockGuard(Lock *lock); ~LockGuard();
~LockGuard();
LockGuard(const LockGuard &) = delete; LockGuard(const LockGuard &) = delete;
LockGuard &operator=(const LockGuard &) = delete; LockGuard &operator=(const LockGuard &) = delete;
private: private:
Lock *lock; Lock *lock;
}; };
} // namespace concurrency } // namespace concurrency
src/concurrency/NotifiedWorkerThread.cpp
+44 -51
View File
@@ -2,45 +2,42 @@
#include "configuration.h" #include "configuration.h"
#include "main.h" #include "main.h"
namespace concurrency namespace concurrency {
{
static bool debugNotification; static bool debugNotification;
/** /**
* Notify this thread so it can run * Notify this thread so it can run
*/ */
bool NotifiedWorkerThread::notify(uint32_t v, bool overwrite) bool NotifiedWorkerThread::notify(uint32_t v, bool overwrite) {
{ bool r = notifyCommon(v, overwrite);
bool r = notifyCommon(v, overwrite);
if (r) if (r)
mainDelay.interrupt(); mainDelay.interrupt();
return r; return r;
} }
/** /**
* Notify this thread so it can run * Notify this thread so it can run
*/ */
IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite) IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite) {
{ if (overwrite || notification == 0) {
if (overwrite || notification == 0) { enabled = true;
enabled = true; setInterval(0); // Run ASAP
setInterval(0); // Run ASAP runASAP = true;
runASAP = true;
notification = v; notification = v;
if (debugNotification) { if (debugNotification) {
LOG_DEBUG("Set notification %d", v); LOG_DEBUG("Set notification %d", v);
}
return true;
} else {
if (debugNotification) {
LOG_DEBUG("Drop notification %d", v);
}
return false;
} }
return true;
} else {
if (debugNotification) {
LOG_DEBUG("Drop notification %d", v);
}
return false;
}
} }
/** /**
@@ -48,47 +45,43 @@ IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite)
* *
* This must be inline or IRAM_ATTR on ESP32 * This must be inline or IRAM_ATTR on ESP32
*/ */
IRAM_ATTR bool NotifiedWorkerThread::notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite) IRAM_ATTR bool NotifiedWorkerThread::notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite) {
{ bool r = notifyCommon(v, overwrite);
bool r = notifyCommon(v, overwrite); if (r)
if (r) mainDelay.interruptFromISR(highPriWoken);
mainDelay.interruptFromISR(highPriWoken);
return r; return r;
} }
/** /**
* Schedule a notification to fire in delay msecs * Schedule a notification to fire in delay msecs
*/ */
bool NotifiedWorkerThread::notifyLater(uint32_t delay, uint32_t v, bool overwrite) bool NotifiedWorkerThread::notifyLater(uint32_t delay, uint32_t v, bool overwrite) {
{ bool didIt = notify(v, overwrite);
bool didIt = notify(v, overwrite);
if (didIt) { // If we didn't already have something queued, override the delay to be larger if (didIt) { // If we didn't already have something queued, override the delay to be larger
setIntervalFromNow(delay); // a new version of setInterval relative to the current time setIntervalFromNow(delay); // a new version of setInterval relative to the current time
if (debugNotification) { if (debugNotification) {
LOG_DEBUG("Delay notification %u", delay); LOG_DEBUG("Delay notification %u", delay);
}
} }
}
return didIt; return didIt;
} }
void NotifiedWorkerThread::checkNotification() void NotifiedWorkerThread::checkNotification() {
{ auto n = notification;
auto n = notification; notification = 0; // clear notification
notification = 0; // clear notification if (n) {
if (n) { onNotify(n);
onNotify(n); }
}
} }
int32_t NotifiedWorkerThread::runOnce() int32_t NotifiedWorkerThread::runOnce() {
{ enabled = false; // Only run once per notification
enabled = false; // Only run once per notification checkNotification();
checkNotification();
return RUN_SAME; return RUN_SAME;
} }
} // namespace concurrency } // namespace concurrency
src/concurrency/NotifiedWorkerThread.h
+35 -37
View File
@@ -2,55 +2,53 @@
#include "OSThread.h" #include "OSThread.h"
namespace concurrency namespace concurrency {
{
/** /**
* @brief A worker thread that waits on a freertos notification * @brief A worker thread that waits on a freertos notification
*/ */
class NotifiedWorkerThread : public OSThread class NotifiedWorkerThread : public OSThread {
{ /**
/** * The notification that was most recently used to wake the thread. Read from runOnce()
* The notification that was most recently used to wake the thread. Read from runOnce() */
*/ uint32_t notification = 0;
uint32_t notification = 0;
public: public:
NotifiedWorkerThread(const char *name) : OSThread(name) {} NotifiedWorkerThread(const char *name) : OSThread(name) {}
/** /**
* Notify this thread so it can run * Notify this thread so it can run
*/ */
bool notify(uint32_t v, bool overwrite); bool notify(uint32_t v, bool overwrite);
/** /**
* Notify from an ISR * Notify from an ISR
* *
* This must be inline or IRAM_ATTR on ESP32 * This must be inline or IRAM_ATTR on ESP32
*/ */
bool notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite); bool notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite);
/** /**
* Schedule a notification to fire in delay msecs * Schedule a notification to fire in delay msecs
*/ */
bool notifyLater(uint32_t delay, uint32_t v, bool overwrite); bool notifyLater(uint32_t delay, uint32_t v, bool overwrite);
protected: protected:
virtual void onNotify(uint32_t notification) = 0; virtual void onNotify(uint32_t notification) = 0;
/// just calls checkNotification() /// just calls checkNotification()
virtual int32_t runOnce() override; virtual int32_t runOnce() override;
/// Sometimes we might want to check notifications independently of when our thread was getting woken up (i.e. if we are about /// Sometimes we might want to check notifications independently of when our thread was getting woken up (i.e. if we
/// to change radio transmit/receive modes we want to handle any pending interrupts first). You can call this method and if /// are about to change radio transmit/receive modes we want to handle any pending interrupts first). You can call
/// any notifications are currently pending they will be handled immediately. /// this method and if any notifications are currently pending they will be handled immediately.
void checkNotification(); void checkNotification();
private: private:
/** /**
* Notify this thread so it can run * Notify this thread so it can run
*/ */
bool notifyCommon(uint32_t v, bool overwrite); bool notifyCommon(uint32_t v, bool overwrite);
}; };
} // namespace concurrency } // namespace concurrency
src/concurrency/OSThread.cpp
+64 -74
View File
@@ -3,8 +3,7 @@
#include "memGet.h" #include "memGet.h"
#include <assert.h> #include <assert.h>
namespace concurrency namespace concurrency {
{
/// Show debugging info for disabled threads /// Show debugging info for disabled threads
bool OSThread::showDisabled; bool OSThread::showDisabled;
@@ -20,93 +19,85 @@ const OSThread *OSThread::currentThread;
ThreadController mainController, timerController; ThreadController mainController, timerController;
InterruptableDelay mainDelay; InterruptableDelay mainDelay;
void OSThread::setup() void OSThread::setup() {
{ mainController.ThreadName = "mainController";
mainController.ThreadName = "mainController"; timerController.ThreadName = "timerController";
timerController.ThreadName = "timerController";
} }
OSThread::OSThread(const char *_name, uint32_t period, ThreadController *_controller) OSThread::OSThread(const char *_name, uint32_t period, ThreadController *_controller) : Thread(NULL, period), controller(_controller) {
: Thread(NULL, period), controller(_controller) assertIsSetup();
{
assertIsSetup();
ThreadName = _name; ThreadName = _name;
if (controller) { if (controller) {
bool added = controller->add(this); bool added = controller->add(this);
assert(added); assert(added);
} }
} }
OSThread::~OSThread() OSThread::~OSThread() {
{ if (controller)
if (controller) controller->remove(this);
controller->remove(this);
} }
/** /**
* Wait a specified number msecs starting from the current time (rather than the last time we were run) * Wait a specified number msecs starting from the current time (rather than the last time we were run)
*/ */
void OSThread::setIntervalFromNow(unsigned long _interval) void OSThread::setIntervalFromNow(unsigned long _interval) {
{ // Save interval
// Save interval interval = _interval;
interval = _interval;
// Cache the next run based on the last_run // Cache the next run based on the last_run
_cached_next_run = millis() + interval; _cached_next_run = millis() + interval;
} }
bool OSThread::shouldRun(unsigned long time) bool OSThread::shouldRun(unsigned long time) {
{ bool r = Thread::shouldRun(time);
bool r = Thread::shouldRun(time);
if (showRun && r) { if (showRun && r) {
LOG_DEBUG("Thread %s: run", ThreadName.c_str()); LOG_DEBUG("Thread %s: run", ThreadName.c_str());
} }
if (showWaiting && enabled && !r) { if (showWaiting && enabled && !r) {
LOG_DEBUG("Thread %s: wait %lu", ThreadName.c_str(), interval); LOG_DEBUG("Thread %s: wait %lu", ThreadName.c_str(), interval);
} }
if (showDisabled && !enabled) { if (showDisabled && !enabled) {
LOG_DEBUG("Thread %s: disabled", ThreadName.c_str()); LOG_DEBUG("Thread %s: disabled", ThreadName.c_str());
} }
return r; return r;
} }
void OSThread::run() void OSThread::run() {
{
#ifdef DEBUG_HEAP #ifdef DEBUG_HEAP
auto heap = memGet.getFreeHeap(); auto heap = memGet.getFreeHeap();
#endif #endif
currentThread = this; currentThread = this;
auto newDelay = runOnce(); auto newDelay = runOnce();
#ifdef DEBUG_HEAP #ifdef DEBUG_HEAP
auto newHeap = memGet.getFreeHeap(); auto newHeap = memGet.getFreeHeap();
if (newHeap < heap) if (newHeap < heap)
LOG_HEAP("------ Thread %s leaked heap %d -> %d (%d) ------", ThreadName.c_str(), heap, newHeap, newHeap - heap); LOG_HEAP("------ Thread %s leaked heap %d -> %d (%d) ------", ThreadName.c_str(), heap, newHeap, newHeap - heap);
if (heap < newHeap) if (heap < newHeap)
LOG_HEAP("++++++ Thread %s freed heap %d -> %d (%d) ++++++", ThreadName.c_str(), heap, newHeap, newHeap - heap); LOG_HEAP("++++++ Thread %s freed heap %d -> %d (%d) ++++++", ThreadName.c_str(), heap, newHeap, newHeap - heap);
#endif #endif
#ifdef DEBUG_LOOP_TIMING #ifdef DEBUG_LOOP_TIMING
LOG_DEBUG("====== Thread next run in: %d", newDelay); LOG_DEBUG("====== Thread next run in: %d", newDelay);
#endif #endif
runned(); runned();
if (newDelay >= 0) if (newDelay >= 0)
setInterval(newDelay); setInterval(newDelay);
currentThread = NULL; currentThread = NULL;
} }
int32_t OSThread::disable() int32_t OSThread::disable() {
{ enabled = false;
enabled = false; setInterval(INT32_MAX);
setInterval(INT32_MAX);
return INT32_MAX; return INT32_MAX;
} }
/** /**
@@ -122,23 +113,22 @@ int32_t OSThread::disable()
*/ */
bool hasBeenSetup; bool hasBeenSetup;
void assertIsSetup() void assertIsSetup() {
{
/** /**
* Dear developer comrade - If this assert fails() that means you need to fix the following: * Dear developer comrade - If this assert fails() that means you need to fix the following:
* *
* This flag is set **only** when setup() starts, to provide a way for us to check for sloppy static constructor calls. * This flag is set **only** when setup() starts, to provide a way for us to check for sloppy static constructor
* Call assertIsSetup() to force a crash if someone tries to create an instance too early. * calls. Call assertIsSetup() to force a crash if someone tries to create an instance too early.
* *
* it is super important to never allocate those object statically. instead, you should explicitly * it is super important to never allocate those object statically. instead, you should explicitly
* new them at a point where you are guaranteed that other objects that this instance * new them at a point where you are guaranteed that other objects that this instance
* depends on have already been created. * depends on have already been created.
* *
* in particular, for OSThread that means "all instances must be declared via new() in setup() or later" - * in particular, for OSThread that means "all instances must be declared via new() in setup() or later" -
* this makes it guaranteed that the global mainController is fully constructed first. * this makes it guaranteed that the global mainController is fully constructed first.
*/ */
assert(hasBeenSetup); assert(hasBeenSetup);
} }
} // namespace concurrency } // namespace concurrency
src/concurrency/OSThread.h
+33 -34
View File
@@ -7,8 +7,7 @@
#include "ThreadController.h" #include "ThreadController.h"
#include "concurrency/InterruptableDelay.h" #include "concurrency/InterruptableDelay.h"
namespace concurrency namespace concurrency {
{
extern ThreadController mainController, timerController; extern ThreadController mainController, timerController;
extern InterruptableDelay mainDelay; extern InterruptableDelay mainDelay;
@@ -18,7 +17,8 @@ extern InterruptableDelay mainDelay;
/** /**
* @brief Base threading * @brief Base threading
* *
* This is a pseudo threading layer that is super easy to port, well suited to our slow network and very ram & power efficient. * This is a pseudo threading layer that is super easy to port, well suited to our slow network and very ram & power
* efficient.
* *
* TODO FIXME @geeksville * TODO FIXME @geeksville
* *
@@ -28,49 +28,48 @@ extern InterruptableDelay mainDelay;
* move typedQueue into concurrency * move typedQueue into concurrency
* remove freertos from typedqueue * remove freertos from typedqueue
*/ */
class OSThread : public Thread class OSThread : public Thread {
{ ThreadController *controller;
ThreadController *controller;
/// Show debugging info for disabled threads /// Show debugging info for disabled threads
static bool showDisabled; static bool showDisabled;
/// Show debugging info for threads when we run them /// Show debugging info for threads when we run them
static bool showRun; static bool showRun;
/// Show debugging info for threads we decide not to run; /// Show debugging info for threads we decide not to run;
static bool showWaiting; static bool showWaiting;
public: public:
/// For debug printing only (might be null) /// For debug printing only (might be null)
static const OSThread *currentThread; static const OSThread *currentThread;
OSThread(const char *name, uint32_t period = 0, ThreadController *controller = &mainController); OSThread(const char *name, uint32_t period = 0, ThreadController *controller = &mainController);
virtual ~OSThread(); virtual ~OSThread();
virtual bool shouldRun(unsigned long time); virtual bool shouldRun(unsigned long time);
static void setup(); static void setup();
virtual int32_t disable(); virtual int32_t disable();
/** /**
* Wait a specified number msecs starting from the current time (rather than the last time we were run) * Wait a specified number msecs starting from the current time (rather than the last time we were run)
*/ */
void setIntervalFromNow(unsigned long _interval); void setIntervalFromNow(unsigned long _interval);
protected: protected:
/** /**
* The method that will be called each time our thread gets a chance to run * The method that will be called each time our thread gets a chance to run
* *
* Returns desired period for next invocation (or RUN_SAME for no change) * Returns desired period for next invocation (or RUN_SAME for no change)
*/ */
virtual int32_t runOnce() = 0; virtual int32_t runOnce() = 0;
bool sleepOnNextExecution = false; bool sleepOnNextExecution = false;
// Do not override this // Do not override this
virtual void run(); virtual void run();
}; };
/** /**
src/concurrency/Periodic.h
+8 -10
View File
@@ -2,23 +2,21 @@
#include "concurrency/OSThread.h" #include "concurrency/OSThread.h"
namespace concurrency namespace concurrency {
{
/** /**
* @brief Periodically invoke a callback. This just provides C-style callback conventions * @brief Periodically invoke a callback. This just provides C-style callback conventions
* rather than a virtual function - FIXME, remove? * rather than a virtual function - FIXME, remove?
*/ */
class Periodic : public OSThread class Periodic : public OSThread {
{ int32_t (*callback)();
int32_t (*callback)();
public: public:
// callback returns the period for the next callback invocation (or 0 if we should no longer be called) // callback returns the period for the next callback invocation (or 0 if we should no longer be called)
Periodic(const char *name, int32_t (*_callback)()) : OSThread(name), callback(_callback) {} Periodic(const char *name, int32_t (*_callback)()) : OSThread(name), callback(_callback) {}
protected: protected:
int32_t runOnce() override { return callback(); } int32_t runOnce() override { return callback(); }
}; };
} // namespace concurrency } // namespace concurrency
src/configuration.h
+2 -2
View File
@@ -68,8 +68,8 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
#error APP_VERSION must be set by the build environment #error APP_VERSION must be set by the build environment
#endif #endif
// FIXME: This is still needed by the Bluetooth Stack and needs to be replaced by something better. Remnant of the old versioning // FIXME: This is still needed by the Bluetooth Stack and needs to be replaced by something better. Remnant of the old
// system. // versioning system.
#ifndef HW_VERSION #ifndef HW_VERSION
#define HW_VERSION "1.0" #define HW_VERSION "1.0"
#endif #endif
src/detect/LoRaRadioType.h
+11 -11
View File
@@ -1,17 +1,17 @@
#pragma once #pragma once
enum LoRaRadioType { enum LoRaRadioType {
NO_RADIO, NO_RADIO,
STM32WLx_RADIO, STM32WLx_RADIO,
SIM_RADIO, SIM_RADIO,
RF95_RADIO, RF95_RADIO,
SX1262_RADIO, SX1262_RADIO,
SX1268_RADIO, SX1268_RADIO,
LLCC68_RADIO, LLCC68_RADIO,
SX1280_RADIO, SX1280_RADIO,
LR1110_RADIO, LR1110_RADIO,
LR1120_RADIO, LR1120_RADIO,
LR1121_RADIO LR1121_RADIO
}; };
extern LoRaRadioType radioType; extern LoRaRadioType radioType;
src/detect/ScanI2C.cpp
+40 -62
View File
@@ -8,82 +8,60 @@ ScanI2C::ScanI2C() = default;
void ScanI2C::scanPort(ScanI2C::I2CPort port) {} void ScanI2C::scanPort(ScanI2C::I2CPort port) {}
void ScanI2C::scanPort(ScanI2C::I2CPort port, uint8_t *address, uint8_t asize) {} void ScanI2C::scanPort(ScanI2C::I2CPort port, uint8_t *address, uint8_t asize) {}
void ScanI2C::setSuppressScreen() void ScanI2C::setSuppressScreen() { shouldSuppressScreen = true; }
{
shouldSuppressScreen = true;
}
ScanI2C::FoundDevice ScanI2C::firstScreen() const ScanI2C::FoundDevice ScanI2C::firstScreen() const {
{ // Allow to override the scanner results for screen
// Allow to override the scanner results for screen if (shouldSuppressScreen)
if (shouldSuppressScreen)
return DEVICE_NONE;
ScanI2C::DeviceType types[] = {SCREEN_SSD1306, SCREEN_SH1106, SCREEN_ST7567, SCREEN_UNKNOWN};
return firstOfOrNONE(4, types);
}
ScanI2C::FoundDevice ScanI2C::firstRTC() const
{
ScanI2C::DeviceType types[] = {RTC_RV3028, RTC_PCF8563, RTC_PCF85063, RTC_RX8130CE};
return firstOfOrNONE(4, types);
}
ScanI2C::FoundDevice ScanI2C::firstKeyboard() const
{
ScanI2C::DeviceType types[] = {CARDKB, TDECKKB, BBQ10KB, RAK14004, MPR121KB, TCA8418KB};
return firstOfOrNONE(6, types);
}
ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const
{
ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948, QMA6100P, BMM150};
return firstOfOrNONE(9, types);
}
ScanI2C::FoundDevice ScanI2C::firstAQI() const
{
ScanI2C::DeviceType types[] = {PMSA0031, SCD4X};
return firstOfOrNONE(2, types);
}
ScanI2C::FoundDevice ScanI2C::firstRGBLED() const
{
ScanI2C::DeviceType types[] = {NCP5623, LP5562};
return firstOfOrNONE(2, types);
}
ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const
{
return DEVICE_NONE; return DEVICE_NONE;
ScanI2C::DeviceType types[] = {SCREEN_SSD1306, SCREEN_SH1106, SCREEN_ST7567, SCREEN_UNKNOWN};
return firstOfOrNONE(4, types);
} }
bool ScanI2C::exists(ScanI2C::DeviceType) const ScanI2C::FoundDevice ScanI2C::firstRTC() const {
{ ScanI2C::DeviceType types[] = {RTC_RV3028, RTC_PCF8563, RTC_PCF85063, RTC_RX8130CE};
return false; return firstOfOrNONE(4, types);
} }
ScanI2C::FoundDevice ScanI2C::firstOfOrNONE(size_t count, ScanI2C::DeviceType *types) const ScanI2C::FoundDevice ScanI2C::firstKeyboard() const {
{ ScanI2C::DeviceType types[] = {CARDKB, TDECKKB, BBQ10KB, RAK14004, MPR121KB, TCA8418KB};
return DEVICE_NONE; return firstOfOrNONE(6, types);
} }
size_t ScanI2C::countDevices() const ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const {
{ ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948, QMA6100P, BMM150};
return 0; return firstOfOrNONE(9, types);
} }
ScanI2C::FoundDevice ScanI2C::firstAQI() const {
ScanI2C::DeviceType types[] = {PMSA0031, SCD4X};
return firstOfOrNONE(2, types);
}
ScanI2C::FoundDevice ScanI2C::firstRGBLED() const {
ScanI2C::DeviceType types[] = {NCP5623, LP5562};
return firstOfOrNONE(2, types);
}
ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const { return DEVICE_NONE; }
bool ScanI2C::exists(ScanI2C::DeviceType) const { return false; }
ScanI2C::FoundDevice ScanI2C::firstOfOrNONE(size_t count, ScanI2C::DeviceType *types) const { return DEVICE_NONE; }
size_t ScanI2C::countDevices() const { return 0; }
ScanI2C::DeviceAddress::DeviceAddress(ScanI2C::I2CPort port, uint8_t address) : port(port), address(address) {} ScanI2C::DeviceAddress::DeviceAddress(ScanI2C::I2CPort port, uint8_t address) : port(port), address(address) {}
ScanI2C::DeviceAddress::DeviceAddress() : DeviceAddress(I2CPort::NO_I2C, 0) {} ScanI2C::DeviceAddress::DeviceAddress() : DeviceAddress(I2CPort::NO_I2C, 0) {}
bool ScanI2C::DeviceAddress::operator<(const ScanI2C::DeviceAddress &other) const bool ScanI2C::DeviceAddress::operator<(const ScanI2C::DeviceAddress &other) const {
{ return
return // If this one has no port and other has a port
// If this one has no port and other has a port (port == NO_I2C && other.port != NO_I2C)
(port == NO_I2C && other.port != NO_I2C) // if both have a port and this one's address is lower
// if both have a port and this one's address is lower || (port != NO_I2C && other.port != NO_I2C && (address < other.address));
|| (port != NO_I2C && other.port != NO_I2C && (address < other.address));
} }
ScanI2C::FoundDevice::FoundDevice(ScanI2C::DeviceType type, ScanI2C::DeviceAddress address) : type(type), address(address) {} ScanI2C::FoundDevice::FoundDevice(ScanI2C::DeviceType type, ScanI2C::DeviceAddress address) : type(type), address(address) {}
src/detect/ScanI2C.h
+128 -129
View File
@@ -3,156 +3,155 @@
#include <stddef.h> #include <stddef.h>
#include <stdint.h> #include <stdint.h>
class ScanI2C class ScanI2C {
{ public:
public: typedef enum DeviceType {
typedef enum DeviceType { NONE,
NONE, SCREEN_SSD1306,
SCREEN_SSD1306, SCREEN_SH1106,
SCREEN_SH1106, SCREEN_UNKNOWN, // has the same address as the two above but does not respond to the same commands
SCREEN_UNKNOWN, // has the same address as the two above but does not respond to the same commands SCREEN_ST7567,
SCREEN_ST7567, RTC_RV3028,
RTC_RV3028, RTC_PCF8563,
RTC_PCF8563, RTC_PCF85063,
RTC_PCF85063, RTC_RX8130CE,
RTC_RX8130CE, CARDKB,
CARDKB, TDECKKB,
TDECKKB, BBQ10KB,
BBQ10KB, RAK14004,
RAK14004, PMU_AXP192_AXP2101, // has the same adress as the TCA8418KB
PMU_AXP192_AXP2101, // has the same adress as the TCA8418KB BME_680,
BME_680, BME_280,
BME_280, BMP_280,
BMP_280, BMP_085,
BMP_085, BMP_3XX,
BMP_3XX, INA260,
INA260, INA219,
INA219, INA3221,
INA3221, MAX17048,
MAX17048, MCP9808,
MCP9808, SHT31,
SHT31, SHT4X,
SHT4X, SHTC3,
SHTC3, LPS22HB,
LPS22HB, QMC6310,
QMC6310, QMI8658,
QMI8658, QMC5883L,
QMC5883L, HMC5883L,
HMC5883L, PMSA0031,
PMSA0031, QMA6100P,
QMA6100P, MPU6050,
MPU6050, LIS3DH,
LIS3DH, BMA423,
BMA423, BQ24295,
BQ24295, LSM6DS3,
LSM6DS3, TCA9535,
TCA9535, TCA9555,
TCA9555, VEML7700,
VEML7700, RCWL9620,
RCWL9620, NCP5623,
NCP5623, LP5562,
LP5562, TSL2591,
TSL2591, OPT3001,
OPT3001, MLX90632,
MLX90632, MLX90614,
MLX90614, AHT10,
AHT10, BMX160,
BMX160, DFROBOT_LARK,
DFROBOT_LARK, NAU7802,
NAU7802, FT6336U,
FT6336U, STK8BAXX,
STK8BAXX, ICM20948,
ICM20948, SCD4X,
SCD4X, MAX30102,
MAX30102, TPS65233,
TPS65233, MPR121KB,
MPR121KB, CGRADSENS,
CGRADSENS, INA226,
INA226, NXP_SE050,
NXP_SE050, DFROBOT_RAIN,
DFROBOT_RAIN, DPS310,
DPS310, LTR390UV,
LTR390UV, RAK12035,
RAK12035, TCA8418KB,
TCA8418KB, PCT2075,
PCT2075, CST328,
CST328, BQ25896,
BQ25896, BQ27220,
BQ27220, LTR553ALS,
LTR553ALS, BHI260AP,
BHI260AP, BMM150,
BMM150, TSL2561,
TSL2561, DRV2605,
DRV2605, BH1750,
BH1750, DA217,
DA217, CHSC6X,
CHSC6X, CST226SE
CST226SE } DeviceType;
} DeviceType;
// typedef uint8_t DeviceAddress; // typedef uint8_t DeviceAddress;
typedef enum I2CPort { typedef enum I2CPort {
NO_I2C, NO_I2C,
WIRE, WIRE,
WIRE1, WIRE1,
} I2CPort; } I2CPort;
typedef struct DeviceAddress { typedef struct DeviceAddress {
// set default values for ADDRESS_NONE // set default values for ADDRESS_NONE
I2CPort port = I2CPort::NO_I2C; I2CPort port = I2CPort::NO_I2C;
uint8_t address = 0; uint8_t address = 0;
explicit DeviceAddress(I2CPort port, uint8_t address); explicit DeviceAddress(I2CPort port, uint8_t address);
DeviceAddress(); DeviceAddress();
bool operator<(const DeviceAddress &other) const; bool operator<(const DeviceAddress &other) const;
} DeviceAddress; } DeviceAddress;
static const DeviceAddress ADDRESS_NONE; static const DeviceAddress ADDRESS_NONE;
typedef uint8_t RegisterAddress; typedef uint8_t RegisterAddress;
typedef struct FoundDevice { typedef struct FoundDevice {
DeviceType type; DeviceType type;
DeviceAddress address; DeviceAddress address;
explicit FoundDevice(DeviceType = DeviceType::NONE, DeviceAddress = ADDRESS_NONE); explicit FoundDevice(DeviceType = DeviceType::NONE, DeviceAddress = ADDRESS_NONE);
} FoundDevice; } FoundDevice;
static const FoundDevice DEVICE_NONE; static const FoundDevice DEVICE_NONE;
public: public:
ScanI2C(); ScanI2C();
virtual void scanPort(ScanI2C::I2CPort); virtual void scanPort(ScanI2C::I2CPort);
virtual void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t); virtual void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t);
/* /*
* A bit of a hack, this tells the scanner not to tell later systems there is a screen to avoid enabling it. * A bit of a hack, this tells the scanner not to tell later systems there is a screen to avoid enabling it.
*/ */
void setSuppressScreen(); void setSuppressScreen();
FoundDevice firstScreen() const; FoundDevice firstScreen() const;
FoundDevice firstRTC() const; FoundDevice firstRTC() const;
FoundDevice firstKeyboard() const; FoundDevice firstKeyboard() const;
FoundDevice firstAccelerometer() const; FoundDevice firstAccelerometer() const;
FoundDevice firstAQI() const; FoundDevice firstAQI() const;
FoundDevice firstRGBLED() const; FoundDevice firstRGBLED() const;
virtual FoundDevice find(DeviceType) const; virtual FoundDevice find(DeviceType) const;
virtual bool exists(DeviceType) const; virtual bool exists(DeviceType) const;
virtual size_t countDevices() const; virtual size_t countDevices() const;
protected: protected:
virtual FoundDevice firstOfOrNONE(size_t, DeviceType[]) const; virtual FoundDevice firstOfOrNONE(size_t, DeviceType[]) const;
private: private:
bool shouldSuppressScreen = false; bool shouldSuppressScreen = false;
}; };
src/detect/ScanI2CConsumer.cpp
+5 -9
View File
@@ -3,14 +3,10 @@
static std::forward_list<ScanI2CConsumer *> ScanI2CConsumers; static std::forward_list<ScanI2CConsumer *> ScanI2CConsumers;
ScanI2CConsumer::ScanI2CConsumer() ScanI2CConsumer::ScanI2CConsumer() { ScanI2CConsumers.push_front(this); }
{
ScanI2CConsumers.push_front(this);
}
void ScanI2CCompleted(ScanI2C *i2cScanner) void ScanI2CCompleted(ScanI2C *i2cScanner) {
{ for (ScanI2CConsumer *consumer : ScanI2CConsumers) {
for (ScanI2CConsumer *consumer : ScanI2CConsumers) { consumer->i2cScanFinished(i2cScanner);
consumer->i2cScanFinished(i2cScanner); }
}
} }
src/detect/ScanI2CConsumer.h
+4 -5
View File
@@ -3,11 +3,10 @@
#include "ScanI2C.h" #include "ScanI2C.h"
#include <stddef.h> #include <stddef.h>
class ScanI2CConsumer class ScanI2CConsumer {
{ public:
public: ScanI2CConsumer();
ScanI2CConsumer(); virtual void i2cScanFinished(ScanI2C *i2cScanner) = 0;
virtual void i2cScanFinished(ScanI2C *i2cScanner) = 0;
}; };
void ScanI2CCompleted(ScanI2C *i2cScanner); void ScanI2CCompleted(ScanI2C *i2cScanner);
src/detect/ScanI2CTwoWire.cpp
+552 -571
View File
File diff suppressed because it is too large Load Diff
src/detect/ScanI2CTwoWire.h
+24 -28
View File
@@ -14,49 +14,45 @@
#include "../concurrency/Lock.h" #include "../concurrency/Lock.h"
class ScanI2CTwoWire : public ScanI2C class ScanI2CTwoWire : public ScanI2C {
{ public:
public: void scanPort(ScanI2C::I2CPort) override;
void scanPort(ScanI2C::I2CPort) override;
void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t) override; void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t) override;
ScanI2C::FoundDevice find(ScanI2C::DeviceType) const override; ScanI2C::FoundDevice find(ScanI2C::DeviceType) const override;
bool exists(ScanI2C::DeviceType) const override; bool exists(ScanI2C::DeviceType) const override;
size_t countDevices() const override; size_t countDevices() const override;
static TwoWire *fetchI2CBus(ScanI2C::DeviceAddress); static TwoWire *fetchI2CBus(ScanI2C::DeviceAddress);
protected: protected:
FoundDevice firstOfOrNONE(size_t, DeviceType[]) const override; FoundDevice firstOfOrNONE(size_t, DeviceType[]) const override;
private: private:
typedef struct RegisterLocation { typedef struct RegisterLocation {
DeviceAddress i2cAddress; DeviceAddress i2cAddress;
RegisterAddress registerAddress; RegisterAddress registerAddress;
RegisterLocation(DeviceAddress deviceAddress, RegisterAddress registerAddress) RegisterLocation(DeviceAddress deviceAddress, RegisterAddress registerAddress) : i2cAddress(deviceAddress), registerAddress(registerAddress) {}
: i2cAddress(deviceAddress), registerAddress(registerAddress)
{
}
} RegisterLocation; } RegisterLocation;
typedef uint8_t ResponseWidth; typedef uint8_t ResponseWidth;
std::map<ScanI2C::DeviceAddress, ScanI2C::DeviceType> foundDevices; std::map<ScanI2C::DeviceAddress, ScanI2C::DeviceType> foundDevices;
// note: prone to overwriting if multiple devices of a type are added at different addresses (rare?) // note: prone to overwriting if multiple devices of a type are added at different addresses (rare?)
std::map<ScanI2C::DeviceType, ScanI2C::DeviceAddress> deviceAddresses; std::map<ScanI2C::DeviceType, ScanI2C::DeviceAddress> deviceAddresses;
concurrency::Lock lock; concurrency::Lock lock;
uint16_t getRegisterValue(const RegisterLocation &, ResponseWidth, bool) const; uint16_t getRegisterValue(const RegisterLocation &, ResponseWidth, bool) const;
DeviceType probeOLED(ScanI2C::DeviceAddress) const; DeviceType probeOLED(ScanI2C::DeviceAddress) const;
static void logFoundDevice(const char *device, uint8_t address); static void logFoundDevice(const char *device, uint8_t address);
}; };
#endif #endif
src/detect/einkScan.h
+49 -53
View File
@@ -4,64 +4,60 @@
#include "../main.h" #include "../main.h"
#include <SPI.h> #include <SPI.h>
void d_writeCommand(uint8_t c) void d_writeCommand(uint8_t c) {
{ SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0));
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0)); if (PIN_EINK_DC >= 0)
if (PIN_EINK_DC >= 0) digitalWrite(PIN_EINK_DC, LOW);
digitalWrite(PIN_EINK_DC, LOW); if (PIN_EINK_CS >= 0)
if (PIN_EINK_CS >= 0) digitalWrite(PIN_EINK_CS, LOW);
digitalWrite(PIN_EINK_CS, LOW); SPI1.transfer(c);
SPI1.transfer(c); if (PIN_EINK_CS >= 0)
if (PIN_EINK_CS >= 0) digitalWrite(PIN_EINK_CS, HIGH);
digitalWrite(PIN_EINK_CS, HIGH); if (PIN_EINK_DC >= 0)
if (PIN_EINK_DC >= 0) digitalWrite(PIN_EINK_DC, HIGH);
digitalWrite(PIN_EINK_DC, HIGH); SPI1.endTransaction();
SPI1.endTransaction();
} }
void d_writeData(uint8_t d) void d_writeData(uint8_t d) {
{ SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0));
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0)); if (PIN_EINK_CS >= 0)
if (PIN_EINK_CS >= 0) digitalWrite(PIN_EINK_CS, LOW);
digitalWrite(PIN_EINK_CS, LOW); SPI1.transfer(d);
SPI1.transfer(d); if (PIN_EINK_CS >= 0)
if (PIN_EINK_CS >= 0) digitalWrite(PIN_EINK_CS, HIGH);
digitalWrite(PIN_EINK_CS, HIGH); SPI1.endTransaction();
SPI1.endTransaction();
} }
unsigned long d_waitWhileBusy(uint16_t busy_time) unsigned long d_waitWhileBusy(uint16_t busy_time) {
{ if (PIN_EINK_BUSY >= 0) {
if (PIN_EINK_BUSY >= 0) { delay(1); // add some margin to become active
delay(1); // add some margin to become active unsigned long start = micros();
unsigned long start = micros(); while (1) {
while (1) { if (digitalRead(PIN_EINK_BUSY) != HIGH)
if (digitalRead(PIN_EINK_BUSY) != HIGH) break;
break; delay(1);
delay(1); if (digitalRead(PIN_EINK_BUSY) != HIGH)
if (digitalRead(PIN_EINK_BUSY) != HIGH) break;
break; if (micros() - start > 10000000)
if (micros() - start > 10000000) break;
break; }
} unsigned long elapsed = micros() - start;
unsigned long elapsed = micros() - start; (void)start;
(void)start; return elapsed;
return elapsed; } else
} else return busy_time;
return busy_time;
} }
void scanEInkDevice(void) void scanEInkDevice(void) {
{ SPI1.begin();
SPI1.begin(); d_writeCommand(0x22);
d_writeCommand(0x22); d_writeData(0x83);
d_writeData(0x83); d_writeCommand(0x20);
d_writeCommand(0x20); eink_found = (d_waitWhileBusy(150) > 0) ? true : false;
eink_found = (d_waitWhileBusy(150) > 0) ? true : false; if (eink_found)
if (eink_found) LOG_DEBUG("EInk display found");
LOG_DEBUG("EInk display found"); else
else LOG_DEBUG("EInk display not found");
LOG_DEBUG("EInk display not found"); SPI1.end();
SPI1.end();
} }
#endif #endif
src/freertosinc.h
+2 -2
View File
@@ -1,7 +1,7 @@
#pragma once #pragma once
// The FreeRTOS includes are in a different directory on ESP32 and I can't figure out how to make that work with platformio gcc // The FreeRTOS includes are in a different directory on ESP32 and I can't figure out how to make that work with
// options so this is my quick hack to make things work // platformio gcc options so this is my quick hack to make things work
#if defined(ARDUINO_ARCH_ESP32) #if defined(ARDUINO_ARCH_ESP32)
#define HAS_FREE_RTOS #define HAS_FREE_RTOS
src/gps/GPS.cpp
+1468 -1515
View File
File diff suppressed because it is too large Load Diff
src/gps/GPS.h
+158 -159
View File
@@ -20,235 +20,234 @@ static constexpr uint32_t GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS = 10 * 1000UL;
static constexpr uint32_t GPS_FIX_HOLD_MAX_MS = 20000; static constexpr uint32_t GPS_FIX_HOLD_MAX_MS = 20000;
typedef enum { typedef enum {
GNSS_MODEL_ATGM336H, GNSS_MODEL_ATGM336H,
GNSS_MODEL_MTK, GNSS_MODEL_MTK,
GNSS_MODEL_UBLOX6, GNSS_MODEL_UBLOX6,
GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX7,
GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX8,
GNSS_MODEL_UBLOX9, GNSS_MODEL_UBLOX9,
GNSS_MODEL_UBLOX10, GNSS_MODEL_UBLOX10,
GNSS_MODEL_UC6580, GNSS_MODEL_UC6580,
GNSS_MODEL_UNKNOWN, GNSS_MODEL_UNKNOWN,
GNSS_MODEL_MTK_L76B, GNSS_MODEL_MTK_L76B,
GNSS_MODEL_MTK_PA1010D, GNSS_MODEL_MTK_PA1010D,
GNSS_MODEL_MTK_PA1616S, GNSS_MODEL_MTK_PA1616S,
GNSS_MODEL_AG3335, GNSS_MODEL_AG3335,
GNSS_MODEL_AG3352, GNSS_MODEL_AG3352,
GNSS_MODEL_LS20031, GNSS_MODEL_LS20031,
GNSS_MODEL_CM121 GNSS_MODEL_CM121
} GnssModel_t; } GnssModel_t;
typedef enum { typedef enum {
GNSS_RESPONSE_NONE, GNSS_RESPONSE_NONE,
GNSS_RESPONSE_NAK, GNSS_RESPONSE_NAK,
GNSS_RESPONSE_FRAME_ERRORS, GNSS_RESPONSE_FRAME_ERRORS,
GNSS_RESPONSE_OK, GNSS_RESPONSE_OK,
} GPS_RESPONSE; } GPS_RESPONSE;
enum GPSPowerState : uint8_t { enum GPSPowerState : uint8_t {
GPS_ACTIVE, // Awake and want a position GPS_ACTIVE, // Awake and want a position
GPS_IDLE, // Awake, but not wanting another position yet GPS_IDLE, // Awake, but not wanting another position yet
GPS_SOFTSLEEP, // Physically powered on, but soft-sleeping GPS_SOFTSLEEP, // Physically powered on, but soft-sleeping
GPS_HARDSLEEP, // Physically powered off, but scheduled to wake GPS_HARDSLEEP, // Physically powered off, but scheduled to wake
GPS_OFF // Powered off indefinitely GPS_OFF // Powered off indefinitely
}; };
struct ChipInfo { struct ChipInfo {
String chipName; // The name of the chip (for logging) String chipName; // The name of the chip (for logging)
String detectionString; // The string to match in the response String detectionString; // The string to match in the response
GnssModel_t driver; // The driver to use GnssModel_t driver; // The driver to use
}; };
/** /**
* A gps class that only reads from the GPS periodically and keeps the gps powered down except when reading * A gps class that only reads from the GPS periodically and keeps the gps powered down except when reading
* *
* When new data is available it will notify observers. * When new data is available it will notify observers.
*/ */
class GPS : private concurrency::OSThread class GPS : private concurrency::OSThread {
{ public:
public: meshtastic_Position p = meshtastic_Position_init_default;
meshtastic_Position p = meshtastic_Position_init_default;
/** This is normally bound to config.position.gps_en_gpio but some rare boards (like heltec tracker) need more advanced /** This is normally bound to config.position.gps_en_gpio but some rare boards (like heltec tracker) need more
* implementations. Those boards will set this public variable to a custom implementation. * advanced implementations. Those boards will set this public variable to a custom implementation.
* *
* Normally set by GPS::createGPS() * Normally set by GPS::createGPS()
*/ */
GpioVirtPin *enablePin = NULL; GpioVirtPin *enablePin = NULL;
virtual ~GPS(); virtual ~GPS();
/** We will notify this observable anytime GPS state has changed meaningfully */ /** We will notify this observable anytime GPS state has changed meaningfully */
Observable<const meshtastic::GPSStatus *> newStatus; Observable<const meshtastic::GPSStatus *> newStatus;
/** /**
* Returns true if we succeeded * Returns true if we succeeded
*/ */
virtual bool setup(); virtual bool setup();
// re-enable the thread // re-enable the thread
void enable(); void enable();
// Disable the thread // Disable the thread
int32_t disable() override; int32_t disable() override;
// toggle between enabled/disabled // toggle between enabled/disabled
void toggleGpsMode(); void toggleGpsMode();
// Change the power state of the GPS - for power saving / shutdown // Change the power state of the GPS - for power saving / shutdown
void setPowerState(GPSPowerState newState, uint32_t sleepMs = 0); void setPowerState(GPSPowerState newState, uint32_t sleepMs = 0);
/// Returns true if we have acquired GPS lock. /// Returns true if we have acquired GPS lock.
virtual bool hasLock(); virtual bool hasLock();
/// Returns true if there's valid data flow with the chip. /// Returns true if there's valid data flow with the chip.
virtual bool hasFlow(); virtual bool hasFlow();
/// Return true if we are connected to a GPS /// Return true if we are connected to a GPS
bool isConnected() const { return hasGPS; } bool isConnected() const { return hasGPS; }
bool isPowerSaving() const { return config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED; } bool isPowerSaving() const { return config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED; }
// Empty the input buffer as quickly as possible // Empty the input buffer as quickly as possible
void clearBuffer(); void clearBuffer();
// Creates an instance of the GPS class. // Creates an instance of the GPS class.
// Returns the new instance or null if the GPS is not present. // Returns the new instance or null if the GPS is not present.
static GPS *createGps(); static GPS *createGps();
// Wake the GPS hardware - ready for an update // Wake the GPS hardware - ready for an update
void up(); void up();
// Let the GPS hardware save power between updates // Let the GPS hardware save power between updates
void down(); void down();
private: private:
GPS() : concurrency::OSThread("GPS") {} GPS() : concurrency::OSThread("GPS") {}
/// Record that we have a GPS /// Record that we have a GPS
void setConnected(); void setConnected();
/** Subclasses should look for serial rx characters here and feed it to their GPS parser /** Subclasses should look for serial rx characters here and feed it to their GPS parser
* *
* Return true if we received a valid message from the GPS * Return true if we received a valid message from the GPS
*/ */
virtual bool whileActive(); virtual bool whileActive();
/** /**
* Perform any processing that should be done only while the GPS is awake and looking for a fix. * Perform any processing that should be done only while the GPS is awake and looking for a fix.
* Override this method to check for new locations * Override this method to check for new locations
* *
* @return true if we've acquired a time * @return true if we've acquired a time
*/ */
virtual bool lookForTime(); virtual bool lookForTime();
/** /**
* Perform any processing that should be done only while the GPS is awake and looking for a fix. * Perform any processing that should be done only while the GPS is awake and looking for a fix.
* Override this method to check for new locations * Override this method to check for new locations
* *
* @return true if we've acquired a new location * @return true if we've acquired a new location
*/ */
virtual bool lookForLocation(); virtual bool lookForLocation();
GnssModel_t gnssModel = GNSS_MODEL_UNKNOWN; GnssModel_t gnssModel = GNSS_MODEL_UNKNOWN;
TinyGPSPlus reader; TinyGPSPlus reader;
uint8_t fixQual = 0; // fix quality from GPGGA uint8_t fixQual = 0; // fix quality from GPGGA
uint32_t lastChecksumFailCount = 0; uint32_t lastChecksumFailCount = 0;
uint8_t currentStep = 0; uint8_t currentStep = 0;
int32_t currentDelay = 2000; int32_t currentDelay = 2000;
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS #ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
// (20210908) TinyGps++ can only read the GPGSA "FIX TYPE" field // (20210908) TinyGps++ can only read the GPGSA "FIX TYPE" field
// via optional feature "custom fields", currently disabled (bug #525) // via optional feature "custom fields", currently disabled (bug #525)
TinyGPSCustom gsafixtype; // custom extract fix type from GPGSA TinyGPSCustom gsafixtype; // custom extract fix type from GPGSA
TinyGPSCustom gsapdop; // custom extract PDOP from GPGSA TinyGPSCustom gsapdop; // custom extract PDOP from GPGSA
uint8_t fixType = 0; // fix type from GPGSA uint8_t fixType = 0; // fix type from GPGSA
#endif #endif
uint32_t fixHoldEnds = 0; uint32_t fixHoldEnds = 0;
uint32_t rx_gpio = 0; uint32_t rx_gpio = 0;
uint32_t tx_gpio = 0; uint32_t tx_gpio = 0;
uint8_t speedSelect = 0; uint8_t speedSelect = 0;
uint8_t probeTries = 0; uint8_t probeTries = 0;
/** /**
* hasValidLocation - indicates that the position variables contain a complete * hasValidLocation - indicates that the position variables contain a complete
* GPS location, valid and fresh (< gps_update_interval + position_broadcast_secs) * GPS location, valid and fresh (< gps_update_interval + position_broadcast_secs)
*/ */
bool hasValidLocation = false; // default to false, until we complete our first read bool hasValidLocation = false; // default to false, until we complete our first read
bool shouldPublish = false; // If we've changed GPS state, this will force a publish the next loop() bool shouldPublish = false; // If we've changed GPS state, this will force a publish the next loop()
bool hasGPS = false; // Do we have a GPS we are talking to bool hasGPS = false; // Do we have a GPS we are talking to
bool GPSInitFinished = false; // Init thread finished? bool GPSInitFinished = false; // Init thread finished?
bool GPSInitStarted = false; // Init thread finished? bool GPSInitStarted = false; // Init thread finished?
GPSPowerState powerState = GPS_OFF; // GPS_ACTIVE if we want a location right now GPSPowerState powerState = GPS_OFF; // GPS_ACTIVE if we want a location right now
uint8_t numSatellites = 0; uint8_t numSatellites = 0;
CallbackObserver<GPS, void *> notifyDeepSleepObserver = CallbackObserver<GPS, void *>(this, &GPS::prepareDeepSleep); CallbackObserver<GPS, void *> notifyDeepSleepObserver = CallbackObserver<GPS, void *>(this, &GPS::prepareDeepSleep);
/** If !NULL we will use this serial port to construct our GPS */ /** If !NULL we will use this serial port to construct our GPS */
#if defined(ARCH_RP2040) #if defined(ARCH_RP2040)
static SerialUART *_serial_gps; static SerialUART *_serial_gps;
#elif defined(ARCH_NRF52) #elif defined(ARCH_NRF52)
static Uart *_serial_gps; static Uart *_serial_gps;
#else #else
static HardwareSerial *_serial_gps; static HardwareSerial *_serial_gps;
#endif #endif
// Create a ublox packet for editing in memory // Create a ublox packet for editing in memory
uint8_t makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg); uint8_t makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
uint8_t makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg); uint8_t makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
// scratch space for creating ublox packets // scratch space for creating ublox packets
uint8_t UBXscratch[250] = {0}; uint8_t UBXscratch[250] = {0};
int rebootsSeen = 0; int rebootsSeen = 0;
int getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, uint32_t waitMillis); int getACK(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID, uint32_t waitMillis);
GPS_RESPONSE getACK(uint8_t c, uint8_t i, uint32_t waitMillis); GPS_RESPONSE getACK(uint8_t c, uint8_t i, uint32_t waitMillis);
GPS_RESPONSE getACK(const char *message, uint32_t waitMillis); GPS_RESPONSE getACK(const char *message, uint32_t waitMillis);
GPS_RESPONSE getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis); GPS_RESPONSE getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis);
/// Prepare the GPS for the cpu entering deep sleep, expect to be gone for at least 100s of msecs /// Prepare the GPS for the cpu entering deep sleep, expect to be gone for at least 100s of msecs
/// always returns 0 to indicate okay to sleep /// always returns 0 to indicate okay to sleep
int prepareDeepSleep(void *unused); int prepareDeepSleep(void *unused);
/** Set power with EN pin, if relevant /** Set power with EN pin, if relevant
*/ */
void writePinEN(bool on); void writePinEN(bool on);
/** Set the value of the STANDBY pin, if relevant /** Set the value of the STANDBY pin, if relevant
*/ */
void writePinStandby(bool standby); void writePinStandby(bool standby);
/** Set GPS power with PMU, if relevant /** Set GPS power with PMU, if relevant
*/ */
void setPowerPMU(bool on); void setPowerPMU(bool on);
/** Set UBLOX power, if relevant /** Set UBLOX power, if relevant
*/ */
void setPowerUBLOX(bool on, uint32_t sleepMs = 0); void setPowerUBLOX(bool on, uint32_t sleepMs = 0);
/** /**
* Tell users we have new GPS readings * Tell users we have new GPS readings
*/ */
void publishUpdate(); void publishUpdate();
virtual int32_t runOnce() override; virtual int32_t runOnce() override;
GnssModel_t getProbeResponse(unsigned long timeout, const std::vector<ChipInfo> &responseMap, int serialSpeed); GnssModel_t getProbeResponse(unsigned long timeout, const std::vector<ChipInfo> &responseMap, int serialSpeed);
// Get GNSS model // Get GNSS model
GnssModel_t probe(int serialSpeed); GnssModel_t probe(int serialSpeed);
// delay counter to allow more sats before fixed position stops GPS thread // delay counter to allow more sats before fixed position stops GPS thread
uint8_t fixeddelayCtr = 0; uint8_t fixeddelayCtr = 0;
}; };
extern GPS *gps; extern GPS *gps;
src/gps/GPSUpdateScheduling.cpp
+58 -74
View File
@@ -3,116 +3,100 @@
#include "Default.h" #include "Default.h"
// Mark the time when searching for GPS position begins // Mark the time when searching for GPS position begins
void GPSUpdateScheduling::informSearching() void GPSUpdateScheduling::informSearching() { searchStartedMs = millis(); }
{
searchStartedMs = millis();
}
// Mark the time when searching for GPS is complete, // Mark the time when searching for GPS is complete,
// then update the predicted lock-time // then update the predicted lock-time
void GPSUpdateScheduling::informGotLock() void GPSUpdateScheduling::informGotLock() {
{ searchEndedMs = millis();
searchEndedMs = millis(); LOG_DEBUG("Took %us to get lock", (searchEndedMs - searchStartedMs) / 1000);
LOG_DEBUG("Took %us to get lock", (searchEndedMs - searchStartedMs) / 1000); updateLockTimePrediction();
updateLockTimePrediction();
} }
// Clear old lock-time prediction data. // Clear old lock-time prediction data.
// When re-enabling GPS with user button. // When re-enabling GPS with user button.
void GPSUpdateScheduling::reset() void GPSUpdateScheduling::reset() {
{ searchStartedMs = 0;
searchStartedMs = 0; searchEndedMs = 0;
searchEndedMs = 0; searchCount = 0;
searchCount = 0; predictedMsToGetLock = 0;
predictedMsToGetLock = 0;
} }
// How many milliseconds before we should next search for GPS position // How many milliseconds before we should next search for GPS position
// Used by GPS hardware directly, to enter timed hardware sleep // Used by GPS hardware directly, to enter timed hardware sleep
uint32_t GPSUpdateScheduling::msUntilNextSearch() uint32_t GPSUpdateScheduling::msUntilNextSearch() {
{ uint32_t now = millis();
uint32_t now = millis();
// Target interval (seconds), between GPS updates // Target interval (seconds), between GPS updates
uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval, default_gps_update_interval); uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval, default_gps_update_interval);
// Check how long until we should start searching, to hopefully hit our target interval // Check how long until we should start searching, to hopefully hit our target interval
uint32_t dueAtMs = searchEndedMs + updateInterval; uint32_t dueAtMs = searchEndedMs + updateInterval;
uint32_t compensatedStart = dueAtMs - predictedMsToGetLock; uint32_t compensatedStart = dueAtMs - predictedMsToGetLock;
int32_t remainingMs = compensatedStart - now; int32_t remainingMs = compensatedStart - now;
// If we should have already started (negative value), start ASAP // If we should have already started (negative value), start ASAP
if (remainingMs < 0) if (remainingMs < 0)
remainingMs = 0; remainingMs = 0;
return (uint32_t)remainingMs; return (uint32_t)remainingMs;
} }
// How long have we already been searching? // How long have we already been searching?
// Used to abort a search in progress, if it runs unacceptably long // Used to abort a search in progress, if it runs unacceptably long
uint32_t GPSUpdateScheduling::elapsedSearchMs() uint32_t GPSUpdateScheduling::elapsedSearchMs() {
{ // If searching
// If searching if (searchStartedMs > searchEndedMs)
if (searchStartedMs > searchEndedMs) return millis() - searchStartedMs;
return millis() - searchStartedMs;
// If not searching - 0ms. We shouldn't really consume this value // If not searching - 0ms. We shouldn't really consume this value
else else
return 0; return 0;
} }
// Is it now time to begin searching for a GPS position? // Is it now time to begin searching for a GPS position?
bool GPSUpdateScheduling::isUpdateDue() bool GPSUpdateScheduling::isUpdateDue() { return (msUntilNextSearch() == 0); }
{
return (msUntilNextSearch() == 0);
}
// Have we been searching for a GPS position for too long? // Have we been searching for a GPS position for too long?
bool GPSUpdateScheduling::searchedTooLong() bool GPSUpdateScheduling::searchedTooLong() {
{ uint32_t minimumOrConfiguredSecs = Default::getConfiguredOrMinimumValue(config.position.position_broadcast_secs, default_broadcast_interval_secs);
uint32_t minimumOrConfiguredSecs = uint32_t maxSearchMs = Default::getConfiguredOrDefaultMs(minimumOrConfiguredSecs, default_broadcast_interval_secs);
Default::getConfiguredOrMinimumValue(config.position.position_broadcast_secs, default_broadcast_interval_secs); // If broadcast interval set to max, no such thing as "too long"
uint32_t maxSearchMs = Default::getConfiguredOrDefaultMs(minimumOrConfiguredSecs, default_broadcast_interval_secs); if (maxSearchMs == UINT32_MAX)
// If broadcast interval set to max, no such thing as "too long" return false;
if (maxSearchMs == UINT32_MAX)
return false;
// If we've been searching longer than our position broadcast interval: that's too long // If we've been searching longer than our position broadcast interval: that's too long
else if (elapsedSearchMs() > maxSearchMs) else if (elapsedSearchMs() > maxSearchMs)
return true; return true;
// Otherwise, not too long yet! // Otherwise, not too long yet!
else else
return false; return false;
} }
// Updates the predicted time-to-get-lock, by exponentially smoothing the latest observation // Updates the predicted time-to-get-lock, by exponentially smoothing the latest observation
void GPSUpdateScheduling::updateLockTimePrediction() void GPSUpdateScheduling::updateLockTimePrediction() {
{
// How long did it take to get GPS lock this time? // How long did it take to get GPS lock this time?
// Duration between down() calls // Duration between down() calls
int32_t lockTime = searchEndedMs - searchStartedMs; int32_t lockTime = searchEndedMs - searchStartedMs;
if (lockTime < 0) if (lockTime < 0)
lockTime = 0; lockTime = 0;
// Ignore the first lock-time: likely to be long, will skew data // Ignore the first lock-time: likely to be long, will skew data
// Second locktime: likely stable. Use to initialize the smoothing filter // Second locktime: likely stable. Use to initialize the smoothing filter
if (searchCount == 1) if (searchCount == 1)
predictedMsToGetLock = lockTime; predictedMsToGetLock = lockTime;
// Third locktime and after: predict using exponential smoothing. Respond slowly to changes // Third locktime and after: predict using exponential smoothing. Respond slowly to changes
else if (searchCount > 1) else if (searchCount > 1)
predictedMsToGetLock = (lockTime * weighting) + (predictedMsToGetLock * (1 - weighting)); predictedMsToGetLock = (lockTime * weighting) + (predictedMsToGetLock * (1 - weighting));
searchCount++; // Only tracked so we can disregard initial lock-times searchCount++; // Only tracked so we can disregard initial lock-times
LOG_DEBUG("Predict %us to get next lock", predictedMsToGetLock / 1000); LOG_DEBUG("Predict %us to get next lock", predictedMsToGetLock / 1000);
} }
// How long do we expect to spend searching for a lock? // How long do we expect to spend searching for a lock?
uint32_t GPSUpdateScheduling::predictedSearchDurationMs() uint32_t GPSUpdateScheduling::predictedSearchDurationMs() { return GPSUpdateScheduling::predictedMsToGetLock; }
{
return GPSUpdateScheduling::predictedMsToGetLock;
}
src/gps/GPSUpdateScheduling.h
+18 -19
View File
@@ -3,27 +3,26 @@
#include "configuration.h" #include "configuration.h"
// Encapsulates code responsible for the timing of GPS updates // Encapsulates code responsible for the timing of GPS updates
class GPSUpdateScheduling class GPSUpdateScheduling {
{ public:
public: // Marks the time of these events, for calculation use
// Marks the time of these events, for calculation use void informSearching();
void informSearching(); void informGotLock(); // Predicted lock-time is recalculated here
void informGotLock(); // Predicted lock-time is recalculated here
void reset(); // Reset the prediction - after GPS::disable() / GPS::enable() void reset(); // Reset the prediction - after GPS::disable() / GPS::enable()
bool isUpdateDue(); // Is it time to begin searching for a GPS position? bool isUpdateDue(); // Is it time to begin searching for a GPS position?
bool searchedTooLong(); // Have we been searching for too long? bool searchedTooLong(); // Have we been searching for too long?
uint32_t msUntilNextSearch(); // How long until we need to begin searching for a GPS? Info provided to GPS hardware for sleep uint32_t msUntilNextSearch(); // How long until we need to begin searching for a GPS? Info provided to GPS hardware for sleep
uint32_t elapsedSearchMs(); // How long have we been searching so far? uint32_t elapsedSearchMs(); // How long have we been searching so far?
uint32_t predictedSearchDurationMs(); // How long do we expect to spend searching for a lock? uint32_t predictedSearchDurationMs(); // How long do we expect to spend searching for a lock?
private: private:
void updateLockTimePrediction(); // Called from informGotLock void updateLockTimePrediction(); // Called from informGotLock
uint32_t searchStartedMs = 0; uint32_t searchStartedMs = 0;
uint32_t searchEndedMs = 0; uint32_t searchEndedMs = 0;
uint32_t searchCount = 0; uint32_t searchCount = 0;
uint32_t predictedMsToGetLock = 0; uint32_t predictedMsToGetLock = 0;
const float weighting = 0.2; // Controls exponential smoothing of lock-times prediction. 20% weighting of "latest lock-time". const float weighting = 0.2; // Controls exponential smoothing of lock-times prediction. 20% weighting of "latest lock-time".
}; };
src/gps/GeoCoord.cpp
+408 -449
View File
@@ -1,402 +1,378 @@
#include "GeoCoord.h" #include "GeoCoord.h"
GeoCoord::GeoCoord() GeoCoord::GeoCoord() { _dirty = true; }
{
_dirty = true; GeoCoord::GeoCoord(int32_t lat, int32_t lon, int32_t alt) : _latitude(lat), _longitude(lon), _altitude(alt) { GeoCoord::setCoords(); }
GeoCoord::GeoCoord(float lat, float lon, int32_t alt) : _altitude(alt) {
// Change decimial representation to int32_t. I.e., 12.345 becomes 123450000
_latitude = int32_t(lat * 1e+7);
_longitude = int32_t(lon * 1e+7);
GeoCoord::setCoords();
} }
GeoCoord::GeoCoord(int32_t lat, int32_t lon, int32_t alt) : _latitude(lat), _longitude(lon), _altitude(alt) GeoCoord::GeoCoord(double lat, double lon, int32_t alt) : _altitude(alt) {
{ // Change decimial representation to int32_t. I.e., 12.345 becomes 123450000
GeoCoord::setCoords(); _latitude = int32_t(lat * 1e+7);
} _longitude = int32_t(lon * 1e+7);
GeoCoord::setCoords();
GeoCoord::GeoCoord(float lat, float lon, int32_t alt) : _altitude(alt)
{
// Change decimial representation to int32_t. I.e., 12.345 becomes 123450000
_latitude = int32_t(lat * 1e+7);
_longitude = int32_t(lon * 1e+7);
GeoCoord::setCoords();
}
GeoCoord::GeoCoord(double lat, double lon, int32_t alt) : _altitude(alt)
{
// Change decimial representation to int32_t. I.e., 12.345 becomes 123450000
_latitude = int32_t(lat * 1e+7);
_longitude = int32_t(lon * 1e+7);
GeoCoord::setCoords();
} }
// Initialize all the coordinate systems // Initialize all the coordinate systems
void GeoCoord::setCoords() void GeoCoord::setCoords() {
{ double lat = _latitude * 1e-7;
double lat = _latitude * 1e-7; double lon = _longitude * 1e-7;
double lon = _longitude * 1e-7; GeoCoord::latLongToDMS(lat, lon, _dms);
GeoCoord::latLongToDMS(lat, lon, _dms); GeoCoord::latLongToUTM(lat, lon, _utm);
GeoCoord::latLongToUTM(lat, lon, _utm); GeoCoord::latLongToMGRS(lat, lon, _mgrs);
GeoCoord::latLongToMGRS(lat, lon, _mgrs); GeoCoord::latLongToOSGR(lat, lon, _osgr);
GeoCoord::latLongToOSGR(lat, lon, _osgr); GeoCoord::latLongToOLC(lat, lon, _olc);
GeoCoord::latLongToOLC(lat, lon, _olc); _dirty = false;
_dirty = false;
} }
void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt) void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt) {
{ // If marked dirty or new coordinates
// If marked dirty or new coordinates if (_dirty || _latitude != lat || _longitude != lon || _altitude != alt) {
if (_dirty || _latitude != lat || _longitude != lon || _altitude != alt) { _dirty = true;
_dirty = true; _latitude = lat;
_latitude = lat; _longitude = lon;
_longitude = lon; _altitude = alt;
_altitude = alt; setCoords();
setCoords(); }
}
} }
void GeoCoord::updateCoords(const double lat, const double lon, const int32_t alt) void GeoCoord::updateCoords(const double lat, const double lon, const int32_t alt) {
{ int32_t iLat = lat * 1e+7;
int32_t iLat = lat * 1e+7; int32_t iLon = lon * 1e+7;
int32_t iLon = lon * 1e+7; // If marked dirty or new coordinates
// If marked dirty or new coordinates if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) { _dirty = true;
_dirty = true; _latitude = iLat;
_latitude = iLat; _longitude = iLon;
_longitude = iLon; _altitude = alt;
_altitude = alt; setCoords();
setCoords(); }
}
} }
void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt) void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt) {
{ int32_t iLat = lat * 1e+7;
int32_t iLat = lat * 1e+7; int32_t iLon = lon * 1e+7;
int32_t iLon = lon * 1e+7; // If marked dirty or new coordinates
// If marked dirty or new coordinates if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) { _dirty = true;
_dirty = true; _latitude = iLat;
_latitude = iLat; _longitude = iLon;
_longitude = iLon; _altitude = alt;
_altitude = alt; setCoords();
setCoords(); }
}
} }
/** /**
* Converts lat long coordinates from decimal degrees to degrees minutes seconds format. * Converts lat long coordinates from decimal degrees to degrees minutes seconds format.
* DD°MM'SS"C DDD°MM'SS"C * DD°MM'SS"C DDD°MM'SS"C
*/ */
void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms) void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms) {
{ if (lat < 0)
if (lat < 0) dms.latCP = 'S';
dms.latCP = 'S'; else
else dms.latCP = 'N';
dms.latCP = 'N';
double latDeg = lat; double latDeg = lat;
if (lat < 0) if (lat < 0)
latDeg = latDeg * -1; latDeg = latDeg * -1;
dms.latDeg = floor(latDeg); dms.latDeg = floor(latDeg);
double latMin = (latDeg - dms.latDeg) * 60; double latMin = (latDeg - dms.latDeg) * 60;
dms.latMin = floor(latMin); dms.latMin = floor(latMin);
dms.latSec = (latMin - dms.latMin) * 60; dms.latSec = (latMin - dms.latMin) * 60;
if (lon < 0) if (lon < 0)
dms.lonCP = 'W'; dms.lonCP = 'W';
else else
dms.lonCP = 'E'; dms.lonCP = 'E';
double lonDeg = lon; double lonDeg = lon;
if (lon < 0) if (lon < 0)
lonDeg = lonDeg * -1; lonDeg = lonDeg * -1;
dms.lonDeg = floor(lonDeg); dms.lonDeg = floor(lonDeg);
double lonMin = (lonDeg - dms.lonDeg) * 60; double lonMin = (lonDeg - dms.lonDeg) * 60;
dms.lonMin = floor(lonMin); dms.lonMin = floor(lonMin);
dms.lonSec = (lonMin - dms.lonMin) * 60; dms.lonSec = (lonMin - dms.lonMin) * 60;
} }
/** /**
* Converts lat long coordinates to UTM. * Converts lat long coordinates to UTM.
* based on this: https://github.com/walvok/LatLonToUTM/blob/master/latlon_utm.ino * based on this: https://github.com/walvok/LatLonToUTM/blob/master/latlon_utm.ino
*/ */
void GeoCoord::latLongToUTM(const double lat, const double lon, UTM &utm) void GeoCoord::latLongToUTM(const double lat, const double lon, UTM &utm) {
{
const std::string latBands = "CDEFGHJKLMNPQRSTUVWXX"; const std::string latBands = "CDEFGHJKLMNPQRSTUVWXX";
utm.zone = int((lon + 180) / 6 + 1); utm.zone = int((lon + 180) / 6 + 1);
utm.band = latBands[int(lat / 8 + 10)]; utm.band = latBands[int(lat / 8 + 10)];
double a = 6378137; // WGS84 - equatorial radius double a = 6378137; // WGS84 - equatorial radius
double k0 = 0.9996; // UTM point scale on the central meridian double k0 = 0.9996; // UTM point scale on the central meridian
double eccSquared = 0.00669438; // eccentricity squared double eccSquared = 0.00669438; // eccentricity squared
double lonTemp = (lon + 180) - int((lon + 180) / 360) * 360 - 180; // Make sure the longitude is between -180.00 .. 179.9 double lonTemp = (lon + 180) - int((lon + 180) / 360) * 360 - 180; // Make sure the longitude is between -180.00 .. 179.9
double latRad = toRadians(lat); double latRad = toRadians(lat);
double lonRad = toRadians(lonTemp); double lonRad = toRadians(lonTemp);
// Special Zones for Norway and Svalbard // Special Zones for Norway and Svalbard
if (lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0) // Norway if (lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0) // Norway
utm.zone = 32; utm.zone = 32;
if (lat >= 72.0 && lat < 84.0) { // Svalbard if (lat >= 72.0 && lat < 84.0) { // Svalbard
if (lonTemp >= 0.0 && lonTemp < 9.0) if (lonTemp >= 0.0 && lonTemp < 9.0)
utm.zone = 31; utm.zone = 31;
else if (lonTemp >= 9.0 && lonTemp < 21.0) else if (lonTemp >= 9.0 && lonTemp < 21.0)
utm.zone = 33; utm.zone = 33;
else if (lonTemp >= 21.0 && lonTemp < 33.0) else if (lonTemp >= 21.0 && lonTemp < 33.0)
utm.zone = 35; utm.zone = 35;
else if (lonTemp >= 33.0 && lonTemp < 42.0) else if (lonTemp >= 33.0 && lonTemp < 42.0)
utm.zone = 37; utm.zone = 37;
} }
double lonOrigin = (utm.zone - 1) * 6 - 180 + 3; // puts origin in middle of zone double lonOrigin = (utm.zone - 1) * 6 - 180 + 3; // puts origin in middle of zone
double lonOriginRad = toRadians(lonOrigin); double lonOriginRad = toRadians(lonOrigin);
double eccPrimeSquared = (eccSquared) / (1 - eccSquared); double eccPrimeSquared = (eccSquared) / (1 - eccSquared);
double N = a / sqrt(1 - eccSquared * sin(latRad) * sin(latRad)); double N = a / sqrt(1 - eccSquared * sin(latRad) * sin(latRad));
double T = tan(latRad) * tan(latRad); double T = tan(latRad) * tan(latRad);
double C = eccPrimeSquared * cos(latRad) * cos(latRad); double C = eccPrimeSquared * cos(latRad) * cos(latRad);
double A = cos(latRad) * (lonRad - lonOriginRad); double A = cos(latRad) * (lonRad - lonOriginRad);
double M = double M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * latRad -
a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * latRad - (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * latRad) +
(3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * latRad) -
sin(2 * latRad) + (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * latRad));
(15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * latRad) - utm.easting = (double)(k0 * N * (A + (1 - T + C) * pow(A, 3) / 6 + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) +
(35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * latRad)); 500000.0);
utm.easting = (double)(k0 * N * utm.northing = (double)(k0 * (M + N * tan(latRad) *
(A + (1 - T + C) * pow(A, 3) / 6 + (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
(5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120) + (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
500000.0);
utm.northing =
(double)(k0 * (M + N * tan(latRad) *
(A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24 +
(61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
if (lat < 0) if (lat < 0)
utm.northing += 10000000.0; // 10000000 meter offset for southern hemisphere utm.northing += 10000000.0; // 10000000 meter offset for southern hemisphere
} }
// Converts lat long coordinates to an MGRS. // Converts lat long coordinates to an MGRS.
void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs) void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs) {
{ const std::string e100kLetters[3] = {"ABCDEFGH", "JKLMNPQR", "STUVWXYZ"};
const std::string e100kLetters[3] = {"ABCDEFGH", "JKLMNPQR", "STUVWXYZ"}; const std::string n100kLetters[2] = {"ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE"};
const std::string n100kLetters[2] = {"ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE"}; UTM utm;
UTM utm; latLongToUTM(lat, lon, utm);
latLongToUTM(lat, lon, utm); mgrs.zone = utm.zone;
mgrs.zone = utm.zone; mgrs.band = utm.band;
mgrs.band = utm.band; double col = floor(utm.easting / 100000);
double col = floor(utm.easting / 100000); mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3][col - 1];
mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3][col - 1]; double row = (int32_t)floor(utm.northing / 100000.0) % 20;
double row = (int32_t)floor(utm.northing / 100000.0) % 20; mgrs.north100k = n100kLetters[(mgrs.zone - 1) % 2][row];
mgrs.north100k = n100kLetters[(mgrs.zone - 1) % 2][row]; mgrs.easting = (int32_t)utm.easting % 100000;
mgrs.easting = (int32_t)utm.easting % 100000; mgrs.northing = (int32_t)utm.northing % 100000;
mgrs.northing = (int32_t)utm.northing % 100000;
} }
/** /**
* Converts lat long coordinates to Ordnance Survey Grid Reference (UK National Grid Ref). * Converts lat long coordinates to Ordnance Survey Grid Reference (UK National Grid Ref).
* Based on: https://www.movable-type.co.uk/scripts/latlong-os-gridref.html * Based on: https://www.movable-type.co.uk/scripts/latlong-os-gridref.html
*/ */
void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr) void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr) {
{ const char letter[] = "ABCDEFGHJKLMNOPQRSTUVWXYZ"; // No 'I' in OSGR
const char letter[] = "ABCDEFGHJKLMNOPQRSTUVWXYZ"; // No 'I' in OSGR double a = 6377563.396; // Airy 1830 semi-major axis
double a = 6377563.396; // Airy 1830 semi-major axis double b = 6356256.909; // Airy 1830 semi-minor axis
double b = 6356256.909; // Airy 1830 semi-minor axis double f0 = 0.9996012717; // National Grid point scale factor on the central meridian
double f0 = 0.9996012717; // National Grid point scale factor on the central meridian double phi0 = toRadians(49);
double phi0 = toRadians(49); double lambda0 = toRadians(-2);
double lambda0 = toRadians(-2); double n0 = -100000;
double n0 = -100000; double e0 = 400000;
double e0 = 400000; double e2 = 1 - (b * b) / (a * a); // eccentricity squared
double e2 = 1 - (b * b) / (a * a); // eccentricity squared double n = (a - b) / (a + b);
double n = (a - b) / (a + b);
double osgb_Latitude; double osgb_Latitude;
double osgb_Longitude; double osgb_Longitude;
convertWGS84ToOSGB36(lat, lon, osgb_Latitude, osgb_Longitude); convertWGS84ToOSGB36(lat, lon, osgb_Latitude, osgb_Longitude);
double phi = osgb_Latitude; // already in radians double phi = osgb_Latitude; // already in radians
double lambda = osgb_Longitude; // already in radians double lambda = osgb_Longitude; // already in radians
double v = a * f0 / sqrt(1 - e2 * sin(phi) * sin(phi)); double v = a * f0 / sqrt(1 - e2 * sin(phi) * sin(phi));
double rho = a * f0 * (1 - e2) / pow(1 - e2 * sin(phi) * sin(phi), 1.5); double rho = a * f0 * (1 - e2) / pow(1 - e2 * sin(phi) * sin(phi), 1.5);
double eta2 = v / rho - 1; double eta2 = v / rho - 1;
double mA = (1 + n + (5 / 4) * n * n + (5 / 4) * n * n * n) * (phi - phi0); double mA = (1 + n + (5 / 4) * n * n + (5 / 4) * n * n * n) * (phi - phi0);
double mB = (3 * n + 3 * n * n + (21 / 8) * n * n * n) * sin(phi - phi0) * cos(phi + phi0); double mB = (3 * n + 3 * n * n + (21 / 8) * n * n * n) * sin(phi - phi0) * cos(phi + phi0);
// loss of precision in mC & mD due to floating point rounding can cause inaccuracy of northing by a few meters // loss of precision in mC & mD due to floating point rounding can cause inaccuracy of northing by a few meters
double mC = (15 / 8 * n * n + 15 / 8 * n * n * n) * sin(2 * (phi - phi0)) * cos(2 * (phi + phi0)); double mC = (15 / 8 * n * n + 15 / 8 * n * n * n) * sin(2 * (phi - phi0)) * cos(2 * (phi + phi0));
double mD = (35 / 24) * n * n * n * sin(3 * (phi - phi0)) * cos(3 * (phi + phi0)); double mD = (35 / 24) * n * n * n * sin(3 * (phi - phi0)) * cos(3 * (phi + phi0));
double m = b * f0 * (mA - mB + mC - mD); double m = b * f0 * (mA - mB + mC - mD);
double cos3Phi = cos(phi) * cos(phi) * cos(phi); double cos3Phi = cos(phi) * cos(phi) * cos(phi);
double cos5Phi = cos3Phi * cos(phi) * cos(phi); double cos5Phi = cos3Phi * cos(phi) * cos(phi);
double tan2Phi = tan(phi) * tan(phi); double tan2Phi = tan(phi) * tan(phi);
double tan4Phi = tan2Phi * tan2Phi; double tan4Phi = tan2Phi * tan2Phi;
double I = m + n0; double I = m + n0;
double II = (v / 2) * sin(phi) * cos(phi); double II = (v / 2) * sin(phi) * cos(phi);
double III = (v / 24) * sin(phi) * cos3Phi * (5 - tan2Phi + 9 * eta2); double III = (v / 24) * sin(phi) * cos3Phi * (5 - tan2Phi + 9 * eta2);
double IIIA = (v / 720) * sin(phi) * cos5Phi * (61 - 58 * tan2Phi + tan4Phi); double IIIA = (v / 720) * sin(phi) * cos5Phi * (61 - 58 * tan2Phi + tan4Phi);
double IV = v * cos(phi); double IV = v * cos(phi);
double V = (v / 6) * cos3Phi * (v / rho - tan2Phi); double V = (v / 6) * cos3Phi * (v / rho - tan2Phi);
double VI = (v / 120) * cos5Phi * (5 - 18 * tan2Phi + tan4Phi + 14 * eta2 - 58 * tan2Phi * eta2); double VI = (v / 120) * cos5Phi * (5 - 18 * tan2Phi + tan4Phi + 14 * eta2 - 58 * tan2Phi * eta2);
double deltaLambda = lambda - lambda0; double deltaLambda = lambda - lambda0;
double deltaLambda2 = deltaLambda * deltaLambda; double deltaLambda2 = deltaLambda * deltaLambda;
double northing = double northing = I + II * deltaLambda2 + III * deltaLambda2 * deltaLambda2 + IIIA * deltaLambda2 * deltaLambda2 * deltaLambda2;
I + II * deltaLambda2 + III * deltaLambda2 * deltaLambda2 + IIIA * deltaLambda2 * deltaLambda2 * deltaLambda2; double easting = e0 + IV * deltaLambda + V * deltaLambda2 * deltaLambda + VI * deltaLambda2 * deltaLambda2 * deltaLambda;
double easting = e0 + IV * deltaLambda + V * deltaLambda2 * deltaLambda + VI * deltaLambda2 * deltaLambda2 * deltaLambda;
if (easting < 0 || easting > 700000 || northing < 0 || northing > 1300000) // Check if out of boundaries if (easting < 0 || easting > 700000 || northing < 0 || northing > 1300000) // Check if out of boundaries
osgr = {'I', 'I', 0, 0}; osgr = {'I', 'I', 0, 0};
else { else {
uint32_t e100k = floor(easting / 100000); uint32_t e100k = floor(easting / 100000);
uint32_t n100k = floor(northing / 100000); uint32_t n100k = floor(northing / 100000);
int8_t l1 = (19 - n100k) - (19 - n100k) % 5 + floor((e100k + 10) / 5); int8_t l1 = (19 - n100k) - (19 - n100k) % 5 + floor((e100k + 10) / 5);
int8_t l2 = (19 - n100k) * 5 % 25 + e100k % 5; int8_t l2 = (19 - n100k) * 5 % 25 + e100k % 5;
osgr.e100k = letter[l1]; osgr.e100k = letter[l1];
osgr.n100k = letter[l2]; osgr.n100k = letter[l2];
osgr.easting = floor((int)easting % 100000); osgr.easting = floor((int)easting % 100000);
osgr.northing = floor((int)northing % 100000); osgr.northing = floor((int)northing % 100000);
} }
} }
/** /**
* Converts lat long coordinates to Open Location Code. * Converts lat long coordinates to Open Location Code.
* Based on: https://github.com/google/open-location-code/blob/main/c/src/olc.c * Based on: https://github.com/google/open-location-code/blob/main/c/src/olc.c
*/ */
void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
{ char tempCode[] = "1234567890abc";
char tempCode[] = "1234567890abc"; const char kAlphabet[] = "23456789CFGHJMPQRVWX";
const char kAlphabet[] = "23456789CFGHJMPQRVWX"; double latitude;
double latitude; double longitude = lon;
double longitude = lon; double latitude_degrees = std::min(90.0, std::max(-90.0, lat));
double latitude_degrees = std::min(90.0, std::max(-90.0, lat));
if (latitude_degrees < 90) // Check latitude less than lat max if (latitude_degrees < 90) // Check latitude less than lat max
latitude = latitude_degrees; latitude = latitude_degrees;
else { else {
double precision; double precision;
if (OLC_CODE_LEN <= 10) if (OLC_CODE_LEN <= 10)
precision = pow_neg(20, floor((OLC_CODE_LEN / -2) + 2)); precision = pow_neg(20, floor((OLC_CODE_LEN / -2) + 2));
else else
precision = pow_neg(20, -3) / pow(5, OLC_CODE_LEN - 10); precision = pow_neg(20, -3) / pow(5, OLC_CODE_LEN - 10);
latitude = latitude_degrees - precision / 2; latitude = latitude_degrees - precision / 2;
}
while (longitude < -180) // Normalize longitude
longitude += 360;
while (longitude >= 180)
longitude -= 360;
int64_t lat_val = 90 * 2.5e7;
int64_t lng_val = 180 * 8.192e6;
lat_val += latitude * 2.5e7;
lng_val += longitude * 8.192e6;
size_t pos = OLC_CODE_LEN;
if (OLC_CODE_LEN > 10) { // Compute grid part of code if needed
for (size_t i = 0; i < 5; i++) {
int lat_digit = lat_val % 5;
int lng_digit = lng_val % 4;
int ndx = lat_digit * 4 + lng_digit;
tempCode[pos--] = kAlphabet[ndx];
lat_val /= 5;
lng_val /= 4;
} }
while (longitude < -180) // Normalize longitude } else {
longitude += 360; lat_val /= pow(5, 5);
while (longitude >= 180) lng_val /= pow(4, 5);
longitude -= 360; }
int64_t lat_val = 90 * 2.5e7;
int64_t lng_val = 180 * 8.192e6;
lat_val += latitude * 2.5e7;
lng_val += longitude * 8.192e6;
size_t pos = OLC_CODE_LEN;
if (OLC_CODE_LEN > 10) { // Compute grid part of code if needed pos = 10;
for (size_t i = 0; i < 5; i++) {
int lat_digit = lat_val % 5;
int lng_digit = lng_val % 4;
int ndx = lat_digit * 4 + lng_digit;
tempCode[pos--] = kAlphabet[ndx];
lat_val /= 5;
lng_val /= 4;
}
} else {
lat_val /= pow(5, 5);
lng_val /= pow(4, 5);
}
pos = 10; for (size_t i = 0; i < 5; i++) { // Compute pair section of code
int lat_ndx = lat_val % 20;
int lng_ndx = lng_val % 20;
tempCode[pos--] = kAlphabet[lng_ndx];
tempCode[pos--] = kAlphabet[lat_ndx];
lat_val /= 20;
lng_val /= 20;
for (size_t i = 0; i < 5; i++) { // Compute pair section of code if (i == 0)
int lat_ndx = lat_val % 20; tempCode[pos--] = '+';
int lng_ndx = lng_val % 20; }
tempCode[pos--] = kAlphabet[lng_ndx];
tempCode[pos--] = kAlphabet[lat_ndx];
lat_val /= 20;
lng_val /= 20;
if (i == 0) if (OLC_CODE_LEN < 9) { // Add padding if needed
tempCode[pos--] = '+'; for (size_t i = OLC_CODE_LEN; i < 9; i++)
} tempCode[i] = '0';
tempCode[9] = '+';
}
if (OLC_CODE_LEN < 9) { // Add padding if needed size_t char_count = OLC_CODE_LEN;
for (size_t i = OLC_CODE_LEN; i < 9; i++) if (10 > char_count) {
tempCode[i] = '0'; char_count = 10;
tempCode[9] = '+'; }
} for (size_t i = 0; i < char_count; i++) {
olc.code[i] = tempCode[i];
size_t char_count = OLC_CODE_LEN; }
if (10 > char_count) { olc.code[char_count] = '\0';
char_count = 10;
}
for (size_t i = 0; i < char_count; i++) {
olc.code[i] = tempCode[i];
}
olc.code[char_count] = '\0';
} }
// Converts the coordinate in WGS84 datum to the OSGB36 datum. // Converts the coordinate in WGS84 datum to the OSGB36 datum.
void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude) void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude) {
{ // Convert lat long to cartesian
// Convert lat long to cartesian double phi = toRadians(lat);
double phi = toRadians(lat); double lambda = toRadians(lon);
double lambda = toRadians(lon); double h = 0.0; // No OSTN height data used, some loss of accuracy (up to 5m)
double h = 0.0; // No OSTN height data used, some loss of accuracy (up to 5m) double wgsA = 6378137; // WGS84 datum semi major axis
double wgsA = 6378137; // WGS84 datum semi major axis double wgsF = 1 / 298.257223563; // WGS84 datum flattening
double wgsF = 1 / 298.257223563; // WGS84 datum flattening double ecc = 2 * wgsF - wgsF * wgsF;
double ecc = 2 * wgsF - wgsF * wgsF; double vee = wgsA / sqrt(1 - ecc * pow(sin(phi), 2));
double vee = wgsA / sqrt(1 - ecc * pow(sin(phi), 2)); double wgsX = (vee + h) * cos(phi) * cos(lambda);
double wgsX = (vee + h) * cos(phi) * cos(lambda); double wgsY = (vee + h) * cos(phi) * sin(lambda);
double wgsY = (vee + h) * cos(phi) * sin(lambda); double wgsZ = ((1 - ecc) * vee + h) * sin(phi);
double wgsZ = ((1 - ecc) * vee + h) * sin(phi);
// 7-parameter Helmert transform // 7-parameter Helmert transform
double tx = -446.448; // x shift in meters double tx = -446.448; // x shift in meters
double ty = 125.157; // y shift in meters double ty = 125.157; // y shift in meters
double tz = -542.060; // z shift in meters double tz = -542.060; // z shift in meters
double s = 20.4894 / 1e6 + 1; // scale normalized parts per million to (s + 1) double s = 20.4894 / 1e6 + 1; // scale normalized parts per million to (s + 1)
double rx = toRadians(-0.1502 / 3600); // x rotation normalize arcseconds to radians double rx = toRadians(-0.1502 / 3600); // x rotation normalize arcseconds to radians
double ry = toRadians(-0.2470 / 3600); // y rotation normalize arcseconds to radians double ry = toRadians(-0.2470 / 3600); // y rotation normalize arcseconds to radians
double rz = toRadians(-0.8421 / 3600); // z rotation normalize arcseconds to radians double rz = toRadians(-0.8421 / 3600); // z rotation normalize arcseconds to radians
double osgbX = tx + wgsX * s - wgsY * rz + wgsZ * ry; double osgbX = tx + wgsX * s - wgsY * rz + wgsZ * ry;
double osgbY = ty + wgsX * rz + wgsY * s - wgsZ * rx; double osgbY = ty + wgsX * rz + wgsY * s - wgsZ * rx;
double osgbZ = tz - wgsX * ry + wgsY * rx + wgsZ * s; double osgbZ = tz - wgsX * ry + wgsY * rx + wgsZ * s;
// Convert cartesian to lat long // Convert cartesian to lat long
double airyA = 6377563.396; // Airy1830 datum semi major axis double airyA = 6377563.396; // Airy1830 datum semi major axis
double airyB = 6356256.909; // Airy1830 datum semi minor axis double airyB = 6356256.909; // Airy1830 datum semi minor axis
double airyF = 1 / 299.3249646; // Airy1830 datum flattening double airyF = 1 / 299.3249646; // Airy1830 datum flattening
double airyEcc = 2 * airyF - airyF * airyF; double airyEcc = 2 * airyF - airyF * airyF;
double airyEcc2 = airyEcc / (1 - airyEcc); double airyEcc2 = airyEcc / (1 - airyEcc);
double p = sqrt(osgbX * osgbX + osgbY * osgbY); double p = sqrt(osgbX * osgbX + osgbY * osgbY);
double R = sqrt(p * p + osgbZ * osgbZ); double R = sqrt(p * p + osgbZ * osgbZ);
double tanBeta = (airyB * osgbZ) / (airyA * p) * (1 + airyEcc2 * airyB / R); double tanBeta = (airyB * osgbZ) / (airyA * p) * (1 + airyEcc2 * airyB / R);
double sinBeta = tanBeta / sqrt(1 + tanBeta * tanBeta); double sinBeta = tanBeta / sqrt(1 + tanBeta * tanBeta);
double cosBeta = sinBeta / tanBeta; double cosBeta = sinBeta / tanBeta;
osgb_Latitude = atan2(osgbZ + airyEcc2 * airyB * sinBeta * sinBeta * sinBeta, osgb_Latitude = atan2(osgbZ + airyEcc2 * airyB * sinBeta * sinBeta * sinBeta,
p - airyEcc * airyA * cosBeta * cosBeta * cosBeta); // leave in radians p - airyEcc * airyA * cosBeta * cosBeta * cosBeta); // leave in radians
osgb_Longitude = atan2(osgbY, osgbX); // leave in radians osgb_Longitude = atan2(osgbY, osgbX); // leave in radians
// osgb height = p*cos(osgb.latitude) + osgbZ*sin(osgb.latitude) - // osgb height = p*cos(osgb.latitude) + osgbZ*sin(osgb.latitude) -
//(airyA*airyA/(airyA / sqrt(1 - airyEcc*sin(osgb.latitude)*sin(osgb.latitude)))); // Not used, no OSTN data //(airyA*airyA/(airyA / sqrt(1 -
// airyEcc*sin(osgb.latitude)*sin(osgb.latitude)))); // Not used, no OSTN data
} }
/// Ported from my old java code, returns distance in meters along the globe /// Ported from my old java code, returns distance in meters along the globe
/// surface (by Haversine formula) /// surface (by Haversine formula)
float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) {
{ // Don't do math if the points are the same
// Don't do math if the points are the same if (lat_a == lat_b && lng_a == lng_b)
if (lat_a == lat_b && lng_a == lng_b) return 0.0;
return 0.0;
double a1 = lat_a / DEG_CONVERT; double a1 = lat_a / DEG_CONVERT;
double a2 = lng_a / DEG_CONVERT; double a2 = lng_a / DEG_CONVERT;
double b1 = lat_b / DEG_CONVERT; double b1 = lat_b / DEG_CONVERT;
double b2 = lng_b / DEG_CONVERT; double b2 = lng_b / DEG_CONVERT;
double cos_b1 = cos(b1); double cos_b1 = cos(b1);
double cos_a1 = cos(a1); double cos_a1 = cos(a1);
double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2); double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);
double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2); double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2);
double t3 = sin(a1) * sin(b1); double t3 = sin(a1) * sin(b1);
double tt = acos(t1 + t2 + t3); double tt = acos(t1 + t2 + t3);
if (std::isnan(tt)) if (std::isnan(tt))
tt = 0.0; // Must have been the same point? tt = 0.0; // Must have been the same point?
return (float)(6366000 * tt); return (float)(6366000 * tt);
} }
/** /**
@@ -414,14 +390,13 @@ float GeoCoord::latLongToMeter(double lat_a, double lng_a, double lat_b, double
* @return Bearing from point 1 to point 2 in radians. A value of 0 means due * @return Bearing from point 1 to point 2 in radians. A value of 0 means due
* north. * north.
*/ */
float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2) float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2) {
{ double lat1Rad = toRadians(lat1);
double lat1Rad = toRadians(lat1); double lat2Rad = toRadians(lat2);
double lat2Rad = toRadians(lat2); double deltaLonRad = toRadians(lon2 - lon1);
double deltaLonRad = toRadians(lon2 - lon1); double y = sin(deltaLonRad) * cos(lat2Rad);
double y = sin(deltaLonRad) * cos(lat2Rad); double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad)); return atan2(y, x);
return atan2(y, x);
} }
/** /**
@@ -431,11 +406,10 @@ float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2)
* The range in meters * The range in meters
* @return range in radians on a great circle * @return range in radians on a great circle
*/ */
float GeoCoord::rangeMetersToRadians(double range_meters) float GeoCoord::rangeMetersToRadians(double range_meters) {
{ // 1 nm is 1852 meters
// 1 nm is 1852 meters double distance_nm = range_meters * 1852;
double distance_nm = range_meters * 1852; return (PI / (180 * 60)) * distance_nm;
return (PI / (180 * 60)) * distance_nm;
} }
/** /**
@@ -445,25 +419,20 @@ float GeoCoord::rangeMetersToRadians(double range_meters)
* The range in radians * The range in radians
* @return Range in meters on a great circle * @return Range in meters on a great circle
*/ */
float GeoCoord::rangeRadiansToMeters(double range_radians) float GeoCoord::rangeRadiansToMeters(double range_radians) {
{ double distance_nm = ((180 * 60) / PI) * range_radians;
double distance_nm = ((180 * 60) / PI) * range_radians; // 1 meter is 0.000539957 nm
// 1 meter is 0.000539957 nm return distance_nm * 0.000539957;
return distance_nm * 0.000539957;
} }
// Find distance from point to passed in point // Find distance from point to passed in point
int32_t GeoCoord::distanceTo(const GeoCoord &pointB) int32_t GeoCoord::distanceTo(const GeoCoord &pointB) {
{ return latLongToMeter(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7);
return latLongToMeter(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7,
pointB.getLongitude() * 1e-7);
} }
// Find bearing from point to passed in point // Find bearing from point to passed in point
int32_t GeoCoord::bearingTo(const GeoCoord &pointB) int32_t GeoCoord::bearingTo(const GeoCoord &pointB) {
{ return bearing(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7, pointB.getLongitude() * 1e-7);
return bearing(this->getLatitude() * 1e-7, this->getLongitude() * 1e-7, pointB.getLatitude() * 1e-7,
pointB.getLongitude() * 1e-7);
} }
/** /**
@@ -475,16 +444,15 @@ int32_t GeoCoord::bearingTo(const GeoCoord &pointB)
* range in meters * range in meters
* @return GeoCoord object of point at bearing and range from initial point * @return GeoCoord object of point at bearing and range from initial point
*/ */
std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range_meters) std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range_meters) {
{ double range_radians = rangeMetersToRadians(range_meters);
double range_radians = rangeMetersToRadians(range_meters); double lat1 = this->getLatitude() * 1e-7;
double lat1 = this->getLatitude() * 1e-7; double lon1 = this->getLongitude() * 1e-7;
double lon1 = this->getLongitude() * 1e-7; double lat = asin(sin(lat1) * cos(range_radians) + cos(lat1) * sin(range_radians) * cos(bearing));
double lat = asin(sin(lat1) * cos(range_radians) + cos(lat1) * sin(range_radians) * cos(bearing)); double dlon = atan2(sin(bearing) * sin(range_radians) * cos(lat1), cos(range_radians) - sin(lat1) * sin(lat));
double dlon = atan2(sin(bearing) * sin(range_radians) * cos(lat1), cos(range_radians) - sin(lat1) * sin(lat)); double lon = fmod(lon1 - dlon + PI, 2 * PI) - PI;
double lon = fmod(lon1 - dlon + PI, 2 * PI) - PI;
return std::make_shared<GeoCoord>(double(lat), double(lon), this->getAltitude()); return std::make_shared<GeoCoord>(double(lat), double(lon), this->getAltitude());
} }
/** /**
@@ -493,42 +461,41 @@ std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range
* The bearing in string format * The bearing in string format
* @return Bearing in degrees * @return Bearing in degrees
*/ */
unsigned int GeoCoord::bearingToDegrees(const char *bearing) unsigned int GeoCoord::bearingToDegrees(const char *bearing) {
{ if (strcmp(bearing, "N") == 0)
if (strcmp(bearing, "N") == 0) return 0;
return 0; else if (strcmp(bearing, "NNE") == 0)
else if (strcmp(bearing, "NNE") == 0) return 22;
return 22; else if (strcmp(bearing, "NE") == 0)
else if (strcmp(bearing, "NE") == 0) return 45;
return 45; else if (strcmp(bearing, "ENE") == 0)
else if (strcmp(bearing, "ENE") == 0) return 67;
return 67; else if (strcmp(bearing, "E") == 0)
else if (strcmp(bearing, "E") == 0) return 90;
return 90; else if (strcmp(bearing, "ESE") == 0)
else if (strcmp(bearing, "ESE") == 0) return 112;
return 112; else if (strcmp(bearing, "SE") == 0)
else if (strcmp(bearing, "SE") == 0) return 135;
return 135; else if (strcmp(bearing, "SSE") == 0)
else if (strcmp(bearing, "SSE") == 0) return 157;
return 157; else if (strcmp(bearing, "S") == 0)
else if (strcmp(bearing, "S") == 0) return 180;
return 180; else if (strcmp(bearing, "SSW") == 0)
else if (strcmp(bearing, "SSW") == 0) return 202;
return 202; else if (strcmp(bearing, "SW") == 0)
else if (strcmp(bearing, "SW") == 0) return 225;
return 225; else if (strcmp(bearing, "WSW") == 0)
else if (strcmp(bearing, "WSW") == 0) return 247;
return 247; else if (strcmp(bearing, "W") == 0)
else if (strcmp(bearing, "W") == 0) return 270;
return 270; else if (strcmp(bearing, "WNW") == 0)
else if (strcmp(bearing, "WNW") == 0) return 292;
return 292; else if (strcmp(bearing, "NW") == 0)
else if (strcmp(bearing, "NW") == 0) return 315;
return 315; else if (strcmp(bearing, "NNW") == 0)
else if (strcmp(bearing, "NNW") == 0) return 337;
return 337; else
else return 0;
return 0;
} }
/** /**
@@ -537,60 +504,52 @@ unsigned int GeoCoord::bearingToDegrees(const char *bearing)
* The bearing in degrees * The bearing in degrees
* @return Bearing in string format * @return Bearing in string format
*/ */
const char *GeoCoord::degreesToBearing(unsigned int degrees) const char *GeoCoord::degreesToBearing(unsigned int degrees) {
{ if (degrees >= 348 || degrees < 11)
if (degrees >= 348 || degrees < 11) return "N";
return "N"; else if (degrees >= 11 && degrees < 34)
else if (degrees >= 11 && degrees < 34) return "NNE";
return "NNE"; else if (degrees >= 34 && degrees < 56)
else if (degrees >= 34 && degrees < 56) return "NE";
return "NE"; else if (degrees >= 56 && degrees < 79)
else if (degrees >= 56 && degrees < 79) return "ENE";
return "ENE"; else if (degrees >= 79 && degrees < 101)
else if (degrees >= 79 && degrees < 101) return "E";
return "E"; else if (degrees >= 101 && degrees < 124)
else if (degrees >= 101 && degrees < 124) return "ESE";
return "ESE"; else if (degrees >= 124 && degrees < 146)
else if (degrees >= 124 && degrees < 146) return "SE";
return "SE"; else if (degrees >= 146 && degrees < 169)
else if (degrees >= 146 && degrees < 169) return "SSE";
return "SSE"; else if (degrees >= 169 && degrees < 191)
else if (degrees >= 169 && degrees < 191) return "S";
return "S"; else if (degrees >= 191 && degrees < 214)
else if (degrees >= 191 && degrees < 214) return "SSW";
return "SSW"; else if (degrees >= 214 && degrees < 236)
else if (degrees >= 214 && degrees < 236) return "SW";
return "SW"; else if (degrees >= 236 && degrees < 259)
else if (degrees >= 236 && degrees < 259) return "WSW";
return "WSW"; else if (degrees >= 259 && degrees < 281)
else if (degrees >= 259 && degrees < 281) return "W";
return "W"; else if (degrees >= 281 && degrees < 304)
else if (degrees >= 281 && degrees < 304) return "WNW";
return "WNW"; else if (degrees >= 304 && degrees < 326)
else if (degrees >= 304 && degrees < 326) return "NW";
return "NW"; else if (degrees >= 326 && degrees < 348)
else if (degrees >= 326 && degrees < 348) return "NNW";
return "NNW"; else
else return "N";
return "N";
} }
double GeoCoord::pow_neg(double base, double exponent) double GeoCoord::pow_neg(double base, double exponent) {
{ if (exponent == 0) {
if (exponent == 0) { return 1;
return 1; } else if (exponent > 0) {
} else if (exponent > 0) { return pow(base, exponent);
return pow(base, exponent); }
} return 1 / pow(base, -exponent);
return 1 / pow(base, -exponent);
} }
double GeoCoord::toRadians(double deg) double GeoCoord::toRadians(double deg) { return deg * PI / 180; }
{
return deg * PI / 180;
}
double GeoCoord::toDegrees(double r) double GeoCoord::toDegrees(double r) { return r * 180 / PI; }
{
return r * 180 / PI;
}
src/gps/GeoCoord.h
+96 -97
View File
@@ -16,133 +16,132 @@
// GeoCoord structs/classes // GeoCoord structs/classes
// A struct to hold the data for a DMS coordinate. // A struct to hold the data for a DMS coordinate.
struct DMS { struct DMS {
uint8_t latDeg; uint8_t latDeg;
uint8_t latMin; uint8_t latMin;
uint32_t latSec; uint32_t latSec;
char latCP; char latCP;
uint8_t lonDeg; uint8_t lonDeg;
uint8_t lonMin; uint8_t lonMin;
uint32_t lonSec; uint32_t lonSec;
char lonCP; char lonCP;
}; };
// A struct to hold the data for a UTM coordinate, this is also used when creating an MGRS coordinate. // A struct to hold the data for a UTM coordinate, this is also used when creating an MGRS coordinate.
struct UTM { struct UTM {
uint8_t zone; uint8_t zone;
char band; char band;
uint32_t easting; uint32_t easting;
uint32_t northing; uint32_t northing;
}; };
// A struct to hold the data for a MGRS coordinate. // A struct to hold the data for a MGRS coordinate.
struct MGRS { struct MGRS {
uint8_t zone; uint8_t zone;
char band; char band;
char east100k; char east100k;
char north100k; char north100k;
uint32_t easting; uint32_t easting;
uint32_t northing; uint32_t northing;
}; };
// A struct to hold the data for a OSGR coordinate // A struct to hold the data for a OSGR coordinate
struct OSGR { struct OSGR {
char e100k; char e100k;
char n100k; char n100k;
uint32_t easting; uint32_t easting;
uint32_t northing; uint32_t northing;
}; };
// A struct to hold the data for a OLC coordinate // A struct to hold the data for a OLC coordinate
struct OLC { struct OLC {
char code[OLC_CODE_LEN + 1]; // +1 for null termination char code[OLC_CODE_LEN + 1]; // +1 for null termination
}; };
class GeoCoord class GeoCoord {
{ private:
private: int32_t _latitude = 0;
int32_t _latitude = 0; int32_t _longitude = 0;
int32_t _longitude = 0; int32_t _altitude = 0;
int32_t _altitude = 0;
DMS _dms = {}; DMS _dms = {};
UTM _utm = {}; UTM _utm = {};
MGRS _mgrs = {}; MGRS _mgrs = {};
OSGR _osgr = {}; OSGR _osgr = {};
OLC _olc = {}; OLC _olc = {};
bool _dirty = true; bool _dirty = true;
void setCoords(); void setCoords();
public: public:
GeoCoord(); GeoCoord();
GeoCoord(int32_t lat, int32_t lon, int32_t alt); GeoCoord(int32_t lat, int32_t lon, int32_t alt);
GeoCoord(double lat, double lon, int32_t alt); GeoCoord(double lat, double lon, int32_t alt);
GeoCoord(float lat, float lon, int32_t alt); GeoCoord(float lat, float lon, int32_t alt);
void updateCoords(const int32_t lat, const int32_t lon, const int32_t alt); void updateCoords(const int32_t lat, const int32_t lon, const int32_t alt);
void updateCoords(const double lat, const double lon, const int32_t alt); void updateCoords(const double lat, const double lon, const int32_t alt);
void updateCoords(const float lat, const float lon, const int32_t alt); void updateCoords(const float lat, const float lon, const int32_t alt);
// Conversions // Conversions
static void latLongToDMS(const double lat, const double lon, DMS &dms); static void latLongToDMS(const double lat, const double lon, DMS &dms);
static void latLongToUTM(const double lat, const double lon, UTM &utm); static void latLongToUTM(const double lat, const double lon, UTM &utm);
static void latLongToMGRS(const double lat, const double lon, MGRS &mgrs); static void latLongToMGRS(const double lat, const double lon, MGRS &mgrs);
static void latLongToOSGR(const double lat, const double lon, OSGR &osgr); static void latLongToOSGR(const double lat, const double lon, OSGR &osgr);
static void latLongToOLC(const double lat, const double lon, OLC &olc); static void latLongToOLC(const double lat, const double lon, OLC &olc);
static void convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude); static void convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude);
static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b); static float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b);
static float bearing(double lat1, double lon1, double lat2, double lon2); static float bearing(double lat1, double lon1, double lat2, double lon2);
static float rangeRadiansToMeters(double range_radians); static float rangeRadiansToMeters(double range_radians);
static float rangeMetersToRadians(double range_meters); static float rangeMetersToRadians(double range_meters);
static unsigned int bearingToDegrees(const char *bearing); static unsigned int bearingToDegrees(const char *bearing);
static const char *degreesToBearing(unsigned int degrees); static const char *degreesToBearing(unsigned int degrees);
// Raises a number to an exponent, handling negative exponents. // Raises a number to an exponent, handling negative exponents.
static double pow_neg(double base, double exponent); static double pow_neg(double base, double exponent);
static double toRadians(double deg); static double toRadians(double deg);
static double toDegrees(double r); static double toDegrees(double r);
// Point to point conversions // Point to point conversions
int32_t distanceTo(const GeoCoord &pointB); int32_t distanceTo(const GeoCoord &pointB);
int32_t bearingTo(const GeoCoord &pointB); int32_t bearingTo(const GeoCoord &pointB);
std::shared_ptr<GeoCoord> pointAtDistance(double bearing, double range); std::shared_ptr<GeoCoord> pointAtDistance(double bearing, double range);
// Lat lon alt getters // Lat lon alt getters
int32_t getLatitude() const { return _latitude; } int32_t getLatitude() const { return _latitude; }
int32_t getLongitude() const { return _longitude; } int32_t getLongitude() const { return _longitude; }
int32_t getAltitude() const { return _altitude; } int32_t getAltitude() const { return _altitude; }
// DMS getters // DMS getters
uint8_t getDMSLatDeg() const { return _dms.latDeg; } uint8_t getDMSLatDeg() const { return _dms.latDeg; }
uint8_t getDMSLatMin() const { return _dms.latMin; } uint8_t getDMSLatMin() const { return _dms.latMin; }
uint32_t getDMSLatSec() const { return _dms.latSec; } uint32_t getDMSLatSec() const { return _dms.latSec; }
char getDMSLatCP() const { return _dms.latCP; } char getDMSLatCP() const { return _dms.latCP; }
uint8_t getDMSLonDeg() const { return _dms.lonDeg; } uint8_t getDMSLonDeg() const { return _dms.lonDeg; }
uint8_t getDMSLonMin() const { return _dms.lonMin; } uint8_t getDMSLonMin() const { return _dms.lonMin; }
uint32_t getDMSLonSec() const { return _dms.lonSec; } uint32_t getDMSLonSec() const { return _dms.lonSec; }
char getDMSLonCP() const { return _dms.lonCP; } char getDMSLonCP() const { return _dms.lonCP; }
// UTM getters // UTM getters
uint8_t getUTMZone() const { return _utm.zone; } uint8_t getUTMZone() const { return _utm.zone; }
char getUTMBand() const { return _utm.band; } char getUTMBand() const { return _utm.band; }
uint32_t getUTMEasting() const { return _utm.easting; } uint32_t getUTMEasting() const { return _utm.easting; }
uint32_t getUTMNorthing() const { return _utm.northing; } uint32_t getUTMNorthing() const { return _utm.northing; }
// MGRS getters // MGRS getters
uint8_t getMGRSZone() const { return _mgrs.zone; } uint8_t getMGRSZone() const { return _mgrs.zone; }
char getMGRSBand() const { return _mgrs.band; } char getMGRSBand() const { return _mgrs.band; }
char getMGRSEast100k() const { return _mgrs.east100k; } char getMGRSEast100k() const { return _mgrs.east100k; }
char getMGRSNorth100k() const { return _mgrs.north100k; } char getMGRSNorth100k() const { return _mgrs.north100k; }
uint32_t getMGRSEasting() const { return _mgrs.easting; } uint32_t getMGRSEasting() const { return _mgrs.easting; }
uint32_t getMGRSNorthing() const { return _mgrs.northing; } uint32_t getMGRSNorthing() const { return _mgrs.northing; }
// OSGR getters // OSGR getters
char getOSGRE100k() const { return _osgr.e100k; } char getOSGRE100k() const { return _osgr.e100k; }
char getOSGRN100k() const { return _osgr.n100k; } char getOSGRN100k() const { return _osgr.n100k; }
uint32_t getOSGREasting() const { return _osgr.easting; } uint32_t getOSGREasting() const { return _osgr.easting; }
uint32_t getOSGRNorthing() const { return _osgr.northing; } uint32_t getOSGRNorthing() const { return _osgr.northing; }
// OLC getter // OLC getter
void getOLCCode(char *code) { strncpy(code, _olc.code, OLC_CODE_LEN + 1); } // +1 for null termination void getOLCCode(char *code) { strncpy(code, _olc.code, OLC_CODE_LEN + 1); } // +1 for null termination
}; };
src/gps/NMEAWPL.cpp
+48 -53
View File
@@ -19,36 +19,32 @@
* ------------------------------------------- * -------------------------------------------
*/ */
uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_PositionLite &pos, const char *name, bool isCaltopoMode) uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_PositionLite &pos, const char *name, bool isCaltopoMode) {
{ GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude); char type = isCaltopoMode ? 'P' : 'N';
char type = isCaltopoMode ? 'P' : 'N'; uint32_t len = snprintf(buf, bufsz, "\r\n$G%cWPL,%02d%07.4f,%c,%03d%07.4f,%c,%s", type, geoCoord.getDMSLatDeg(),
uint32_t len = snprintf(buf, bufsz, "\r\n$G%cWPL,%02d%07.4f,%c,%03d%07.4f,%c,%s", type, geoCoord.getDMSLatDeg(), (abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), name);
geoCoord.getDMSLonDeg(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, uint32_t chk = 0;
geoCoord.getDMSLonCP(), name); for (uint32_t i = 1; i < len; i++) {
uint32_t chk = 0; chk ^= buf[i];
for (uint32_t i = 1; i < len; i++) { }
chk ^= buf[i]; len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
} return len;
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
return len;
} }
uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_Position &pos, const char *name, bool isCaltopoMode) uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_Position &pos, const char *name, bool isCaltopoMode) {
{ GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude); char type = isCaltopoMode ? 'P' : 'N';
char type = isCaltopoMode ? 'P' : 'N'; uint32_t len = snprintf(buf, bufsz, "$G%cWPL,%02d%07.4f,%c,%03d%07.4f,%c,%s", type, geoCoord.getDMSLatDeg(),
uint32_t len = snprintf(buf, bufsz, "$G%cWPL,%02d%07.4f,%c,%03d%07.4f,%c,%s", type, geoCoord.getDMSLatDeg(), (abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), name);
geoCoord.getDMSLonDeg(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, uint32_t chk = 0;
geoCoord.getDMSLonCP(), name); for (uint32_t i = 1; i < len; i++) {
uint32_t chk = 0; chk ^= buf[i];
for (uint32_t i = 1; i < len; i++) { }
chk ^= buf[i]; len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
} return len;
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
return len;
} }
/* ------------------------------------------- /* -------------------------------------------
* 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 * 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
@@ -66,37 +62,36 @@ uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_Position &pos, const
* 8 Horizontal Dilution of precision (meters) * 8 Horizontal Dilution of precision (meters)
* 9 Antenna Altitude above/below mean-sea-level (geoid) (in meters) * 9 Antenna Altitude above/below mean-sea-level (geoid) (in meters)
* 10 Units of antenna altitude, meters * 10 Units of antenna altitude, meters
* 11 Geoidal separation, the difference between the WGS-84 earth ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level * 11 Geoidal separation, the difference between the WGS-84 earth ellipsoid and mean-sea-level (geoid), "-" means
* below ellipsoid 12 Units of geoidal separation, meters 13 Age of differential GPS data, time in seconds since last SC104 type 1 * mean-sea-level below ellipsoid 12 Units of geoidal separation, meters 13 Age of differential GPS data, time in
* or 9 update, null field when DGPS is not used 14 Differential reference station ID, 0000-1023 15 Checksum * seconds since last SC104 type 1 or 9 update, null field when DGPS is not used 14 Differential reference station ID,
* 0000-1023 15 Checksum
* ------------------------------------------- * -------------------------------------------
*/ */
uint32_t printGGA(char *buf, size_t bufsz, const meshtastic_Position &pos) uint32_t printGGA(char *buf, size_t bufsz, const meshtastic_Position &pos) {
{ GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude); time_t timestamp = pos.timestamp;
time_t timestamp = pos.timestamp;
tm *t = gmtime(&timestamp); tm *t = gmtime(&timestamp);
if (getRTCQuality() > 0) { // use the device clock if we got time from somewhere. If not, use the GPS timestamp. if (getRTCQuality() > 0) { // use the device clock if we got time from somewhere. If not, use the GPS timestamp.
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice); uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice);
timestamp = rtc_sec; timestamp = rtc_sec;
t = gmtime(&timestamp); t = gmtime(&timestamp);
} }
uint32_t len = snprintf( uint32_t len = snprintf(buf, bufsz, "$GNGGA,%02d%02d%02d.%02d,%02d%07.4f,%c,%03d%07.4f,%c,%u,%02u,%04u,%04d,%c,%04d,%c,%d,%04d", t->tm_hour,
buf, bufsz, "$GNGGA,%02d%02d%02d.%02d,%02d%07.4f,%c,%03d%07.4f,%c,%u,%02u,%04u,%04d,%c,%04d,%c,%d,%04d", t->tm_hour, t->tm_min, t->tm_sec, pos.timestamp_millis_adjust, geoCoord.getDMSLatDeg(),
t->tm_min, t->tm_sec, pos.timestamp_millis_adjust, geoCoord.getDMSLatDeg(), (abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), pos.fix_quality,
(abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), pos.fix_quality, pos.sats_in_view, pos.HDOP, geoCoord.getAltitude(), 'M', pos.altitude_geoidal_separation, 'M', 0, 0);
pos.sats_in_view, pos.HDOP, geoCoord.getAltitude(), 'M', pos.altitude_geoidal_separation, 'M', 0, 0);
uint32_t chk = 0; uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) { for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i]; chk ^= buf[i];
} }
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk); len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
return len; return len;
} }
#endif #endif
src/gps/RTC.cpp
+287 -301
View File
@@ -12,149 +12,145 @@ uint32_t lastSetFromPhoneNtpOrGps = 0;
static uint32_t lastTimeValidationWarning = 0; static uint32_t lastTimeValidationWarning = 0;
static const uint32_t TIME_VALIDATION_WARNING_INTERVAL_MS = 15000; // 15 seconds static const uint32_t TIME_VALIDATION_WARNING_INTERVAL_MS = 15000; // 15 seconds
RTCQuality getRTCQuality() RTCQuality getRTCQuality() { return currentQuality; }
{
return currentQuality;
}
// stuff that really should be in in the instance instead... // stuff that really should be in in the instance instead...
static uint32_t static uint32_t timeStartMsec; // Once we have a GPS lock, this is where we hold the initial msec clock that corresponds
timeStartMsec; // Once we have a GPS lock, this is where we hold the initial msec clock that corresponds to that time // to that time
static uint64_t zeroOffsetSecs; // GPS based time in secs since 1970 - only updated once on initial lock static uint64_t zeroOffsetSecs; // GPS based time in secs since 1970 - only updated once on initial lock
/** /**
* Reads the current date and time from the RTC module and updates the system time. * Reads the current date and time from the RTC module and updates the system time.
* @return True if the RTC was successfully read and the system time was updated, false otherwise. * @return True if the RTC was successfully read and the system time was updated, false otherwise.
*/ */
RTCSetResult readFromRTC() RTCSetResult readFromRTC() {
{ struct timeval tv; /* btw settimeofday() is helpful here too*/
struct timeval tv; /* btw settimeofday() is helpful here too*/
#ifdef RV3028_RTC #ifdef RV3028_RTC
if (rtc_found.address == RV3028_RTC) { if (rtc_found.address == RV3028_RTC) {
uint32_t now = millis(); uint32_t now = millis();
Melopero_RV3028 rtc; Melopero_RV3028 rtc;
#if WIRE_INTERFACES_COUNT == 2 #if WIRE_INTERFACES_COUNT == 2
rtc.initI2C(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire); rtc.initI2C(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else #else
rtc.initI2C(); rtc.initI2C();
#endif #endif
tm t; tm t;
t.tm_year = rtc.getYear() - 1900; t.tm_year = rtc.getYear() - 1900;
t.tm_mon = rtc.getMonth() - 1; t.tm_mon = rtc.getMonth() - 1;
t.tm_mday = rtc.getDate(); t.tm_mday = rtc.getDate();
t.tm_hour = rtc.getHour(); t.tm_hour = rtc.getHour();
t.tm_min = rtc.getMinute(); t.tm_min = rtc.getMinute();
t.tm_sec = rtc.getSecond(); t.tm_sec = rtc.getSecond();
tv.tv_sec = gm_mktime(&t); tv.tv_sec = gm_mktime(&t);
tv.tv_usec = 0; tv.tv_usec = 0;
uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
#ifdef BUILD_EPOCH #ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) { if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) { if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH); LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
} }
return RTCSetResultInvalidTime; return RTCSetResultInvalidTime;
} }
#endif #endif
LOG_DEBUG("Read RTC time from RV3028 getTime as %02d-%02d-%02d %02d:%02d:%02d (%ld)", t.tm_year + 1900, t.tm_mon + 1, LOG_DEBUG("Read RTC time from RV3028 getTime as %02d-%02d-%02d %02d:%02d:%02d (%ld)", t.tm_year + 1900, t.tm_mon + 1, t.tm_mday, t.tm_hour,
t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec, printableEpoch); t.tm_min, t.tm_sec, printableEpoch);
if (currentQuality == RTCQualityNone) { if (currentQuality == RTCQualityNone) {
timeStartMsec = now; timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec; zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice; currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
} }
return RTCSetResultSuccess;
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
#elif defined(PCF8563_RTC) || defined(PCF85063_RTC) #elif defined(PCF8563_RTC) || defined(PCF85063_RTC)
#if defined(PCF8563_RTC) #if defined(PCF8563_RTC)
if (rtc_found.address == PCF8563_RTC) { if (rtc_found.address == PCF8563_RTC) {
#elif defined(PCF85063_RTC) #elif defined(PCF85063_RTC)
if (rtc_found.address == PCF85063_RTC) { if (rtc_found.address == PCF85063_RTC) {
#endif #endif
uint32_t now = millis(); uint32_t now = millis();
SensorRtcHelper rtc; SensorRtcHelper rtc;
#if WIRE_INTERFACES_COUNT == 2 #if WIRE_INTERFACES_COUNT == 2
rtc.begin(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire); rtc.begin(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else #else
rtc.begin(Wire); rtc.begin(Wire);
#endif #endif
RTC_DateTime datetime = rtc.getDateTime(); RTC_DateTime datetime = rtc.getDateTime();
tm t = datetime.toUnixTime(); tm t = datetime.toUnixTime();
tv.tv_sec = gm_mktime(&t); tv.tv_sec = gm_mktime(&t);
tv.tv_usec = 0; tv.tv_usec = 0;
uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
#ifdef BUILD_EPOCH #ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) { if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) { if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH); LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis(); lastTimeValidationWarning = millis();
} }
return RTCSetResultInvalidTime; return RTCSetResultInvalidTime;
} }
#endif #endif
LOG_DEBUG("Read RTC time from %s getDateTime as %02d-%02d-%02d %02d:%02d:%02d (%ld)", rtc.getChipName(), t.tm_year + 1900, LOG_DEBUG("Read RTC time from %s getDateTime as %02d-%02d-%02d %02d:%02d:%02d (%ld)", rtc.getChipName(), t.tm_year + 1900, t.tm_mon + 1,
t.tm_mon + 1, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec, printableEpoch); t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec, printableEpoch);
if (currentQuality == RTCQualityNone) { if (currentQuality == RTCQualityNone) {
timeStartMsec = now; timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec; zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice; currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
} }
return RTCSetResultSuccess;
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
#elif defined(RX8130CE_RTC) #elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) { if (rtc_found.address == RX8130CE_RTC) {
uint32_t now = millis(); uint32_t now = millis();
#ifdef MUZI_BASE #ifdef MUZI_BASE
ArtronShop_RX8130CE rtc(&Wire1); ArtronShop_RX8130CE rtc(&Wire1);
#else #else
ArtronShop_RX8130CE rtc(&Wire); ArtronShop_RX8130CE rtc(&Wire);
#endif #endif
tm t; tm t;
if (rtc.getTime(&t)) { if (rtc.getTime(&t)) {
tv.tv_sec = gm_mktime(&t); tv.tv_sec = gm_mktime(&t);
tv.tv_usec = 0; tv.tv_usec = 0;
uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
LOG_DEBUG("Read RTC time from RX8130CE getDateTime as %02d-%02d-%02d %02d:%02d:%02d (%ld)", t.tm_year + 1900, LOG_DEBUG("Read RTC time from RX8130CE getDateTime as %02d-%02d-%02d %02d:%02d:%02d (%ld)", t.tm_year + 1900, t.tm_mon + 1, t.tm_mday,
t.tm_mon + 1, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec, printableEpoch); t.tm_hour, t.tm_min, t.tm_sec, printableEpoch);
#ifdef BUILD_EPOCH #ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) { if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) { if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH); LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis(); lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
}
#endif
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
} }
} return RTCSetResultInvalidTime;
#else }
if (!gettimeofday(&tv, NULL)) { #endif
uint32_t now = millis(); if (currentQuality == RTCQualityNone) {
uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
LOG_DEBUG("Read RTC time as %ld", printableEpoch);
timeStartMsec = now; timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec; zeroOffsetSecs = tv.tv_sec;
return RTCSetResultSuccess; currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
} }
}
#else
if (!gettimeofday(&tv, NULL)) {
uint32_t now = millis();
uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
LOG_DEBUG("Read RTC time as %ld", printableEpoch);
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
return RTCSetResultSuccess;
}
#endif #endif
return RTCSetResultNotSet; return RTCSetResultNotSet;
} }
/** /**
@@ -166,143 +162,140 @@ RTCSetResult readFromRTC()
* *
* If we haven't yet set our RTC this boot, set it from a GPS derived time * If we haven't yet set our RTC this boot, set it from a GPS derived time
*/ */
RTCSetResult perhapsSetRTC(RTCQuality q, const struct timeval *tv, bool forceUpdate) RTCSetResult perhapsSetRTC(RTCQuality q, const struct timeval *tv, bool forceUpdate) {
{ static uint32_t lastSetMsec = 0;
static uint32_t lastSetMsec = 0; uint32_t now = millis();
uint32_t now = millis(); uint32_t printableEpoch = tv->tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
uint32_t printableEpoch = tv->tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
#ifdef BUILD_EPOCH #ifdef BUILD_EPOCH
if (tv->tv_sec < BUILD_EPOCH) { if (tv->tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) { if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH); LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis(); lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
} else if ((uint64_t)tv->tv_sec > ((uint64_t)BUILD_EPOCH + FORTY_YEARS)) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
// Calculate max allowed time safely to avoid overflow in logging
uint64_t maxAllowedTime = (uint64_t)BUILD_EPOCH + FORTY_YEARS;
uint32_t maxAllowedPrintable = (maxAllowedTime > UINT32_MAX) ? UINT32_MAX : (uint32_t)maxAllowedTime;
LOG_WARN("Ignore time (%ld) too far in the future (build epoch: %ld, max allowed: %ld)!", printableEpoch,
(uint32_t)BUILD_EPOCH, maxAllowedPrintable);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
} }
return RTCSetResultInvalidTime;
} else if ((uint64_t)tv->tv_sec > ((uint64_t)BUILD_EPOCH + FORTY_YEARS)) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
// Calculate max allowed time safely to avoid overflow in logging
uint64_t maxAllowedTime = (uint64_t)BUILD_EPOCH + FORTY_YEARS;
uint32_t maxAllowedPrintable = (maxAllowedTime > UINT32_MAX) ? UINT32_MAX : (uint32_t)maxAllowedTime;
LOG_WARN("Ignore time (%ld) too far in the future (build epoch: %ld, max allowed: %ld)!", printableEpoch, (uint32_t)BUILD_EPOCH,
maxAllowedPrintable);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
}
#endif #endif
bool shouldSet; bool shouldSet;
if (forceUpdate) { if (forceUpdate) {
shouldSet = true; shouldSet = true;
LOG_DEBUG("Override current RTC quality (%s) with incoming time of RTC quality of %s", RtcName(currentQuality), LOG_DEBUG("Override current RTC quality (%s) with incoming time of RTC quality of %s", RtcName(currentQuality), RtcName(q));
RtcName(q)); } else if (q > currentQuality) {
} else if (q > currentQuality) { shouldSet = true;
shouldSet = true; LOG_DEBUG("Upgrade time to quality %s", RtcName(q));
LOG_DEBUG("Upgrade time to quality %s", RtcName(q)); } else if (q == RTCQualityGPS) {
} else if (q == RTCQualityGPS) { shouldSet = true;
shouldSet = true; LOG_DEBUG("Reapply GPS time: %ld secs", printableEpoch);
LOG_DEBUG("Reapply GPS time: %ld secs", printableEpoch); } else if (q == RTCQualityNTP && !Throttle::isWithinTimespanMs(lastSetMsec, (12 * 60 * 60 * 1000UL))) {
} else if (q == RTCQualityNTP && !Throttle::isWithinTimespanMs(lastSetMsec, (12 * 60 * 60 * 1000UL))) { // Every 12 hrs we will slam in a new NTP or Phone GPS / NTP time, to correct for local RTC clock drift
// Every 12 hrs we will slam in a new NTP or Phone GPS / NTP time, to correct for local RTC clock drift shouldSet = true;
shouldSet = true; LOG_DEBUG("Reapply external time to correct clock drift %ld secs", printableEpoch);
LOG_DEBUG("Reapply external time to correct clock drift %ld secs", printableEpoch); } else {
} else { shouldSet = false;
shouldSet = false; LOG_DEBUG("Current RTC quality: %s. Ignore time of RTC quality of %s", RtcName(currentQuality), RtcName(q));
LOG_DEBUG("Current RTC quality: %s. Ignore time of RTC quality of %s", RtcName(currentQuality), RtcName(q)); }
if (shouldSet) {
currentQuality = q;
lastSetMsec = now;
if (currentQuality >= RTCQualityNTP) {
lastSetFromPhoneNtpOrGps = now;
} }
if (shouldSet) { // This delta value works on all platforms
currentQuality = q; timeStartMsec = now;
lastSetMsec = now; zeroOffsetSecs = tv->tv_sec;
if (currentQuality >= RTCQualityNTP) { // If this platform has a setable RTC, set it
lastSetFromPhoneNtpOrGps = now;
}
// This delta value works on all platforms
timeStartMsec = now;
zeroOffsetSecs = tv->tv_sec;
// If this platform has a setable RTC, set it
#ifdef RV3028_RTC #ifdef RV3028_RTC
if (rtc_found.address == RV3028_RTC) { if (rtc_found.address == RV3028_RTC) {
Melopero_RV3028 rtc; Melopero_RV3028 rtc;
#if WIRE_INTERFACES_COUNT == 2 #if WIRE_INTERFACES_COUNT == 2
rtc.initI2C(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire); rtc.initI2C(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else #else
rtc.initI2C(); rtc.initI2C();
#endif #endif
tm *t = gmtime(&tv->tv_sec); tm *t = gmtime(&tv->tv_sec);
rtc.setTime(t->tm_year + 1900, t->tm_mon + 1, t->tm_wday, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec); rtc.setTime(t->tm_year + 1900, t->tm_mon + 1, t->tm_wday, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec);
LOG_DEBUG("RV3028_RTC setTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, LOG_DEBUG("RV3028_RTC setTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min,
t->tm_hour, t->tm_min, t->tm_sec, printableEpoch); t->tm_sec, printableEpoch);
} else { } else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address); LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
} }
#elif defined(PCF8563_RTC) || defined(PCF85063_RTC) #elif defined(PCF8563_RTC) || defined(PCF85063_RTC)
#if defined(PCF8563_RTC) #if defined(PCF8563_RTC)
if (rtc_found.address == PCF8563_RTC) { if (rtc_found.address == PCF8563_RTC) {
#elif defined(PCF85063_RTC) #elif defined(PCF85063_RTC)
if (rtc_found.address == PCF85063_RTC) { if (rtc_found.address == PCF85063_RTC) {
#endif #endif
SensorRtcHelper rtc; SensorRtcHelper rtc;
#if WIRE_INTERFACES_COUNT == 2 #if WIRE_INTERFACES_COUNT == 2
rtc.begin(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire); rtc.begin(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else #else
rtc.begin(Wire); rtc.begin(Wire);
#endif #endif
tm *t = gmtime(&tv->tv_sec); tm *t = gmtime(&tv->tv_sec);
rtc.setDateTime(*t); rtc.setDateTime(*t);
LOG_DEBUG("%s setDateTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", rtc.getChipName(), t->tm_year + 1900, t->tm_mon + 1, LOG_DEBUG("%s setDateTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", rtc.getChipName(), t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour,
t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec, printableEpoch); t->tm_min, t->tm_sec, printableEpoch);
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
#elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) {
#ifdef MUZI_BASE
ArtronShop_RX8130CE rtc(&Wire1);
#else
ArtronShop_RX8130CE rtc(&Wire);
#endif
tm *t = gmtime(&tv->tv_sec);
if (rtc.setTime(*t)) {
LOG_DEBUG("RX8130CE setDateTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", t->tm_year + 1900, t->tm_mon + 1,
t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec, printableEpoch);
} else {
LOG_WARN("Failed to set time for RX8130CE");
}
}
#elif defined(ARCH_ESP32)
settimeofday(tv, NULL);
#endif
// nrf52 doesn't have a readable RTC (yet - software not written)
#if HAS_RTC
readFromRTC();
#endif
return RTCSetResultSuccess;
} else { } else {
return RTCSetResultNotSet; // RTC was already set with a higher quality time LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
} }
#elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) {
#ifdef MUZI_BASE
ArtronShop_RX8130CE rtc(&Wire1);
#else
ArtronShop_RX8130CE rtc(&Wire);
#endif
tm *t = gmtime(&tv->tv_sec);
if (rtc.setTime(*t)) {
LOG_DEBUG("RX8130CE setDateTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min,
t->tm_sec, printableEpoch);
} else {
LOG_WARN("Failed to set time for RX8130CE");
}
}
#elif defined(ARCH_ESP32)
settimeofday(tv, NULL);
#endif
// nrf52 doesn't have a readable RTC (yet - software not written)
#if HAS_RTC
readFromRTC();
#endif
return RTCSetResultSuccess;
} else {
return RTCSetResultNotSet; // RTC was already set with a higher quality time
}
} }
const char *RtcName(RTCQuality quality) const char *RtcName(RTCQuality quality) {
{ switch (quality) {
switch (quality) { case RTCQualityNone:
case RTCQualityNone: return "None";
return "None"; case RTCQualityDevice:
case RTCQualityDevice: return "Device";
return "Device"; case RTCQualityFromNet:
case RTCQualityFromNet: return "Net";
return "Net"; case RTCQualityNTP:
case RTCQualityNTP: return "NTP";
return "NTP"; case RTCQualityGPS:
case RTCQualityGPS: return "GPS";
return "GPS"; default:
default: return "Unknown";
return "Unknown"; }
}
} }
/** /**
@@ -312,46 +305,45 @@ const char *RtcName(RTCQuality quality)
* @param t The time to potentially set the RTC to. * @param t The time to potentially set the RTC to.
* @return True if the RTC was set to the provided time, false otherwise. * @return True if the RTC was set to the provided time, false otherwise.
*/ */
RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t) RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t) {
{ /* Convert to unix time
/* Convert to unix time The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of seconds that have elapsed since January
The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of seconds that have elapsed since January 1, 1970 1, 1970 (midnight UTC/GMT), not counting leap seconds (in ISO 8601: 1970-01-01T00:00:00Z).
(midnight UTC/GMT), not counting leap seconds (in ISO 8601: 1970-01-01T00:00:00Z). */
*/ // horrible hack to make mktime TZ agnostic - best practise according to
// horrible hack to make mktime TZ agnostic - best practise according to // https://www.gnu.org/software/libc/manual/html_node/Broken_002ddown-Time.html
// https://www.gnu.org/software/libc/manual/html_node/Broken_002ddown-Time.html time_t res = gm_mktime(&t);
time_t res = gm_mktime(&t); struct timeval tv;
struct timeval tv; tv.tv_sec = res;
tv.tv_sec = res; tv.tv_usec = 0; // time.centisecond() * (10 / 1000);
tv.tv_usec = 0; // time.centisecond() * (10 / 1000); uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
uint32_t printableEpoch = tv.tv_sec; // Print lib only supports 32 bit but time_t can be 64 bit on some platforms
#ifdef BUILD_EPOCH #ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) { if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) { if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%lu) before build epoch (%lu)!", printableEpoch, BUILD_EPOCH); LOG_WARN("Ignore time (%lu) before build epoch (%lu)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis(); lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
} else if ((uint64_t)tv.tv_sec > ((uint64_t)BUILD_EPOCH + FORTY_YEARS)) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
// Calculate max allowed time safely to avoid overflow in logging
uint64_t maxAllowedTime = (uint64_t)BUILD_EPOCH + FORTY_YEARS;
uint32_t maxAllowedPrintable = (maxAllowedTime > UINT32_MAX) ? UINT32_MAX : (uint32_t)maxAllowedTime;
LOG_WARN("Ignore time (%lu) too far in the future (build epoch: %lu, max allowed: %lu)!", printableEpoch,
(uint32_t)BUILD_EPOCH, maxAllowedPrintable);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
} }
return RTCSetResultInvalidTime;
} else if ((uint64_t)tv.tv_sec > ((uint64_t)BUILD_EPOCH + FORTY_YEARS)) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
// Calculate max allowed time safely to avoid overflow in logging
uint64_t maxAllowedTime = (uint64_t)BUILD_EPOCH + FORTY_YEARS;
uint32_t maxAllowedPrintable = (maxAllowedTime > UINT32_MAX) ? UINT32_MAX : (uint32_t)maxAllowedTime;
LOG_WARN("Ignore time (%lu) too far in the future (build epoch: %lu, max allowed: %lu)!", printableEpoch, (uint32_t)BUILD_EPOCH,
maxAllowedPrintable);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
}
#endif #endif
// LOG_DEBUG("Got time from GPS month=%d, year=%d, unixtime=%ld", t.tm_mon, t.tm_year, tv.tv_sec); // LOG_DEBUG("Got time from GPS month=%d, year=%d, unixtime=%ld", t.tm_mon, t.tm_year, tv.tv_sec);
if (t.tm_year < 0 || t.tm_year >= 300) { if (t.tm_year < 0 || t.tm_year >= 300) {
// LOG_DEBUG("Ignore invalid GPS month=%d, year=%d, unixtime=%ld", t.tm_mon, t.tm_year, tv.tv_sec); // LOG_DEBUG("Ignore invalid GPS month=%d, year=%d, unixtime=%ld", t.tm_mon, t.tm_year, tv.tv_sec);
return RTCSetResultInvalidTime; return RTCSetResultInvalidTime;
} else { } else {
return perhapsSetRTC(q, &tv); return perhapsSetRTC(q, &tv);
} }
} }
/** /**
@@ -359,16 +351,15 @@ RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t)
* *
* @return The timezone offset in seconds. * @return The timezone offset in seconds.
*/ */
int32_t getTZOffset() int32_t getTZOffset() {
{
#if MESHTASTIC_EXCLUDE_TZ #if MESHTASTIC_EXCLUDE_TZ
return 0; return 0;
#else #else
time_t now = getTime(false); time_t now = getTime(false);
struct tm *gmt; struct tm *gmt;
gmt = gmtime(&now); gmt = gmtime(&now);
gmt->tm_isdst = -1; gmt->tm_isdst = -1;
return (int32_t)difftime(now, mktime(gmt)); return (int32_t)difftime(now, mktime(gmt));
#endif #endif
} }
@@ -377,13 +368,12 @@ int32_t getTZOffset()
* *
* @return The current time in seconds since the Unix epoch. * @return The current time in seconds since the Unix epoch.
*/ */
uint32_t getTime(bool local) uint32_t getTime(bool local) {
{ if (local) {
if (local) { return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs + getTZOffset();
return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs + getTZOffset(); } else {
} else { return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs;
return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs; }
}
} }
/** /**
@@ -392,49 +382,45 @@ uint32_t getTime(bool local)
* @param minQuality The minimum quality of the RTC time required for it to be considered valid. * @param minQuality The minimum quality of the RTC time required for it to be considered valid.
* @return The current time from the RTC if it meets the minimum quality requirement, or 0 if the time is not valid. * @return The current time from the RTC if it meets the minimum quality requirement, or 0 if the time is not valid.
*/ */
uint32_t getValidTime(RTCQuality minQuality, bool local) uint32_t getValidTime(RTCQuality minQuality, bool local) { return (currentQuality >= minQuality) ? getTime(local) : 0; }
{
return (currentQuality >= minQuality) ? getTime(local) : 0;
}
time_t gm_mktime(struct tm *tm) time_t gm_mktime(struct tm *tm) {
{
#if !MESHTASTIC_EXCLUDE_TZ #if !MESHTASTIC_EXCLUDE_TZ
time_t result = 0; time_t result = 0;
// First, get us to the start of tm->year, by calcuating the number of days since the Unix epoch. // First, get us to the start of tm->year, by calcuating the number of days since the Unix epoch.
int year = 1900 + tm->tm_year; // tm_year is years since 1900 int year = 1900 + tm->tm_year; // tm_year is years since 1900
int year_minus_one = year - 1; int year_minus_one = year - 1;
int days_before_this_year = 0; int days_before_this_year = 0;
days_before_this_year += year_minus_one * 365; days_before_this_year += year_minus_one * 365;
// leap days: every 4 years, except 100s, but including 400s. // leap days: every 4 years, except 100s, but including 400s.
days_before_this_year += year_minus_one / 4 - year_minus_one / 100 + year_minus_one / 400; days_before_this_year += year_minus_one / 4 - year_minus_one / 100 + year_minus_one / 400;
// subtract from 1970-01-01 to get days since epoch // subtract from 1970-01-01 to get days since epoch
days_before_this_year -= 719162; // (1969 * 365 + 1969 / 4 - 1969 / 100 + 1969 / 400); days_before_this_year -= 719162; // (1969 * 365 + 1969 / 4 - 1969 / 100 + 1969 / 400);
// Now, within this tm->year, compute the days *before* this tm->month starts. // Now, within this tm->year, compute the days *before* this tm->month starts.
int days_before_month[12] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}; // non-leap year int days_before_month[12] = {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334}; // non-leap year
int days_this_year_before_this_month = days_before_month[tm->tm_mon]; // tm->tm_mon is 0..11 int days_this_year_before_this_month = days_before_month[tm->tm_mon]; // tm->tm_mon is 0..11
// If this is a leap year, and we're past February, add a day: // If this is a leap year, and we're past February, add a day:
if (tm->tm_mon >= 2 && (year % 4) == 0 && ((year % 100) != 0 || (year % 400) == 0)) { if (tm->tm_mon >= 2 && (year % 4) == 0 && ((year % 100) != 0 || (year % 400) == 0)) {
days_this_year_before_this_month += 1; days_this_year_before_this_month += 1;
} }
// And within this month: // And within this month:
int days_this_month_before_today = tm->tm_mday - 1; // tm->tm_mday is 1..31 int days_this_month_before_today = tm->tm_mday - 1; // tm->tm_mday is 1..31
// Now combine them all together, and convert days to seconds: // Now combine them all together, and convert days to seconds:
result += (days_before_this_year + days_this_year_before_this_month + days_this_month_before_today); result += (days_before_this_year + days_this_year_before_this_month + days_this_month_before_today);
result *= 86400L; result *= 86400L;
// Finally, add in the hours, minutes, and seconds of today: // Finally, add in the hours, minutes, and seconds of today:
result += tm->tm_hour * 3600; result += tm->tm_hour * 3600;
result += tm->tm_min * 60; result += tm->tm_min * 60;
result += tm->tm_sec; result += tm->tm_sec;
return result; return result;
#else #else
return mktime(tm); return mktime(tm);
#endif #endif
} }
src/gps/RTC.h
+14 -14
View File
@@ -10,29 +10,29 @@
enum RTCQuality { enum RTCQuality {
/// We haven't had our RTC set yet /// We haven't had our RTC set yet
RTCQualityNone = 0, RTCQualityNone = 0,
/// We got time from an onboard peripheral after boot. /// We got time from an onboard peripheral after boot.
RTCQualityDevice = 1, RTCQualityDevice = 1,
/// Some other node gave us a time we can use /// Some other node gave us a time we can use
RTCQualityFromNet = 2, RTCQualityFromNet = 2,
/// Our time is based on NTP /// Our time is based on NTP
RTCQualityNTP = 3, RTCQualityNTP = 3,
/// Our time is based on our own GPS /// Our time is based on our own GPS
RTCQualityGPS = 4 RTCQualityGPS = 4
}; };
/// The RTC set result codes /// The RTC set result codes
/// Used to indicate the result of an attempt to set the RTC. /// Used to indicate the result of an attempt to set the RTC.
enum RTCSetResult { enum RTCSetResult {
RTCSetResultNotSet = 0, ///< RTC was set successfully RTCSetResultNotSet = 0, ///< RTC was set successfully
RTCSetResultSuccess = 1, ///< RTC was set successfully RTCSetResultSuccess = 1, ///< RTC was set successfully
RTCSetResultInvalidTime = 3, ///< The provided time was invalid (e.g., before the build epoch) RTCSetResultInvalidTime = 3, ///< The provided time was invalid (e.g., before the build epoch)
RTCSetResultError = 4 ///< An error occurred while setting the RTC RTCSetResultError = 4 ///< An error occurred while setting the RTC
}; };
RTCQuality getRTCQuality(); RTCQuality getRTCQuality();
src/gps/ubx.h
+24 -26
View File
@@ -1,13 +1,13 @@
static const char *failMessage = "Unable to %s"; static const char *failMessage = "Unable to %s";
#define SEND_UBX_PACKET(TYPE, ID, DATA, ERRMSG, TIMEOUT) \ #define SEND_UBX_PACKET(TYPE, ID, DATA, ERRMSG, TIMEOUT) \
do { \ do { \
msglen = makeUBXPacket(TYPE, ID, sizeof(DATA), DATA); \ msglen = makeUBXPacket(TYPE, ID, sizeof(DATA), DATA); \
_serial_gps->write(UBXscratch, msglen); \ _serial_gps->write(UBXscratch, msglen); \
if (getACK(TYPE, ID, TIMEOUT) != GNSS_RESPONSE_OK) { \ if (getACK(TYPE, ID, TIMEOUT) != GNSS_RESPONSE_OK) { \
LOG_WARN(failMessage, #ERRMSG); \ LOG_WARN(failMessage, #ERRMSG); \
} \ } \
} while (0) } while (0)
// Power Management // Power Management
@@ -337,8 +337,8 @@ static const uint8_t _message_SAVE_10[] = {
// As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR. // As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR.
// BBR will survive a restart, and power off for a while, but modules with small backup // BBR will survive a restart, and power off for a while, but modules with small backup
// batteries or super caps will not retain the config for a long power off time. // batteries or super caps will not retain the config for a long power off time.
// for all configurations using sleep / low power modes, V_BCKP needs to be hooked to permanent power for fast aquisition after // for all configurations using sleep / low power modes, V_BCKP needs to be hooked to permanent power for fast
// sleep // aquisition after sleep
// VALSET Commands for M10 // VALSET Commands for M10
// Please refer to the M10 Protocol Specification: // Please refer to the M10 Protocol Specification:
@@ -370,11 +370,13 @@ EXTINTACTIVITY U4 0 no ext ints
LIMITPEAKCURRENT L 1 LIMITPEAKCURRENT L 1
// Ram layer config message: // Ram layer config message:
// b5 62 06 8a 26 00 00 01 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01 10 00 d0 // b5 62 06 8a 26 00 00 01 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01
10 00 d0
// 10 01 8b de // 10 01 8b de
// BBR layer config message: // BBR layer config message:
// b5 62 06 8a 26 00 00 02 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01 10 00 d0 // b5 62 06 8a 26 00 00 02 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01
10 00 d0
// 10 01 8c 03 // 10 01 8c 03
*/ */
static const uint8_t _message_VALSET_PM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40, static const uint8_t _message_VALSET_PM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40,
@@ -396,21 +398,21 @@ CFG-ITFM replaced by 5 valset messages which can be combined into one for RAM an
b5 62 06 8a 0e 00 00 01 00 00 0d 00 41 10 01 13 00 41 10 01 63 c6 b5 62 06 8a 0e 00 00 01 00 00 0d 00 41 10 01 13 00 41 10 01 63 c6
*/ */
static const uint8_t _message_VALSET_ITFM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x41, static const uint8_t _message_VALSET_ITFM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x41, 0x10, 0x01, 0x13, 0x00, 0x41, 0x10, 0x01};
0x10, 0x01, 0x13, 0x00, 0x41, 0x10, 0x01}; static const uint8_t _message_VALSET_ITFM_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x0d, 0x00, 0x41, 0x10, 0x01, 0x13, 0x00, 0x41, 0x10, 0x01};
static const uint8_t _message_VALSET_ITFM_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x0d, 0x00, 0x41,
0x10, 0x01, 0x13, 0x00, 0x41, 0x10, 0x01};
// Turn off all NMEA messages: // Turn off all NMEA messages:
// Ram layer config message: // Ram layer config message:
// b5 62 06 8a 22 00 00 01 00 00 c0 00 91 20 00 ca 00 91 20 00 c5 00 91 20 00 ac 00 91 20 00 b1 00 91 20 00 bb 00 91 20 00 40 8f // b5 62 06 8a 22 00 00 01 00 00 c0 00 91 20 00 ca 00 91 20 00 c5 00 91 20 00 ac 00 91 20 00 b1 00 91 20 00 bb 00 91 20
// 00 40 8f
// Disable GLL, GSV, VTG messages in BBR layer // Disable GLL, GSV, VTG messages in BBR layer
// BBR layer config message: // BBR layer config message:
// b5 62 06 8a 13 00 00 02 00 00 ca 00 91 20 00 c5 00 91 20 00 b1 00 91 20 00 f8 4e // b5 62 06 8a 13 00 00 02 00 00 ca 00 91 20 00 c5 00 91 20 00 b1 00 91 20 00 f8 4e
static const uint8_t _message_VALSET_DISABLE_NMEA_RAM[] = { static const uint8_t _message_VALSET_DISABLE_NMEA_RAM[] = {
/*0x00, 0x01, 0x00, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00 */ /*0x00, 0x01, 0x00, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00
*/
0x00, 0x01, 0x00, 0x00, 0xc0, 0x00, 0x91, 0x20, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91, 0x00, 0x01, 0x00, 0x00, 0xc0, 0x00, 0x91, 0x20, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91,
0x20, 0x00, 0xac, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x00}; 0x20, 0x00, 0xac, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x00};
@@ -437,14 +439,10 @@ static const uint8_t _message_VALSET_DISABLE_NMEA_BBR[] = {0x00, 0x02, 0x00, 0x0
static const uint8_t _message_VALSET_DISABLE_TXT_INFO_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x07, 0x00, 0x92, 0x20, 0x03}; static const uint8_t _message_VALSET_DISABLE_TXT_INFO_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x07, 0x00, 0x92, 0x20, 0x03};
static const uint8_t _message_VALSET_DISABLE_TXT_INFO_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x07, 0x00, 0x92, 0x20, 0x03}; static const uint8_t _message_VALSET_DISABLE_TXT_INFO_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x07, 0x00, 0x92, 0x20, 0x03};
static const uint8_t _message_VALSET_ENABLE_NMEA_RAM[] = {0x00, 0x01, 0x00, 0x00, 0xbb, 0x00, 0x91, static const uint8_t _message_VALSET_ENABLE_NMEA_RAM[] = {0x00, 0x01, 0x00, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01};
0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01}; static const uint8_t _message_VALSET_ENABLE_NMEA_BBR[] = {0x00, 0x02, 0x00, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01};
static const uint8_t _message_VALSET_ENABLE_NMEA_BBR[] = {0x00, 0x02, 0x00, 0x00, 0xbb, 0x00, 0x91, static const uint8_t _message_VALSET_DISABLE_SBAS_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x20, 0x00, 0x31, 0x10, 0x00, 0x05, 0x00, 0x31, 0x10, 0x00};
0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01}; static const uint8_t _message_VALSET_DISABLE_SBAS_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x20, 0x00, 0x31, 0x10, 0x00, 0x05, 0x00, 0x31, 0x10, 0x00};
static const uint8_t _message_VALSET_DISABLE_SBAS_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x20, 0x00, 0x31,
0x10, 0x00, 0x05, 0x00, 0x31, 0x10, 0x00};
static const uint8_t _message_VALSET_DISABLE_SBAS_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x20, 0x00, 0x31,
0x10, 0x00, 0x05, 0x00, 0x31, 0x10, 0x00};
/* /*
Operational issues with the M10: Operational issues with the M10:
src/graphics/EInkDisplay2.cpp
+184 -193
View File
@@ -33,252 +33,243 @@
*/ */
// Constructor // Constructor
EInkDisplay::EInkDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus) EInkDisplay::EInkDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus) {
{ // Set dimensions in OLEDDisplay base class
// Set dimensions in OLEDDisplay base class this->geometry = GEOMETRY_RAWMODE;
this->geometry = GEOMETRY_RAWMODE; this->displayWidth = EINK_WIDTH;
this->displayWidth = EINK_WIDTH; this->displayHeight = EINK_HEIGHT;
this->displayHeight = EINK_HEIGHT;
// Round shortest side up to nearest byte, to prevent truncation causing an undersized buffer // Round shortest side up to nearest byte, to prevent truncation causing an undersized buffer
uint16_t shortSide = min(EINK_WIDTH, EINK_HEIGHT); uint16_t shortSide = min(EINK_WIDTH, EINK_HEIGHT);
uint16_t longSide = max(EINK_WIDTH, EINK_HEIGHT); uint16_t longSide = max(EINK_WIDTH, EINK_HEIGHT);
if (shortSide % 8 != 0) if (shortSide % 8 != 0)
shortSide = (shortSide | 7) + 1; shortSide = (shortSide | 7) + 1;
this->displayBufferSize = longSide * (shortSide / 8); this->displayBufferSize = longSide * (shortSide / 8);
} }
/** /**
* Force a display update if we haven't drawn within the specified msecLimit * Force a display update if we haven't drawn within the specified msecLimit
*/ */
bool EInkDisplay::forceDisplay(uint32_t msecLimit) bool EInkDisplay::forceDisplay(uint32_t msecLimit) {
{ // No need to grab this lock because we are on our own SPI bus
// No need to grab this lock because we are on our own SPI bus // concurrency::LockGuard g(spiLock);
// concurrency::LockGuard g(spiLock);
uint32_t now = millis(); uint32_t now = millis();
uint32_t sinceLast = now - lastDrawMsec; uint32_t sinceLast = now - lastDrawMsec;
if (adafruitDisplay && (sinceLast > msecLimit || lastDrawMsec == 0)) if (adafruitDisplay && (sinceLast > msecLimit || lastDrawMsec == 0))
lastDrawMsec = now; lastDrawMsec = now;
else else
return false; return false;
// FIXME - only draw bits have changed (use backbuf similar to the other displays) // FIXME - only draw bits have changed (use backbuf similar to the other displays)
const bool flipped = config.display.flip_screen; const bool flipped = config.display.flip_screen;
// HACK for L1 EInk // HACK for L1 EInk
#if defined(SEEED_WIO_TRACKER_L1_EINK) #if defined(SEEED_WIO_TRACKER_L1_EINK)
// For SEEED_WIO_TRACKER_L1_EINK, setRotation(3) is correct but mirrored; flip both axes // For SEEED_WIO_TRACKER_L1_EINK, setRotation(3) is correct but mirrored; flip both axes
for (uint32_t y = 0; y < displayHeight; y++) { for (uint32_t y = 0; y < displayHeight; y++) {
for (uint32_t x = 0; x < displayWidth; x++) { for (uint32_t x = 0; x < displayWidth; x++) {
auto b = buffer[x + (y / 8) * displayWidth]; auto b = buffer[x + (y / 8) * displayWidth];
auto isset = b & (1 << (y & 7)); auto isset = b & (1 << (y & 7));
adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE); adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE);
}
} }
}
#else #else
for (uint32_t y = 0; y < displayHeight; y++) { for (uint32_t y = 0; y < displayHeight; y++) {
for (uint32_t x = 0; x < displayWidth; x++) { for (uint32_t x = 0; x < displayWidth; x++) {
auto b = buffer[x + (y / 8) * displayWidth]; auto b = buffer[x + (y / 8) * displayWidth];
auto isset = b & (1 << (y & 7)); auto isset = b & (1 << (y & 7));
if (flipped) if (flipped)
adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE); adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE);
else else
adafruitDisplay->drawPixel(x, y, isset ? GxEPD_BLACK : GxEPD_WHITE); adafruitDisplay->drawPixel(x, y, isset ? GxEPD_BLACK : GxEPD_WHITE);
}
} }
}
#endif #endif
// Trigger the refresh in GxEPD2 // Trigger the refresh in GxEPD2
LOG_DEBUG("Update E-Paper"); LOG_DEBUG("Update E-Paper");
adafruitDisplay->nextPage(); adafruitDisplay->nextPage();
// End the update process // End the update process
endUpdate(); endUpdate();
LOG_DEBUG("done"); LOG_DEBUG("done");
return true; return true;
} }
// End the update process - virtual method, overriden in derived class // End the update process - virtual method, overriden in derived class
void EInkDisplay::endUpdate() void EInkDisplay::endUpdate() {
{ // Power off display hardware, then deep-sleep (Except Wireless Paper V1.1, no deep-sleep)
// Power off display hardware, then deep-sleep (Except Wireless Paper V1.1, no deep-sleep) adafruitDisplay->hibernate();
adafruitDisplay->hibernate();
} }
// Write the buffer to the display memory // Write the buffer to the display memory
void EInkDisplay::display(void) void EInkDisplay::display(void) {
{ // We don't allow regular 'dumb' display() calls to draw on eink until we've shown
// We don't allow regular 'dumb' display() calls to draw on eink until we've shown // at least one forceDisplay() keyframe. This prevents flashing when we should the critical
// at least one forceDisplay() keyframe. This prevents flashing when we should the critical // bootscreen (that we want to look nice)
// bootscreen (that we want to look nice)
if (lastDrawMsec) { if (lastDrawMsec) {
forceDisplay(slowUpdateMsec); // Show the first screen a few seconds after boot, then slower forceDisplay(slowUpdateMsec); // Show the first screen a few seconds after boot, then slower
} }
} }
// Send a command to the display (low level function) // Send a command to the display (low level function)
void EInkDisplay::sendCommand(uint8_t com) void EInkDisplay::sendCommand(uint8_t com) {
{ (void)com;
(void)com; // Drop all commands to device (we just update the buffer)
// Drop all commands to device (we just update the buffer)
} }
void EInkDisplay::setDetected(uint8_t detected) void EInkDisplay::setDetected(uint8_t detected) { (void)detected; }
{
(void)detected;
}
// Connect to the display - variant specific // Connect to the display - variant specific
bool EInkDisplay::connect() bool EInkDisplay::connect() {
{ LOG_INFO("Do EInk init");
LOG_INFO("Do EInk init");
#ifdef PIN_EINK_EN #ifdef PIN_EINK_EN
// backlight power, HIGH is backlight on, LOW is off // backlight power, HIGH is backlight on, LOW is off
pinMode(PIN_EINK_EN, OUTPUT); pinMode(PIN_EINK_EN, OUTPUT);
#ifdef ELECROW_ThinkNode_M1 #ifdef ELECROW_ThinkNode_M1
// ThinkNode M1 has a hardware dimmable backlight. Start enabled // ThinkNode M1 has a hardware dimmable backlight. Start enabled
digitalWrite(PIN_EINK_EN, HIGH); digitalWrite(PIN_EINK_EN, HIGH);
#else #else
digitalWrite(PIN_EINK_EN, LOW); digitalWrite(PIN_EINK_EN, LOW);
#endif #endif
#endif #endif
#if defined(TTGO_T_ECHO) || defined(ELECROW_ThinkNode_M1) || defined(T_ECHO_LITE) #if defined(TTGO_T_ECHO) || defined(ELECROW_ThinkNode_M1) || defined(T_ECHO_LITE)
{ {
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, SPI1); auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, SPI1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
#if defined(ELECROW_ThinkNode_M1) || defined(T_ECHO_LITE)
adafruitDisplay->setRotation(4);
#else
adafruitDisplay->setRotation(3);
#endif
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(ELECROW_ThinkNode_M5)
{
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
adafruitDisplay->setRotation(4);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(MESHLINK)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, SPI1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(RAK4630) || defined(MAKERPYTHON)
{
if (eink_found) {
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 10, false, SPI1, SPISettings(4000000, MSBFIRST, SPI_MODE0));
// RAK14000 2.13 inch b/w 250x122 does actually now support fast refresh
adafruitDisplay->setRotation(3);
// Fast refresh support for 1.54, 2.13 RAK14000 b/w , 2.9 and 4.2
// adafruitDisplay->setRotation(1);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
} else {
(void)adafruitDisplay;
}
}
#elif defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E290) || defined(TLORA_T3S3_EPAPER) || \
defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER)
{
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
// VExt already enabled in setup()
// RTC GPIO hold disabled in setup()
// Create GxEPD2 objects
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
#if defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER)
adafruitDisplay->setRotation(0);
#endif
}
#elif defined(PCA10059) || defined(ME25LS01)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI1, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(0);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(M5_COREINK) || defined(T_DECK_PRO)
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel); adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI, SPISettings(4000000, MSBFIRST, SPI_MODE0)); adafruitDisplay->init();
adafruitDisplay->setRotation(0); #if defined(ELECROW_ThinkNode_M1) || defined(T_ECHO_LITE)
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT); adafruitDisplay->setRotation(4);
#elif defined(my) || defined(ESP32_S3_PICO) #else
{ adafruitDisplay->setRotation(3);
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY); #endif
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel); adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
adafruitDisplay->init(115200, true, 40, false, SPI, SPISettings(4000000, MSBFIRST, SPI_MODE0)); }
adafruitDisplay->setRotation(1); #elif defined(ELECROW_ThinkNode_M5)
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT); {
}
#elif defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK)
{
spi1 = &SPI1;
spi1->begin();
// VExt already enabled in setup()
// RTC GPIO hold disabled in setup()
// Create GxEPD2 objects
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *spi1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_VISION_MASTER_E213)
// Detect display model, before starting SPI
EInkDetectionResult displayModel = detectEInk();
// Start HSPI // Start HSPI
hspi = new SPIClass(HSPI); hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
// Create GxEPD2 object auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
adafruitDisplay = new GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1>((uint8_t)displayModel, PIN_EINK_CS, PIN_EINK_DC,
PIN_EINK_RES, PIN_EINK_BUSY, *hspi); adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
adafruitDisplay->setRotation(4);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(MESHLINK)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, SPI1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(RAK4630) || defined(MAKERPYTHON)
{
if (eink_found) {
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 10, false, SPI1, SPISettings(4000000, MSBFIRST, SPI_MODE0));
// RAK14000 2.13 inch b/w 250x122 does actually now support fast refresh
adafruitDisplay->setRotation(3);
// Fast refresh support for 1.54, 2.13 RAK14000 b/w , 2.9 and 4.2
// adafruitDisplay->setRotation(1);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
} else {
(void)adafruitDisplay;
}
}
#elif defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E290) || defined(TLORA_T3S3_EPAPER) || \
defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER)
{
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
// VExt already enabled in setup()
// RTC GPIO hold disabled in setup()
// Create GxEPD2 objects
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
// Init GxEPD2 // Init GxEPD2
adafruitDisplay->init(); adafruitDisplay->init();
adafruitDisplay->setRotation(3); adafruitDisplay->setRotation(3);
#if defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER)
adafruitDisplay->setRotation(0);
#endif
}
#elif defined(PCA10059) || defined(ME25LS01)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI1, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(0);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(M5_COREINK) || defined(T_DECK_PRO)
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(0);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
#elif defined(my) || defined(ESP32_S3_PICO)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(1);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK)
{
spi1 = &SPI1;
spi1->begin();
// VExt already enabled in setup()
// RTC GPIO hold disabled in setup()
// Create GxEPD2 objects
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *spi1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_VISION_MASTER_E213)
// Detect display model, before starting SPI
EInkDetectionResult displayModel = detectEInk();
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
// Create GxEPD2 object
adafruitDisplay =
new GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1>((uint8_t)displayModel, PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
#endif #endif
return true; return true;
} }
#endif #endif
src/graphics/EInkDisplay2.h
+49 -50
View File
@@ -22,75 +22,74 @@
* turn radio back on - currently with both on spi bus is fucked? or are we leaving chip select asserted? * turn radio back on - currently with both on spi bus is fucked? or are we leaving chip select asserted?
* Suggestion: perhaps similar to HELTEC_WIRELESS_PAPER issue, which resolved with rtc_gpio_hold_dis() * Suggestion: perhaps similar to HELTEC_WIRELESS_PAPER issue, which resolved with rtc_gpio_hold_dis()
*/ */
class EInkDisplay : public OLEDDisplay class EInkDisplay : public OLEDDisplay {
{ /// How often should we update the display
/// How often should we update the display /// thereafter we do once per 5 minutes
/// thereafter we do once per 5 minutes uint32_t slowUpdateMsec = 5 * 60 * 1000;
uint32_t slowUpdateMsec = 5 * 60 * 1000;
public: public:
/* constructor /* constructor
FIXME - the parameters are not used, just a temporary hack to keep working like the old displays FIXME - the parameters are not used, just a temporary hack to keep working like the old displays
*/ */
EInkDisplay(uint8_t, int, int, OLEDDISPLAY_GEOMETRY, HW_I2C); EInkDisplay(uint8_t, int, int, OLEDDISPLAY_GEOMETRY, HW_I2C);
// Write the buffer to the display memory (for eink we only do this occasionally) // Write the buffer to the display memory (for eink we only do this occasionally)
virtual void display(void) override; virtual void display(void) override;
/** /**
* Force a display update if we haven't drawn within the specified msecLimit * Force a display update if we haven't drawn within the specified msecLimit
* *
* @return true if we did draw the screen * @return true if we did draw the screen
*/ */
virtual bool forceDisplay(uint32_t msecLimit = 1000); virtual bool forceDisplay(uint32_t msecLimit = 1000);
/** /**
* Run any code needed to complete an update, after the physical refresh has completed. * Run any code needed to complete an update, after the physical refresh has completed.
* Split from forceDisplay(), to enable async refresh in derived EInkDynamicDisplay class. * Split from forceDisplay(), to enable async refresh in derived EInkDynamicDisplay class.
* *
*/ */
virtual void endUpdate(); virtual void endUpdate();
/** /**
* shim to make the abstraction happy * shim to make the abstraction happy
* *
*/ */
void setDetected(uint8_t detected); void setDetected(uint8_t detected);
protected: protected:
// the header size of the buffer used, e.g. for the SPI command header // the header size of the buffer used, e.g. for the SPI command header
virtual int getBufferOffset(void) override { return 0; } virtual int getBufferOffset(void) override { return 0; }
// Send a command to the display (low level function) // Send a command to the display (low level function)
virtual void sendCommand(uint8_t com) override; virtual void sendCommand(uint8_t com) override;
// Connect to the display // Connect to the display
virtual bool connect() override; virtual bool connect() override;
#ifdef GXEPD2_DRIVER_0 #ifdef GXEPD2_DRIVER_0
// AdafruitGFX display object - wrapper for multiple drivers // AdafruitGFX display object - wrapper for multiple drivers
// Allows runtime detection of multiple displays // Allows runtime detection of multiple displays
// Avoid this situation if possible! // Avoid this situation if possible!
GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1> *adafruitDisplay = NULL; GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1> *adafruitDisplay = NULL;
#else #else
// AdafruitGFX display object (for single display model) - instantiated in connect(), variant specific // AdafruitGFX display object (for single display model) - instantiated in connect(), variant specific
GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT> *adafruitDisplay = NULL; GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT> *adafruitDisplay = NULL;
#endif #endif
// If display uses HSPI // If display uses HSPI
#if defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E213) || \ #if defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E213) || \
defined(HELTEC_VISION_MASTER_E290) || defined(TLORA_T3S3_EPAPER) || defined(CROWPANEL_ESP32S3_5_EPAPER) || \ defined(HELTEC_VISION_MASTER_E290) || defined(TLORA_T3S3_EPAPER) || defined(CROWPANEL_ESP32S3_5_EPAPER) || \
defined(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER) || defined(ELECROW_ThinkNode_M5) defined(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER) || defined(ELECROW_ThinkNode_M5)
SPIClass *hspi = NULL; SPIClass *hspi = NULL;
#endif #endif
#if defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK) #if defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK)
SPIClass *spi1 = NULL; SPIClass *spi1 = NULL;
#endif #endif
private: private:
// FIXME quick hack to limit drawing to a very slow rate // FIXME quick hack to limit drawing to a very slow rate
uint32_t lastDrawMsec = 0; uint32_t lastDrawMsec = 0;
}; };
#endif #endif
src/graphics/EInkDynamicDisplay.cpp
+346 -380
View File
@@ -6,558 +6,524 @@
// Constructor // Constructor
EInkDynamicDisplay::EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus) EInkDynamicDisplay::EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus)
: EInkDisplay(address, sda, scl, geometry, i2cBus), NotifiedWorkerThread("EInkDynamicDisplay") : EInkDisplay(address, sda, scl, geometry, i2cBus), NotifiedWorkerThread("EInkDynamicDisplay") {
{ // If tracking ghost pixels, grab memory
// If tracking ghost pixels, grab memory
#ifdef EINK_LIMIT_GHOSTING_PX #ifdef EINK_LIMIT_GHOSTING_PX
dirtyPixels = new uint8_t[EInkDisplay::displayBufferSize](); // Init with zeros dirtyPixels = new uint8_t[EInkDisplay::displayBufferSize](); // Init with zeros
#endif #endif
} }
// Destructor // Destructor
EInkDynamicDisplay::~EInkDynamicDisplay() EInkDynamicDisplay::~EInkDynamicDisplay() {
{ // If we were tracking ghost pixels, free the memory
// If we were tracking ghost pixels, free the memory
#ifdef EINK_LIMIT_GHOSTING_PX #ifdef EINK_LIMIT_GHOSTING_PX
delete[] dirtyPixels; delete[] dirtyPixels;
#endif #endif
} }
// Screen requests a BACKGROUND frame // Screen requests a BACKGROUND frame
void EInkDynamicDisplay::display() void EInkDynamicDisplay::display() {
{ addFrameFlag(BACKGROUND);
addFrameFlag(BACKGROUND); update();
update();
} }
// Screen requests a RESPONSIVE frame // Screen requests a RESPONSIVE frame
bool EInkDynamicDisplay::forceDisplay(uint32_t msecLimit) bool EInkDynamicDisplay::forceDisplay(uint32_t msecLimit) {
{ addFrameFlag(RESPONSIVE);
addFrameFlag(RESPONSIVE); return update(); // (Unutilized) Base class promises to return true if update ran
return update(); // (Unutilized) Base class promises to return true if update ran
} }
// Add flag for the next frame // Add flag for the next frame
void EInkDynamicDisplay::addFrameFlag(frameFlagTypes flag) void EInkDynamicDisplay::addFrameFlag(frameFlagTypes flag) {
{ // OR the new flag into the existing flags
// OR the new flag into the existing flags this->frameFlags = (frameFlagTypes)(this->frameFlags | flag);
this->frameFlags = (frameFlagTypes)(this->frameFlags | flag);
} }
// GxEPD2 code to set fast refresh // GxEPD2 code to set fast refresh
void EInkDynamicDisplay::configForFastRefresh() void EInkDynamicDisplay::configForFastRefresh() {
{ // Variant-specific code can go here
// Variant-specific code can go here
#if defined(PRIVATE_HW) #if defined(PRIVATE_HW)
#else #else
// Otherwise: // Otherwise:
adafruitDisplay->setPartialWindow(0, 0, adafruitDisplay->width(), adafruitDisplay->height()); adafruitDisplay->setPartialWindow(0, 0, adafruitDisplay->width(), adafruitDisplay->height());
#endif #endif
} }
// GxEPD2 code to set full refresh // GxEPD2 code to set full refresh
void EInkDynamicDisplay::configForFullRefresh() void EInkDynamicDisplay::configForFullRefresh() {
{ // Variant-specific code can go here
// Variant-specific code can go here
#if defined(PRIVATE_HW) #if defined(PRIVATE_HW)
#else #else
// Otherwise: // Otherwise:
adafruitDisplay->setFullWindow(); adafruitDisplay->setFullWindow();
#endif #endif
} }
// Run any relevant GxEPD2 code, so next update will use correct refresh type // Run any relevant GxEPD2 code, so next update will use correct refresh type
void EInkDynamicDisplay::applyRefreshMode() void EInkDynamicDisplay::applyRefreshMode() {
{ // Change from FULL to FAST
// Change from FULL to FAST if (currentConfig == FULL && refresh == FAST) {
if (currentConfig == FULL && refresh == FAST) { configForFastRefresh();
configForFastRefresh(); currentConfig = FAST;
currentConfig = FAST; }
}
// Change from FAST back to FULL // Change from FAST back to FULL
else if (currentConfig == FAST && refresh == FULL) { else if (currentConfig == FAST && refresh == FULL) {
configForFullRefresh(); configForFullRefresh();
currentConfig = FULL; currentConfig = FULL;
} }
} }
// Update fastRefreshCount // Update fastRefreshCount
void EInkDynamicDisplay::adjustRefreshCounters() void EInkDynamicDisplay::adjustRefreshCounters() {
{ if (refresh == FAST)
if (refresh == FAST) fastRefreshCount++;
fastRefreshCount++;
else if (refresh == FULL) else if (refresh == FULL)
fastRefreshCount = 0; fastRefreshCount = 0;
} }
// Trigger the display update by calling base class // Trigger the display update by calling base class
bool EInkDynamicDisplay::update() bool EInkDynamicDisplay::update() {
{ // Detemine the refresh mode to use, and start the update
// Detemine the refresh mode to use, and start the update bool refreshApproved = determineMode();
bool refreshApproved = determineMode(); if (refreshApproved) {
if (refreshApproved) { EInkDisplay::forceDisplay(0); // Bypass base class' own rate-limiting system
EInkDisplay::forceDisplay(0); // Bypass base class' own rate-limiting system storeAndReset(); // Store the result of this loop for next time. Note: call *before* endOrDetach()
storeAndReset(); // Store the result of this loop for next time. Note: call *before* endOrDetach() endOrDetach(); // endUpdate() right now, or set the async refresh flag (if FULL and HAS_EINK_ASYNCFULL)
endOrDetach(); // endUpdate() right now, or set the async refresh flag (if FULL and HAS_EINK_ASYNCFULL) } else
} else storeAndReset(); // No update, no post-update code, just store the results
storeAndReset(); // No update, no post-update code, just store the results
return refreshApproved; // (Unutilized) Base class promises to return true if update ran return refreshApproved; // (Unutilized) Base class promises to return true if update ran
} }
// Figure out who runs the post-update code // Figure out who runs the post-update code
void EInkDynamicDisplay::endOrDetach() void EInkDynamicDisplay::endOrDetach() {
{ // If the GxEPD2 version reports that it has the async modifications
// If the GxEPD2 version reports that it has the async modifications
#ifdef HAS_EINK_ASYNCFULL #ifdef HAS_EINK_ASYNCFULL
if (previousRefresh == FULL) { if (previousRefresh == FULL) {
asyncRefreshRunning = true; // Set the flag - checked in determineMode(); cleared by onNotify() asyncRefreshRunning = true; // Set the flag - checked in determineMode(); cleared by onNotify()
if (previousFrameFlags & BLOCKING) if (previousFrameFlags & BLOCKING)
awaitRefresh(); awaitRefresh();
else { else {
// Async begins // Async begins
LOG_DEBUG("Async full-refresh begins (drop frames)"); LOG_DEBUG("Async full-refresh begins (drop frames)");
notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true); // Hand-off to NotifiedWorkerThread notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true); // Hand-off to NotifiedWorkerThread
}
} }
}
// Fast Refresh // Fast Refresh
else if (previousRefresh == FAST) else if (previousRefresh == FAST)
EInkDisplay::endUpdate(); // Still block while updating, but EInkDisplay needs us to call endUpdate() ourselves. EInkDisplay::endUpdate(); // Still block while updating, but EInkDisplay needs us to call endUpdate() ourselves.
// Fallback - If using an unmodified version of GxEPD2 for some reason // Fallback - If using an unmodified version of GxEPD2 for some reason
#else #else
if (previousRefresh == FULL || previousRefresh == FAST) { // If refresh wasn't skipped (on unspecified..) if (previousRefresh == FULL || previousRefresh == FAST) { // If refresh wasn't skipped (on unspecified..)
LOG_WARN( LOG_WARN("GxEPD2 version has not been modified to support async refresh; using fallback behavior. Please update "
"GxEPD2 version has not been modified to support async refresh; using fallback behavior. Please update lib_deps in " "lib_deps in "
"variant's platformio.ini file"); "variant's platformio.ini file");
EInkDisplay::endUpdate(); EInkDisplay::endUpdate();
} }
#endif #endif
} }
// Assess situation, pick a refresh type // Assess situation, pick a refresh type
bool EInkDynamicDisplay::determineMode() bool EInkDynamicDisplay::determineMode() {
{ checkInitialized();
checkInitialized(); checkForPromotion();
checkForPromotion();
#if defined(HAS_EINK_ASYNCFULL) #if defined(HAS_EINK_ASYNCFULL)
checkBusyAsyncRefresh(); checkBusyAsyncRefresh();
#endif #endif
checkRateLimiting(); checkRateLimiting();
// If too soon for a new frame, or display busy, abort early // If too soon for a new frame, or display busy, abort early
if (refresh == SKIPPED) if (refresh == SKIPPED)
return false; // No refresh return false; // No refresh
// -- New frame is due -- // -- New frame is due --
resetRateLimiting(); // Once determineMode() ends, will have to wait again resetRateLimiting(); // Once determineMode() ends, will have to wait again
hashImage(); // Generate here, so we can still copy it to previousImageHash, even if we skip the comparison check hashImage(); // Generate here, so we can still copy it to previousImageHash, even if we skip the comparison check
LOG_DEBUG("determineMode(): "); // Begin log entry LOG_DEBUG("determineMode(): "); // Begin log entry
// Once mode determined, any remaining checks will bypass // Once mode determined, any remaining checks will bypass
checkCosmetic(); checkCosmetic();
checkDemandingFast(); checkDemandingFast();
checkFrameMatchesPrevious(); checkFrameMatchesPrevious();
checkConsecutiveFastRefreshes(); checkConsecutiveFastRefreshes();
#ifdef EINK_LIMIT_GHOSTING_PX #ifdef EINK_LIMIT_GHOSTING_PX
checkExcessiveGhosting(); checkExcessiveGhosting();
#endif #endif
checkFastRequested(); checkFastRequested();
if (refresh == UNSPECIFIED) if (refresh == UNSPECIFIED)
LOG_WARN("There was a flaw in the determineMode() logic"); LOG_WARN("There was a flaw in the determineMode() logic");
// -- Decision has been reached -- // -- Decision has been reached --
applyRefreshMode(); applyRefreshMode();
adjustRefreshCounters(); adjustRefreshCounters();
#ifdef EINK_LIMIT_GHOSTING_PX #ifdef EINK_LIMIT_GHOSTING_PX
// Full refresh clears any ghosting // Full refresh clears any ghosting
if (refresh == FULL) if (refresh == FULL)
resetGhostPixelTracking(); resetGhostPixelTracking();
#endif #endif
// Return - call a refresh or not? // Return - call a refresh or not?
if (refresh == SKIPPED) if (refresh == SKIPPED)
return false; // Don't trigger a refresh return false; // Don't trigger a refresh
else else
return true; // Do trigger a refresh return true; // Do trigger a refresh
} }
// Is this the very first frame? // Is this the very first frame?
void EInkDynamicDisplay::checkInitialized() void EInkDynamicDisplay::checkInitialized() {
{ if (!initialized) {
if (!initialized) { // Undo GxEPD2_BW::partialWindow(), if set by developer in EInkDisplay::connect()
// Undo GxEPD2_BW::partialWindow(), if set by developer in EInkDisplay::connect() configForFullRefresh();
configForFullRefresh();
// Clear any existing image, so we can draw logo with fast-refresh, but also to set GxEPD2_EPD::_initial_write // Clear any existing image, so we can draw logo with fast-refresh, but also to set GxEPD2_EPD::_initial_write
adafruitDisplay->clearScreen(); adafruitDisplay->clearScreen();
LOG_DEBUG("initialized, "); LOG_DEBUG("initialized, ");
initialized = true; initialized = true;
// Use a fast-refresh for the next frame; no skipping or else blank screen when waking from deep sleep // Use a fast-refresh for the next frame; no skipping or else blank screen when waking from deep sleep
addFrameFlag(DEMAND_FAST); addFrameFlag(DEMAND_FAST);
} }
} }
// Was a frame skipped (rate, display busy) that should have been a FAST refresh? // Was a frame skipped (rate, display busy) that should have been a FAST refresh?
void EInkDynamicDisplay::checkForPromotion() void EInkDynamicDisplay::checkForPromotion() {
{ // If a frame was skipped (rate, display busy), then promote a BACKGROUND frame
// If a frame was skipped (rate, display busy), then promote a BACKGROUND frame // Because we DID want a RESPONSIVE/COSMETIC/DEMAND_FULL frame last time, we just didn't get it
// Because we DID want a RESPONSIVE/COSMETIC/DEMAND_FULL frame last time, we just didn't get it
switch (previousReason) { switch (previousReason) {
case ASYNC_REFRESH_BLOCKED_DEMANDFAST: case ASYNC_REFRESH_BLOCKED_DEMANDFAST:
addFrameFlag(DEMAND_FAST); addFrameFlag(DEMAND_FAST);
break; break;
case ASYNC_REFRESH_BLOCKED_COSMETIC: case ASYNC_REFRESH_BLOCKED_COSMETIC:
addFrameFlag(COSMETIC); addFrameFlag(COSMETIC);
break; break;
case ASYNC_REFRESH_BLOCKED_RESPONSIVE: case ASYNC_REFRESH_BLOCKED_RESPONSIVE:
case EXCEEDED_RATELIMIT_FAST: case EXCEEDED_RATELIMIT_FAST:
addFrameFlag(RESPONSIVE); addFrameFlag(RESPONSIVE);
break; break;
default: default:
break; break;
} }
} }
// Is it too soon for another frame of this type? // Is it too soon for another frame of this type?
void EInkDynamicDisplay::checkRateLimiting() void EInkDynamicDisplay::checkRateLimiting() {
{ // Sanity check: millis() overflow - just let the update run..
// Sanity check: millis() overflow - just let the update run.. if (previousRunMs > millis())
if (previousRunMs > millis()) return;
return;
// Skip update: too soon for BACKGROUND // Skip update: too soon for BACKGROUND
if (frameFlags == BACKGROUND) { if (frameFlags == BACKGROUND) {
if (Throttle::isWithinTimespanMs(previousRunMs, 30000)) { if (Throttle::isWithinTimespanMs(previousRunMs, 30000)) {
refresh = SKIPPED; refresh = SKIPPED;
reason = EXCEEDED_RATELIMIT_FULL; reason = EXCEEDED_RATELIMIT_FULL;
return; return;
}
} }
}
// No rate-limit for these special cases // No rate-limit for these special cases
if (frameFlags & COSMETIC || frameFlags & DEMAND_FAST) if (frameFlags & COSMETIC || frameFlags & DEMAND_FAST)
return; return;
// Skip update: too soon for RESPONSIVE // Skip update: too soon for RESPONSIVE
if (frameFlags & RESPONSIVE) { if (frameFlags & RESPONSIVE) {
if (Throttle::isWithinTimespanMs(previousRunMs, 1000)) { if (Throttle::isWithinTimespanMs(previousRunMs, 1000)) {
refresh = SKIPPED; refresh = SKIPPED;
reason = EXCEEDED_RATELIMIT_FAST; reason = EXCEEDED_RATELIMIT_FAST;
LOG_DEBUG("refresh=SKIPPED, reason=EXCEEDED_RATELIMIT_FAST, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=SKIPPED, reason=EXCEEDED_RATELIMIT_FAST, frameFlags=0x%x", frameFlags);
return; return;
}
} }
}
} }
// Is this frame COSMETIC (splash screens?) // Is this frame COSMETIC (splash screens?)
void EInkDynamicDisplay::checkCosmetic() void EInkDynamicDisplay::checkCosmetic() {
{ // If a decision was already reached, don't run the check
// If a decision was already reached, don't run the check if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
// A full refresh is requested for cosmetic purposes: we have a decision // A full refresh is requested for cosmetic purposes: we have a decision
if (frameFlags & COSMETIC) { if (frameFlags & COSMETIC) {
refresh = FULL; refresh = FULL;
reason = FLAGGED_COSMETIC; reason = FLAGGED_COSMETIC;
LOG_DEBUG("refresh=FULL, reason=FLAGGED_COSMETIC, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=FULL, reason=FLAGGED_COSMETIC, frameFlags=0x%x", frameFlags);
} }
} }
// Is this a one-off special circumstance, where we REALLY want a fast refresh? // Is this a one-off special circumstance, where we REALLY want a fast refresh?
void EInkDynamicDisplay::checkDemandingFast() void EInkDynamicDisplay::checkDemandingFast() {
{ // If a decision was already reached, don't run the check
// If a decision was already reached, don't run the check if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
// A fast refresh is demanded: we have a decision // A fast refresh is demanded: we have a decision
if (frameFlags & DEMAND_FAST) { if (frameFlags & DEMAND_FAST) {
refresh = FAST; refresh = FAST;
reason = FLAGGED_DEMAND_FAST; reason = FLAGGED_DEMAND_FAST;
LOG_DEBUG("refresh=FAST, reason=FLAGGED_DEMAND_FAST, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=FAST, reason=FLAGGED_DEMAND_FAST, frameFlags=0x%x", frameFlags);
} }
} }
// Does the new frame match the currently displayed image? // Does the new frame match the currently displayed image?
void EInkDynamicDisplay::checkFrameMatchesPrevious() void EInkDynamicDisplay::checkFrameMatchesPrevious() {
{ // If a decision was already reached, don't run the check
// If a decision was already reached, don't run the check if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
// If frame is *not* a duplicate, abort the check // If frame is *not* a duplicate, abort the check
if (imageHash != previousImageHash) if (imageHash != previousImageHash)
return; return;
#if !defined(EINK_BACKGROUND_USES_FAST) #if !defined(EINK_BACKGROUND_USES_FAST)
// If BACKGROUND, and last update was FAST: redraw the same image in FULL (for display health + image quality) // If BACKGROUND, and last update was FAST: redraw the same image in FULL (for display health + image quality)
if (frameFlags == BACKGROUND && fastRefreshCount > 0) { if (frameFlags == BACKGROUND && fastRefreshCount > 0) {
refresh = FULL; refresh = FULL;
reason = REDRAW_WITH_FULL; reason = REDRAW_WITH_FULL;
LOG_DEBUG("refresh=FULL, reason=REDRAW_WITH_FULL, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=FULL, reason=REDRAW_WITH_FULL, frameFlags=0x%x", frameFlags);
return; return;
} }
#endif #endif
// Not redrawn, not COSMETIC, not DEMAND_FAST // Not redrawn, not COSMETIC, not DEMAND_FAST
refresh = SKIPPED; refresh = SKIPPED;
reason = FRAME_MATCHED_PREVIOUS; reason = FRAME_MATCHED_PREVIOUS;
LOG_DEBUG("refresh=SKIPPED, reason=FRAME_MATCHED_PREVIOUS, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=SKIPPED, reason=FRAME_MATCHED_PREVIOUS, frameFlags=0x%x", frameFlags);
} }
// Have too many fast-refreshes occured consecutively, since last full refresh? // Have too many fast-refreshes occured consecutively, since last full refresh?
void EInkDynamicDisplay::checkConsecutiveFastRefreshes() void EInkDynamicDisplay::checkConsecutiveFastRefreshes() {
{ // If a decision was already reached, don't run the check
// If a decision was already reached, don't run the check if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
// Bypass limit if UNLIMITED_FAST mode is active // Bypass limit if UNLIMITED_FAST mode is active
if (frameFlags & UNLIMITED_FAST) { if (frameFlags & UNLIMITED_FAST) {
refresh = FAST; refresh = FAST;
reason = NO_OBJECTIONS; reason = NO_OBJECTIONS;
LOG_DEBUG("refresh=FAST, reason=UNLIMITED_FAST_MODE_ACTIVE, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=FAST, reason=UNLIMITED_FAST_MODE_ACTIVE, frameFlags=0x%x", frameFlags);
return; return;
} }
// If too many FAST refreshes consecutively - force a FULL refresh // If too many FAST refreshes consecutively - force a FULL refresh
if (fastRefreshCount >= EINK_LIMIT_FASTREFRESH) { if (fastRefreshCount >= EINK_LIMIT_FASTREFRESH) {
refresh = FULL; refresh = FULL;
reason = EXCEEDED_LIMIT_FASTREFRESH; reason = EXCEEDED_LIMIT_FASTREFRESH;
LOG_DEBUG("refresh=FULL, reason=EXCEEDED_LIMIT_FASTREFRESH, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=FULL, reason=EXCEEDED_LIMIT_FASTREFRESH, frameFlags=0x%x", frameFlags);
} }
} }
// No objections, we can perform fast-refresh, if desired // No objections, we can perform fast-refresh, if desired
void EInkDynamicDisplay::checkFastRequested() void EInkDynamicDisplay::checkFastRequested() {
{ if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
if (frameFlags == BACKGROUND) { if (frameFlags == BACKGROUND) {
#ifdef EINK_BACKGROUND_USES_FAST #ifdef EINK_BACKGROUND_USES_FAST
// If we want BACKGROUND to use fast. (FULL only when a limit is hit) // If we want BACKGROUND to use fast. (FULL only when a limit is hit)
refresh = FAST; refresh = FAST;
reason = BACKGROUND_USES_FAST; reason = BACKGROUND_USES_FAST;
LOG_DEBUG("refresh=FAST, reason=BACKGROUND_USES_FAST, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, LOG_DEBUG("refresh=FAST, reason=BACKGROUND_USES_FAST, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, frameFlags);
frameFlags);
#else #else
// If we do want to use FULL for BACKGROUND updates // If we do want to use FULL for BACKGROUND updates
refresh = FULL; refresh = FULL;
reason = FLAGGED_BACKGROUND; reason = FLAGGED_BACKGROUND;
LOG_DEBUG("refresh=FULL, reason=FLAGGED_BACKGROUND"); LOG_DEBUG("refresh=FULL, reason=FLAGGED_BACKGROUND");
#endif #endif
} }
// Sanity: confirm that we did ask for a RESPONSIVE frame. // Sanity: confirm that we did ask for a RESPONSIVE frame.
if (frameFlags & RESPONSIVE) { if (frameFlags & RESPONSIVE) {
refresh = FAST; refresh = FAST;
reason = NO_OBJECTIONS; reason = NO_OBJECTIONS;
LOG_DEBUG("refresh=FAST, reason=NO_OBJECTIONS, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, frameFlags); LOG_DEBUG("refresh=FAST, reason=NO_OBJECTIONS, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, frameFlags);
} }
} }
// Reset the timer used for rate-limiting // Reset the timer used for rate-limiting
void EInkDynamicDisplay::resetRateLimiting() void EInkDynamicDisplay::resetRateLimiting() { previousRunMs = millis(); }
{
previousRunMs = millis();
}
// Generate a hash of this frame, to compare against previous update // Generate a hash of this frame, to compare against previous update
void EInkDynamicDisplay::hashImage() void EInkDynamicDisplay::hashImage() {
{ imageHash = 0;
imageHash = 0;
// Sum all bytes of the image buffer together // Sum all bytes of the image buffer together
for (uint16_t b = 0; b < (displayWidth / 8) * displayHeight; b++) { for (uint16_t b = 0; b < (displayWidth / 8) * displayHeight; b++) {
imageHash ^= buffer[b] << b; imageHash ^= buffer[b] << b;
} }
} }
// Store the results of determineMode() for future use, and reset for next call // Store the results of determineMode() for future use, and reset for next call
void EInkDynamicDisplay::storeAndReset() void EInkDynamicDisplay::storeAndReset() {
{ previousFrameFlags = frameFlags;
previousFrameFlags = frameFlags; previousRefresh = refresh;
previousRefresh = refresh; previousReason = reason;
previousReason = reason;
// Only store image hash if the display will update // Only store image hash if the display will update
if (refresh != SKIPPED) { if (refresh != SKIPPED) {
previousImageHash = imageHash; previousImageHash = imageHash;
} }
frameFlags = BACKGROUND; frameFlags = BACKGROUND;
refresh = UNSPECIFIED; refresh = UNSPECIFIED;
} }
#ifdef EINK_LIMIT_GHOSTING_PX #ifdef EINK_LIMIT_GHOSTING_PX
// Count how many ghost pixels the new image will display // Count how many ghost pixels the new image will display
void EInkDynamicDisplay::countGhostPixels() void EInkDynamicDisplay::countGhostPixels() {
{ // If a decision was already reached, don't run the check
// If a decision was already reached, don't run the check if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
// Start a new count // Start a new count
ghostPixelCount = 0; ghostPixelCount = 0;
// Check new image, bit by bit, for any white pixels at locations marked "dirty" // Check new image, bit by bit, for any white pixels at locations marked "dirty"
for (uint16_t i = 0; i < displayBufferSize; i++) { for (uint16_t i = 0; i < displayBufferSize; i++) {
for (uint8_t bit = 0; bit < 7; bit++) { for (uint8_t bit = 0; bit < 7; bit++) {
const bool dirty = (dirtyPixels[i] >> bit) & 1; // Has pixel location been drawn to since full-refresh? const bool dirty = (dirtyPixels[i] >> bit) & 1; // Has pixel location been drawn to since full-refresh?
const bool shouldBeBlank = !((buffer[i] >> bit) & 1); // Is pixel location white in the new image? const bool shouldBeBlank = !((buffer[i] >> bit) & 1); // Is pixel location white in the new image?
// If pixel is (or has been) black since last full-refresh, and now is white: ghosting // If pixel is (or has been) black since last full-refresh, and now is white: ghosting
if (dirty && shouldBeBlank) if (dirty && shouldBeBlank)
ghostPixelCount++; ghostPixelCount++;
// Update the dirty status for this pixel - will this location become a ghost if set white in future? // Update the dirty status for this pixel - will this location become a ghost if set white in future?
if (!dirty && !shouldBeBlank) if (!dirty && !shouldBeBlank)
dirtyPixels[i] |= (1 << bit); dirtyPixels[i] |= (1 << bit);
}
} }
}
LOG_DEBUG("ghostPixels=%hu, ", ghostPixelCount); LOG_DEBUG("ghostPixels=%hu, ", ghostPixelCount);
} }
// Check if ghost pixel count exceeds the defined limit // Check if ghost pixel count exceeds the defined limit
void EInkDynamicDisplay::checkExcessiveGhosting() void EInkDynamicDisplay::checkExcessiveGhosting() {
{ // If a decision was already reached, don't run the check
// If a decision was already reached, don't run the check if (refresh != UNSPECIFIED)
if (refresh != UNSPECIFIED) return;
return;
countGhostPixels(); countGhostPixels();
// If too many ghost pixels, select full refresh // If too many ghost pixels, select full refresh
if (ghostPixelCount > EINK_LIMIT_GHOSTING_PX) { if (ghostPixelCount > EINK_LIMIT_GHOSTING_PX) {
refresh = FULL; refresh = FULL;
reason = EXCEEDED_GHOSTINGLIMIT; reason = EXCEEDED_GHOSTINGLIMIT;
LOG_DEBUG("refresh=FULL, reason=EXCEEDED_GHOSTINGLIMIT, frameFlags=0x%x", frameFlags); LOG_DEBUG("refresh=FULL, reason=EXCEEDED_GHOSTINGLIMIT, frameFlags=0x%x", frameFlags);
} }
} }
// Clear the dirty pixels array. Call when full-refresh cleans the display. // Clear the dirty pixels array. Call when full-refresh cleans the display.
void EInkDynamicDisplay::resetGhostPixelTracking() void EInkDynamicDisplay::resetGhostPixelTracking() {
{ // Copy the current frame into dirtyPixels[] from the display buffer
// Copy the current frame into dirtyPixels[] from the display buffer memcpy(dirtyPixels, EInkDisplay::buffer, EInkDisplay::displayBufferSize);
memcpy(dirtyPixels, EInkDisplay::buffer, EInkDisplay::displayBufferSize);
} }
#endif // EINK_LIMIT_GHOSTING_PX #endif // EINK_LIMIT_GHOSTING_PX
// Handle any asyc tasks // Handle any asyc tasks
void EInkDynamicDisplay::onNotify(uint32_t notification) void EInkDynamicDisplay::onNotify(uint32_t notification) {
{ // Which task
// Which task switch (notification) {
switch (notification) { case DUE_POLL_ASYNCREFRESH:
case DUE_POLL_ASYNCREFRESH: pollAsyncRefresh();
pollAsyncRefresh(); break;
break; }
}
} }
#ifdef HAS_EINK_ASYNCFULL #ifdef HAS_EINK_ASYNCFULL
// Public: wait for an refresh already in progress, then run the post-update code. See Screen::setScreensaverFrames() // Public: wait for an refresh already in progress, then run the post-update code. See Screen::setScreensaverFrames()
void EInkDynamicDisplay::joinAsyncRefresh() void EInkDynamicDisplay::joinAsyncRefresh() {
{ // If no async refresh running, nothing to do
// If no async refresh running, nothing to do if (!asyncRefreshRunning)
if (!asyncRefreshRunning) return;
return;
LOG_DEBUG("Join an async refresh in progress"); LOG_DEBUG("Join an async refresh in progress");
// Continually poll the BUSY pin // Continually poll the BUSY pin
while (adafruitDisplay->epd2.isBusy()) while (adafruitDisplay->epd2.isBusy())
yield(); yield();
// If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done // If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override) EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag asyncRefreshRunning = false; // Unset the flag
LOG_DEBUG("Refresh complete"); LOG_DEBUG("Refresh complete");
// Note: this code only works because of a modification to meshtastic/GxEPD2. // Note: this code only works because of a modification to meshtastic/GxEPD2.
// It is only equipped to intercept calls to nextPage() // It is only equipped to intercept calls to nextPage()
} }
// Called from NotifiedWorkerThread. Run the post-update code if the hardware is ready // Called from NotifiedWorkerThread. Run the post-update code if the hardware is ready
void EInkDynamicDisplay::pollAsyncRefresh() void EInkDynamicDisplay::pollAsyncRefresh() {
{ // In theory, this condition should never be met
// In theory, this condition should never be met if (!asyncRefreshRunning)
if (!asyncRefreshRunning) return;
return;
// Still running, check back later // Still running, check back later
if (adafruitDisplay->epd2.isBusy()) { if (adafruitDisplay->epd2.isBusy()) {
// Schedule next call of pollAsyncRefresh() // Schedule next call of pollAsyncRefresh()
NotifiedWorkerThread::notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true); NotifiedWorkerThread::notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true);
return; return;
} }
// If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done // If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override) EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag asyncRefreshRunning = false; // Unset the flag
LOG_DEBUG("Async full-refresh complete"); LOG_DEBUG("Async full-refresh complete");
// Note: this code only works because of a modification to meshtastic/GxEPD2. // Note: this code only works because of a modification to meshtastic/GxEPD2.
// It is only equipped to intercept calls to nextPage() // It is only equipped to intercept calls to nextPage()
} }
// Check the status of "async full-refresh"; skip if running // Check the status of "async full-refresh"; skip if running
void EInkDynamicDisplay::checkBusyAsyncRefresh() void EInkDynamicDisplay::checkBusyAsyncRefresh() {
{ // No refresh taking place, continue with determineMode()
// No refresh taking place, continue with determineMode() if (!asyncRefreshRunning)
if (!asyncRefreshRunning) return;
return;
// Full refresh still running // Full refresh still running
if (adafruitDisplay->epd2.isBusy()) { if (adafruitDisplay->epd2.isBusy()) {
// No refresh // No refresh
refresh = SKIPPED; refresh = SKIPPED;
// Set the reason, marking what type of frame we're skipping // Set the reason, marking what type of frame we're skipping
if (frameFlags & DEMAND_FAST) if (frameFlags & DEMAND_FAST)
reason = ASYNC_REFRESH_BLOCKED_DEMANDFAST; reason = ASYNC_REFRESH_BLOCKED_DEMANDFAST;
else if (frameFlags & COSMETIC) else if (frameFlags & COSMETIC)
reason = ASYNC_REFRESH_BLOCKED_COSMETIC; reason = ASYNC_REFRESH_BLOCKED_COSMETIC;
else if (frameFlags & RESPONSIVE) else if (frameFlags & RESPONSIVE)
reason = ASYNC_REFRESH_BLOCKED_RESPONSIVE; reason = ASYNC_REFRESH_BLOCKED_RESPONSIVE;
else
reason = ASYNC_REFRESH_BLOCKED_BACKGROUND;
return;
}
// Async refresh appears to have stopped, but wasn't caught by onNotify()
else else
pollAsyncRefresh(); // Check (and terminate) the async refresh manually reason = ASYNC_REFRESH_BLOCKED_BACKGROUND;
return;
}
// Async refresh appears to have stopped, but wasn't caught by onNotify()
else
pollAsyncRefresh(); // Check (and terminate) the async refresh manually
} }
// Hold control while an async refresh runs // Hold control while an async refresh runs
void EInkDynamicDisplay::awaitRefresh() void EInkDynamicDisplay::awaitRefresh() {
{ // Continually poll the BUSY pin
// Continually poll the BUSY pin while (adafruitDisplay->epd2.isBusy())
while (adafruitDisplay->epd2.isBusy()) yield();
yield();
// End the full-refresh process // End the full-refresh process
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override) EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag asyncRefreshRunning = false; // Unset the flag
} }
#endif // HAS_EINK_ASYNCFULL #endif // HAS_EINK_ASYNCFULL
src/graphics/EInkDynamicDisplay.h
+105 -106
View File
@@ -15,131 +15,130 @@
(Full, Fast, Skip) (Full, Fast, Skip)
*/ */
class EInkDynamicDisplay : public EInkDisplay, protected concurrency::NotifiedWorkerThread class EInkDynamicDisplay : public EInkDisplay, protected concurrency::NotifiedWorkerThread {
{ public:
public: // Constructor
// Constructor // ( Parameters unused, passed to EInkDisplay. Maintains compatibility OLEDDisplay class )
// ( Parameters unused, passed to EInkDisplay. Maintains compatibility OLEDDisplay class ) EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus);
EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus); ~EInkDynamicDisplay();
~EInkDynamicDisplay();
// Methods to enable or disable unlimited fast refresh mode // Methods to enable or disable unlimited fast refresh mode
void enableUnlimitedFastMode() { addFrameFlag(UNLIMITED_FAST); } void enableUnlimitedFastMode() { addFrameFlag(UNLIMITED_FAST); }
void disableUnlimitedFastMode() { frameFlags = (frameFlagTypes)(frameFlags & ~UNLIMITED_FAST); } void disableUnlimitedFastMode() { frameFlags = (frameFlagTypes)(frameFlags & ~UNLIMITED_FAST); }
// What kind of frame is this // What kind of frame is this
enum frameFlagTypes : uint8_t { enum frameFlagTypes : uint8_t {
BACKGROUND = (1 << 0), // For frames via display() BACKGROUND = (1 << 0), // For frames via display()
RESPONSIVE = (1 << 1), // For frames via forceDisplay() RESPONSIVE = (1 << 1), // For frames via forceDisplay()
COSMETIC = (1 << 2), // For splashes COSMETIC = (1 << 2), // For splashes
DEMAND_FAST = (1 << 3), // Special case only DEMAND_FAST = (1 << 3), // Special case only
BLOCKING = (1 << 4), // Modifier - block while refresh runs BLOCKING = (1 << 4), // Modifier - block while refresh runs
UNLIMITED_FAST = (1 << 5) UNLIMITED_FAST = (1 << 5)
}; };
void addFrameFlag(frameFlagTypes flag); void addFrameFlag(frameFlagTypes flag);
// Set the correct frame flag, then call universal "update()" method // Set the correct frame flag, then call universal "update()" method
void display() override; void display() override;
bool forceDisplay(uint32_t msecLimit) override; // Shadows base class. Parameter and return val unused. bool forceDisplay(uint32_t msecLimit) override; // Shadows base class. Parameter and return val unused.
protected: protected:
enum refreshTypes : uint8_t { // Which refresh operation will be used enum refreshTypes : uint8_t { // Which refresh operation will be used
UNSPECIFIED, UNSPECIFIED,
FULL, FULL,
FAST, FAST,
SKIPPED, SKIPPED,
}; };
enum reasonTypes : uint8_t { // How was the decision reached enum reasonTypes : uint8_t { // How was the decision reached
NO_OBJECTIONS, NO_OBJECTIONS,
ASYNC_REFRESH_BLOCKED_DEMANDFAST, ASYNC_REFRESH_BLOCKED_DEMANDFAST,
ASYNC_REFRESH_BLOCKED_COSMETIC, ASYNC_REFRESH_BLOCKED_COSMETIC,
ASYNC_REFRESH_BLOCKED_RESPONSIVE, ASYNC_REFRESH_BLOCKED_RESPONSIVE,
ASYNC_REFRESH_BLOCKED_BACKGROUND, ASYNC_REFRESH_BLOCKED_BACKGROUND,
EXCEEDED_RATELIMIT_FAST, EXCEEDED_RATELIMIT_FAST,
EXCEEDED_RATELIMIT_FULL, EXCEEDED_RATELIMIT_FULL,
FLAGGED_COSMETIC, FLAGGED_COSMETIC,
FLAGGED_DEMAND_FAST, FLAGGED_DEMAND_FAST,
EXCEEDED_LIMIT_FASTREFRESH, EXCEEDED_LIMIT_FASTREFRESH,
EXCEEDED_GHOSTINGLIMIT, EXCEEDED_GHOSTINGLIMIT,
FRAME_MATCHED_PREVIOUS, FRAME_MATCHED_PREVIOUS,
BACKGROUND_USES_FAST, BACKGROUND_USES_FAST,
FLAGGED_BACKGROUND, FLAGGED_BACKGROUND,
REDRAW_WITH_FULL, REDRAW_WITH_FULL,
}; };
enum notificationTypes : uint8_t { // What was onNotify() called for enum notificationTypes : uint8_t { // What was onNotify() called for
NONE = 0, // This behavior (NONE=0) is fixed by NotifiedWorkerThread class NONE = 0, // This behavior (NONE=0) is fixed by NotifiedWorkerThread class
DUE_POLL_ASYNCREFRESH = 1, DUE_POLL_ASYNCREFRESH = 1,
}; };
const uint32_t intervalPollAsyncRefresh = 100; const uint32_t intervalPollAsyncRefresh = 100;
void onNotify(uint32_t notification) override; // Handle any async tasks - overrides NotifiedWorkerThread void onNotify(uint32_t notification) override; // Handle any async tasks - overrides NotifiedWorkerThread
void configForFastRefresh(); // GxEPD2 code to set fast-refresh void configForFastRefresh(); // GxEPD2 code to set fast-refresh
void configForFullRefresh(); // GxEPD2 code to set full-refresh void configForFullRefresh(); // GxEPD2 code to set full-refresh
bool determineMode(); // Assess situation, pick a refresh type bool determineMode(); // Assess situation, pick a refresh type
void applyRefreshMode(); // Run any relevant GxEPD2 code, so next update will use correct refresh type void applyRefreshMode(); // Run any relevant GxEPD2 code, so next update will use correct refresh type
void adjustRefreshCounters(); // Update fastRefreshCount void adjustRefreshCounters(); // Update fastRefreshCount
bool update(); // Trigger the display update - determine mode, then call base class bool update(); // Trigger the display update - determine mode, then call base class
void endOrDetach(); // Run the post-update code, or delegate it off to checkBusyAsyncRefresh() void endOrDetach(); // Run the post-update code, or delegate it off to checkBusyAsyncRefresh()
// Checks as part of determineMode() // Checks as part of determineMode()
void checkInitialized(); // Is this the very first frame? void checkInitialized(); // Is this the very first frame?
void checkForPromotion(); // Was a frame skipped (rate, display busy) that should have been a FAST refresh? void checkForPromotion(); // Was a frame skipped (rate, display busy) that should have been a FAST refresh?
void checkRateLimiting(); // Is this frame too soon? void checkRateLimiting(); // Is this frame too soon?
void checkCosmetic(); // Was the COSMETIC flag set? void checkCosmetic(); // Was the COSMETIC flag set?
void checkDemandingFast(); // Was the DEMAND_FAST flag set? void checkDemandingFast(); // Was the DEMAND_FAST flag set?
void checkFrameMatchesPrevious(); // Does the new frame match the existing display image? void checkFrameMatchesPrevious(); // Does the new frame match the existing display image?
void checkConsecutiveFastRefreshes(); // Too many fast-refreshes consecutively? void checkConsecutiveFastRefreshes(); // Too many fast-refreshes consecutively?
void checkFastRequested(); // Was the flag set for RESPONSIVE, or only BACKGROUND? void checkFastRequested(); // Was the flag set for RESPONSIVE, or only BACKGROUND?
void resetRateLimiting(); // Set previousRunMs - this now counts as an update, for rate-limiting void resetRateLimiting(); // Set previousRunMs - this now counts as an update, for rate-limiting
void hashImage(); // Generate a hashed version of this frame, to compare against previous update void hashImage(); // Generate a hashed version of this frame, to compare against previous update
void storeAndReset(); // Keep results of determineMode() for later, tidy-up for next call void storeAndReset(); // Keep results of determineMode() for later, tidy-up for next call
// What we are determining for this frame // What we are determining for this frame
frameFlagTypes frameFlags = BACKGROUND; // Frame characteristics - determineMode() input frameFlagTypes frameFlags = BACKGROUND; // Frame characteristics - determineMode() input
refreshTypes refresh = UNSPECIFIED; // Refresh type - determineMode() output refreshTypes refresh = UNSPECIFIED; // Refresh type - determineMode() output
reasonTypes reason = NO_OBJECTIONS; // Reason - why was refresh type used reasonTypes reason = NO_OBJECTIONS; // Reason - why was refresh type used
// What happened last time determineMode() ran // What happened last time determineMode() ran
frameFlagTypes previousFrameFlags = BACKGROUND; // (Previous) Frame flags frameFlagTypes previousFrameFlags = BACKGROUND; // (Previous) Frame flags
refreshTypes previousRefresh = UNSPECIFIED; // (Previous) Outcome refreshTypes previousRefresh = UNSPECIFIED; // (Previous) Outcome
reasonTypes previousReason = NO_OBJECTIONS; // (Previous) Reason reasonTypes previousReason = NO_OBJECTIONS; // (Previous) Reason
bool initialized = false; // Have we drawn at least one frame yet? bool initialized = false; // Have we drawn at least one frame yet?
uint32_t previousRunMs = -1; // When did determineMode() last run (rather than rejecting for rate-limiting) uint32_t previousRunMs = -1; // When did determineMode() last run (rather than rejecting for rate-limiting)
uint32_t imageHash = 0; // Hash of the current frame. Don't bother updating if nothing has changed! uint32_t imageHash = 0; // Hash of the current frame. Don't bother updating if nothing has changed!
uint32_t previousImageHash = 0; // Hash of the previous update's frame uint32_t previousImageHash = 0; // Hash of the previous update's frame
uint32_t fastRefreshCount = 0; // How many fast-refreshes consecutively since last full refresh? uint32_t fastRefreshCount = 0; // How many fast-refreshes consecutively since last full refresh?
refreshTypes currentConfig = FULL; // Which refresh type is GxEPD2 currently configured for refreshTypes currentConfig = FULL; // Which refresh type is GxEPD2 currently configured for
// Optional - track ghosting, pixel by pixel // Optional - track ghosting, pixel by pixel
// May 2024: no longer used by any display. Kept for possible future use. // May 2024: no longer used by any display. Kept for possible future use.
#ifdef EINK_LIMIT_GHOSTING_PX #ifdef EINK_LIMIT_GHOSTING_PX
void countGhostPixels(); // Count any pixels which have moved from black to white since last full-refresh void countGhostPixels(); // Count any pixels which have moved from black to white since last full-refresh
void checkExcessiveGhosting(); // Check if ghosting exceeds defined limit void checkExcessiveGhosting(); // Check if ghosting exceeds defined limit
void resetGhostPixelTracking(); // Clear the dirty pixels array. Call when full-refresh cleans the display. void resetGhostPixelTracking(); // Clear the dirty pixels array. Call when full-refresh cleans the display.
uint8_t *dirtyPixels; // Any pixels that have been black since last full-refresh (dynamically allocated mem) uint8_t *dirtyPixels; // Any pixels that have been black since last full-refresh (dynamically allocated mem)
uint32_t ghostPixelCount = 0; // Number of pixels with problematic ghosting. Retained here for LOG_DEBUG use uint32_t ghostPixelCount = 0; // Number of pixels with problematic ghosting. Retained here for LOG_DEBUG use
#endif #endif
// Conditional - async full refresh - only with modified meshtastic/GxEPD2 // Conditional - async full refresh - only with modified meshtastic/GxEPD2
#if defined(HAS_EINK_ASYNCFULL) #if defined(HAS_EINK_ASYNCFULL)
public: public:
void joinAsyncRefresh(); // Main thread joins an async refresh already in progress. Blocks, then runs post-update code void joinAsyncRefresh(); // Main thread joins an async refresh already in progress. Blocks, then runs post-update code
protected: protected:
void pollAsyncRefresh(); // Run the post-update code if the hardware is ready void pollAsyncRefresh(); // Run the post-update code if the hardware is ready
void checkBusyAsyncRefresh(); // Check if display is busy running an async full-refresh (rejecting new frames) void checkBusyAsyncRefresh(); // Check if display is busy running an async full-refresh (rejecting new frames)
void awaitRefresh(); // Hold control while an async refresh runs void awaitRefresh(); // Hold control while an async refresh runs
void endUpdate() override {} // Disable base-class behavior of running post-update immediately after forceDisplay() void endUpdate() override {} // Disable base-class behavior of running post-update immediately after forceDisplay()
bool asyncRefreshRunning = false; // Flag, checked by checkBusyAsyncRefresh() bool asyncRefreshRunning = false; // Flag, checked by checkBusyAsyncRefresh()
#else #else
public: public:
void joinAsyncRefresh() {} // Dummy method void joinAsyncRefresh() {} // Dummy method
protected: protected:
void pollAsyncRefresh() {} // Dummy method. In theory, not reachable void pollAsyncRefresh() {} // Dummy method. In theory, not reachable
#endif #endif
}; };
src/graphics/GxEPD2Multi.h
+110 -125
View File
@@ -4,132 +4,117 @@
// Workaround for issue of GxEPD2_BW objects not having a shared base class // Workaround for issue of GxEPD2_BW objects not having a shared base class
// Only exposes methods which we are actually using // Only exposes methods which we are actually using
template <typename Driver0, typename Driver1> class GxEPD2_Multi template <typename Driver0, typename Driver1> class GxEPD2_Multi {
{ public:
void drawPixel(int16_t x, int16_t y, uint16_t color) {
if (which == 0)
driver0->drawPixel(x, y, color);
else
driver1->drawPixel(x, y, color);
}
bool nextPage() {
if (which == 0)
return driver0->nextPage();
else
return driver1->nextPage();
}
void hibernate() {
if (which == 0)
driver0->hibernate();
else
driver1->hibernate();
}
void init(uint32_t serial_diag_bitrate = 0) {
if (which == 0)
driver0->init(serial_diag_bitrate);
else
driver1->init(serial_diag_bitrate);
}
void init(uint32_t serial_diag_bitrate, bool initial, uint16_t reset_duration = 20, bool pulldown_rst_mode = false) {
if (which == 0)
driver0->init(serial_diag_bitrate, initial, reset_duration, pulldown_rst_mode);
else
driver1->init(serial_diag_bitrate, initial, reset_duration, pulldown_rst_mode);
}
void setRotation(uint8_t x) {
if (which == 0)
driver0->setRotation(x);
else
driver1->setRotation(x);
}
void setPartialWindow(uint16_t x, uint16_t y, uint16_t w, uint16_t h) {
if (which == 0)
driver0->setPartialWindow(x, y, w, h);
else
driver1->setPartialWindow(x, y, w, h);
}
void setFullWindow() {
if (which == 0)
driver0->setFullWindow();
else
driver1->setFullWindow();
}
int16_t width() {
if (which == 0)
return driver0->width();
else
return driver1->width();
}
int16_t height() {
if (which == 0)
return driver0->height();
else
return driver1->height();
}
void clearScreen(uint8_t value = 0xFF) {
if (which == 0)
driver0->clearScreen();
else
driver1->clearScreen();
}
void endAsyncFull() {
if (which == 0)
driver0->endAsyncFull();
else
driver1->endAsyncFull();
}
// Exposes methods of the GxEPD2_EPD object which is usually available as GxEPD2_BW::epd
class Epd2Wrapper {
public: public:
void drawPixel(int16_t x, int16_t y, uint16_t color) bool isBusy() { return m_epd2->isBusy(); }
{ GxEPD2_EPD *m_epd2;
if (which == 0) } epd2;
driver0->drawPixel(x, y, color);
else // Constructor
driver1->drawPixel(x, y, color); // Select driver by passing whichDriver as 0 or 1
GxEPD2_Multi(uint8_t whichDriver, int16_t cs, int16_t dc, int16_t rst, int16_t busy, SPIClass &spi) {
assert(whichDriver == 0 || whichDriver == 1);
which = whichDriver;
LOG_DEBUG("GxEPD2_Multi driver: %d", which);
if (which == 0) {
driver0 = new GxEPD2_BW<Driver0, Driver0::HEIGHT>(Driver0(cs, dc, rst, busy, spi));
epd2.m_epd2 = &(driver0->epd2);
} else if (which == 1) {
driver1 = new GxEPD2_BW<Driver1, Driver1::HEIGHT>(Driver1(cs, dc, rst, busy, spi));
epd2.m_epd2 = &(driver1->epd2);
} }
}
bool nextPage() private:
{ uint8_t which;
if (which == 0) GxEPD2_BW<Driver0, Driver0::HEIGHT> *driver0;
return driver0->nextPage(); GxEPD2_BW<Driver1, Driver1::HEIGHT> *driver1;
else
return driver1->nextPage();
}
void hibernate()
{
if (which == 0)
driver0->hibernate();
else
driver1->hibernate();
}
void init(uint32_t serial_diag_bitrate = 0)
{
if (which == 0)
driver0->init(serial_diag_bitrate);
else
driver1->init(serial_diag_bitrate);
}
void init(uint32_t serial_diag_bitrate, bool initial, uint16_t reset_duration = 20, bool pulldown_rst_mode = false)
{
if (which == 0)
driver0->init(serial_diag_bitrate, initial, reset_duration, pulldown_rst_mode);
else
driver1->init(serial_diag_bitrate, initial, reset_duration, pulldown_rst_mode);
}
void setRotation(uint8_t x)
{
if (which == 0)
driver0->setRotation(x);
else
driver1->setRotation(x);
}
void setPartialWindow(uint16_t x, uint16_t y, uint16_t w, uint16_t h)
{
if (which == 0)
driver0->setPartialWindow(x, y, w, h);
else
driver1->setPartialWindow(x, y, w, h);
}
void setFullWindow()
{
if (which == 0)
driver0->setFullWindow();
else
driver1->setFullWindow();
}
int16_t width()
{
if (which == 0)
return driver0->width();
else
return driver1->width();
}
int16_t height()
{
if (which == 0)
return driver0->height();
else
return driver1->height();
}
void clearScreen(uint8_t value = 0xFF)
{
if (which == 0)
driver0->clearScreen();
else
driver1->clearScreen();
}
void endAsyncFull()
{
if (which == 0)
driver0->endAsyncFull();
else
driver1->endAsyncFull();
}
// Exposes methods of the GxEPD2_EPD object which is usually available as GxEPD2_BW::epd
class Epd2Wrapper
{
public:
bool isBusy() { return m_epd2->isBusy(); }
GxEPD2_EPD *m_epd2;
} epd2;
// Constructor
// Select driver by passing whichDriver as 0 or 1
GxEPD2_Multi(uint8_t whichDriver, int16_t cs, int16_t dc, int16_t rst, int16_t busy, SPIClass &spi)
{
assert(whichDriver == 0 || whichDriver == 1);
which = whichDriver;
LOG_DEBUG("GxEPD2_Multi driver: %d", which);
if (which == 0) {
driver0 = new GxEPD2_BW<Driver0, Driver0::HEIGHT>(Driver0(cs, dc, rst, busy, spi));
epd2.m_epd2 = &(driver0->epd2);
} else if (which == 1) {
driver1 = new GxEPD2_BW<Driver1, Driver1::HEIGHT>(Driver1(cs, dc, rst, busy, spi));
epd2.m_epd2 = &(driver1->epd2);
}
}
private:
uint8_t which;
GxEPD2_BW<Driver0, Driver0::HEIGHT> *driver0;
GxEPD2_BW<Driver1, Driver1::HEIGHT> *driver1;
}; };
src/graphics/Panel_sdl.cpp
+521 -567
View File
File diff suppressed because it is too large Load Diff
src/graphics/Panel_sdl.hpp
+92 -95
View File
@@ -36,129 +36,126 @@ Porting for SDL:
#include "lgfx/v1/panel/Panel_FrameBufferBase.hpp" #include "lgfx/v1/panel/Panel_FrameBufferBase.hpp"
#include <cstdint> #include <cstdint>
namespace lgfx namespace lgfx {
{ inline namespace v1 {
inline namespace v1
{
struct Panel_sdl; struct Panel_sdl;
struct monitor_t { struct monitor_t {
SDL_Window *window = nullptr; SDL_Window *window = nullptr;
SDL_Renderer *renderer = nullptr; SDL_Renderer *renderer = nullptr;
SDL_Texture *texture = nullptr; SDL_Texture *texture = nullptr;
SDL_Texture *texture_frameimage = nullptr; SDL_Texture *texture_frameimage = nullptr;
Panel_sdl *panel = nullptr; Panel_sdl *panel = nullptr;
// 外枠 // 外枠
const void *frame_image = 0; const void *frame_image = 0;
uint_fast16_t frame_width = 0; uint_fast16_t frame_width = 0;
uint_fast16_t frame_height = 0; uint_fast16_t frame_height = 0;
uint_fast16_t frame_inner_x = 0; uint_fast16_t frame_inner_x = 0;
uint_fast16_t frame_inner_y = 0; uint_fast16_t frame_inner_y = 0;
int_fast16_t frame_rotation = 0; int_fast16_t frame_rotation = 0;
int_fast16_t frame_angle = 0; int_fast16_t frame_angle = 0;
float scaling_x = 1; float scaling_x = 1;
float scaling_y = 1; float scaling_y = 1;
int_fast16_t touch_x, touch_y; int_fast16_t touch_x, touch_y;
bool touched = false; bool touched = false;
bool closing = false; bool closing = false;
}; };
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
struct Touch_sdl : public ITouch { struct Touch_sdl : public ITouch {
bool init(void) override { return true; } bool init(void) override { return true; }
void wakeup(void) override {} void wakeup(void) override {}
void sleep(void) override {} void sleep(void) override {}
bool isEnable(void) override { return true; }; bool isEnable(void) override { return true; };
uint_fast8_t getTouchRaw(touch_point_t *tp, uint_fast8_t count) override { return 0; } uint_fast8_t getTouchRaw(touch_point_t *tp, uint_fast8_t count) override { return 0; }
}; };
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
struct Panel_sdl : public Panel_FrameBufferBase { struct Panel_sdl : public Panel_FrameBufferBase {
static constexpr size_t EMULATED_GPIO_MAX = 128; static constexpr size_t EMULATED_GPIO_MAX = 128;
static volatile uint8_t _gpio_dummy_values[EMULATED_GPIO_MAX]; static volatile uint8_t _gpio_dummy_values[EMULATED_GPIO_MAX];
public: public:
Panel_sdl(void); Panel_sdl(void);
virtual ~Panel_sdl(void); virtual ~Panel_sdl(void);
bool init(bool use_reset) override; bool init(bool use_reset) override;
color_depth_t setColorDepth(color_depth_t depth) override; color_depth_t setColorDepth(color_depth_t depth) override;
void display(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h) override; void display(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h) override;
// void setInvert(bool invert) override {} // void setInvert(bool invert) override {}
void drawPixelPreclipped(uint_fast16_t x, uint_fast16_t y, uint32_t rawcolor) override; void drawPixelPreclipped(uint_fast16_t x, uint_fast16_t y, uint32_t rawcolor) override;
void writeFillRectPreclipped(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, uint32_t rawcolor) override; void writeFillRectPreclipped(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, uint32_t rawcolor) override;
void writeBlock(uint32_t rawcolor, uint32_t length) override; void writeBlock(uint32_t rawcolor, uint32_t length) override;
void writeImage(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, pixelcopy_t *param, void writeImage(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, pixelcopy_t *param, bool use_dma) override;
bool use_dma) override; void writeImageARGB(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, pixelcopy_t *param) override;
void writeImageARGB(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, pixelcopy_t *param) override; void writePixels(pixelcopy_t *param, uint32_t len, bool use_dma) override;
void writePixels(pixelcopy_t *param, uint32_t len, bool use_dma) override;
uint_fast8_t getTouchRaw(touch_point_t *tp, uint_fast8_t count) override; uint_fast8_t getTouchRaw(touch_point_t *tp, uint_fast8_t count) override;
void setWindowTitle(const char *title); void setWindowTitle(const char *title);
void setScaling(uint_fast8_t scaling_x, uint_fast8_t scaling_y); void setScaling(uint_fast8_t scaling_x, uint_fast8_t scaling_y);
void setFrameImage(const void *frame_image, int frame_width, int frame_height, int inner_x, int inner_y); void setFrameImage(const void *frame_image, int frame_width, int frame_height, int inner_x, int inner_y);
void setFrameRotation(uint_fast16_t frame_rotaion); void setFrameRotation(uint_fast16_t frame_rotaion);
void setBrightness(uint8_t brightness) override{}; void setBrightness(uint8_t brightness) override{};
static volatile void gpio_hi(uint32_t pin) { _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)] = 1; } static volatile void gpio_hi(uint32_t pin) { _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)] = 1; }
static volatile void gpio_lo(uint32_t pin) { _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)] = 0; } static volatile void gpio_lo(uint32_t pin) { _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)] = 0; }
static volatile bool gpio_in(uint32_t pin) { return _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)]; } static volatile bool gpio_in(uint32_t pin) { return _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)]; }
static int setup(void); static int setup(void);
static int loop(void); static int loop(void);
static int close(void); static int close(void);
static int main(int (*fn)(bool *), uint32_t msec_step_exec = 512); static int main(int (*fn)(bool *), uint32_t msec_step_exec = 512);
static void setShortcutKeymod(SDL_Keymod keymod) { _keymod = keymod; } static void setShortcutKeymod(SDL_Keymod keymod) { _keymod = keymod; }
struct KeyCodeMapping_t { struct KeyCodeMapping_t {
SDL_KeyCode keycode = SDLK_UNKNOWN; SDL_KeyCode keycode = SDLK_UNKNOWN;
uint8_t gpio = 0; uint8_t gpio = 0;
}; };
static void addKeyCodeMapping(SDL_KeyCode keyCode, uint8_t gpio); static void addKeyCodeMapping(SDL_KeyCode keyCode, uint8_t gpio);
static int getKeyCodeMapping(SDL_KeyCode keyCode); static int getKeyCodeMapping(SDL_KeyCode keyCode);
protected:
const char *_window_title = "LGFX Simulator";
SDL_mutex *_sdl_mutex = nullptr;
void sdl_create(monitor_t *m);
void sdl_update(void);
touch_point_t _touch_point;
monitor_t monitor;
rgb888_t *_texturebuf = nullptr;
uint_fast16_t _modified_counter;
uint_fast16_t _texupdate_counter;
uint_fast16_t _display_counter;
bool _invalidated;
static void _event_proc(void);
static void _update_proc(void);
static void _update_scaling(monitor_t *m, float sx, float sy);
void sdl_invalidate(void) { _invalidated = true; }
void render_texture(SDL_Texture *texture, int tx, int ty, int tw, int th, float angle);
bool initFrameBuffer(size_t width, size_t height);
void deinitFrameBuffer(void);
static SDL_Keymod _keymod;
struct lock_t {
lock_t(Panel_sdl *parent);
~lock_t();
protected: protected:
const char *_window_title = "LGFX Simulator"; Panel_sdl *_parent;
SDL_mutex *_sdl_mutex = nullptr; };
void sdl_create(monitor_t *m);
void sdl_update(void);
touch_point_t _touch_point;
monitor_t monitor;
rgb888_t *_texturebuf = nullptr;
uint_fast16_t _modified_counter;
uint_fast16_t _texupdate_counter;
uint_fast16_t _display_counter;
bool _invalidated;
static void _event_proc(void);
static void _update_proc(void);
static void _update_scaling(monitor_t *m, float sx, float sy);
void sdl_invalidate(void) { _invalidated = true; }
void render_texture(SDL_Texture *texture, int tx, int ty, int tw, int th, float angle);
bool initFrameBuffer(size_t width, size_t height);
void deinitFrameBuffer(void);
static SDL_Keymod _keymod;
struct lock_t {
lock_t(Panel_sdl *parent);
~lock_t();
protected:
Panel_sdl *_parent;
};
}; };
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
} // namespace v1 } // namespace v1
src/graphics/PointStruct.h
+2 -2
View File
@@ -1,4 +1,4 @@
struct PointStruct { struct PointStruct {
int x; int x;
int y; int y;
}; };
src/graphics/Screen.cpp
+1211 -1272
View File
File diff suppressed because it is too large Load Diff
src/graphics/Screen.h
+513 -535
View File
File diff suppressed because it is too large Load Diff
src/graphics/ScreenFonts.h
+3 -3
View File
@@ -72,9 +72,9 @@
#endif #endif
#endif #endif
#if (defined(USE_EINK) || defined(ILI9341_DRIVER) || defined(ILI9342_DRIVER) || defined(ST7701_CS) || defined(ST7735_CS) || \ #if (defined(USE_EINK) || defined(ILI9341_DRIVER) || defined(ILI9342_DRIVER) || defined(ST7701_CS) || defined(ST7735_CS) || defined(ST7789_CS) || \
defined(ST7789_CS) || defined(USE_ST7789) || defined(HX8357_CS) || defined(ILI9488_CS) || defined(ST7796_CS) || \ defined(USE_ST7789) || defined(HX8357_CS) || defined(ILI9488_CS) || defined(ST7796_CS) || defined(HACKADAY_COMMUNICATOR) || \
defined(HACKADAY_COMMUNICATOR) || defined(USE_ST7796)) && \ defined(USE_ST7796)) && \
!defined(DISPLAY_FORCE_SMALL_FONTS) !defined(DISPLAY_FORCE_SMALL_FONTS)
// The screen is bigger so use bigger fonts // The screen is bigger so use bigger fonts
#define FONT_SMALL FONT_MEDIUM_LOCAL // Height: 19 #define FONT_SMALL FONT_MEDIUM_LOCAL // Height: 19
src/graphics/SharedUIDisplay.cpp
+416 -428
View File
@@ -13,46 +13,43 @@
#include <OLEDDisplay.h> #include <OLEDDisplay.h>
#include <graphics/images.h> #include <graphics/images.h>
namespace graphics namespace graphics {
{
ScreenResolution determineScreenResolution(int16_t screenheight, int16_t screenwidth) ScreenResolution determineScreenResolution(int16_t screenheight, int16_t screenwidth) {
{
#ifdef FORCE_LOW_RES #ifdef FORCE_LOW_RES
return ScreenResolution::Low; return ScreenResolution::Low;
#else #else
// Unit C6L and other ultra low res screens // Unit C6L and other ultra low res screens
if (screenwidth <= 64 || screenheight <= 48) { if (screenwidth <= 64 || screenheight <= 48) {
return ScreenResolution::UltraLow; return ScreenResolution::UltraLow;
} }
// Standard OLED screens // Standard OLED screens
if (screenwidth > 128 && screenheight <= 64) { if (screenwidth > 128 && screenheight <= 64) {
return ScreenResolution::Low;
}
// High Resolutions screens like T114, TDeck, TLora Pager, etc
if (screenwidth > 128) {
return ScreenResolution::High;
}
// Default to low resolution
return ScreenResolution::Low; return ScreenResolution::Low;
}
// High Resolutions screens like T114, TDeck, TLora Pager, etc
if (screenwidth > 128) {
return ScreenResolution::High;
}
// Default to low resolution
return ScreenResolution::Low;
#endif #endif
} }
void decomposeTime(uint32_t rtc_sec, int &hour, int &minute, int &second) void decomposeTime(uint32_t rtc_sec, int &hour, int &minute, int &second) {
{ hour = 0;
hour = 0; minute = 0;
minute = 0; second = 0;
second = 0; if (rtc_sec == 0)
if (rtc_sec == 0) return;
return; uint32_t hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY;
uint32_t hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY; hour = hms / SEC_PER_HOUR;
hour = hms / SEC_PER_HOUR; minute = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
minute = (hms % SEC_PER_HOUR) / SEC_PER_MIN; second = hms % SEC_PER_MIN;
second = hms % SEC_PER_MIN;
} }
// === Shared External State === // === Shared External State ===
@@ -68,457 +65,448 @@ uint32_t lastMailBlink = 0;
// ********************************* // *********************************
// * Rounded Header when inverted * // * Rounded Header when inverted *
// ********************************* // *********************************
void drawRoundedHighlight(OLEDDisplay *display, int16_t x, int16_t y, int16_t w, int16_t h, int16_t r) void drawRoundedHighlight(OLEDDisplay *display, int16_t x, int16_t y, int16_t w, int16_t h, int16_t r) {
{ // Draw the center and side rectangles
// Draw the center and side rectangles display->fillRect(x + r, y, w - 2 * r, h); // center bar
display->fillRect(x + r, y, w - 2 * r, h); // center bar display->fillRect(x, y + r, r, h - 2 * r); // left edge
display->fillRect(x, y + r, r, h - 2 * r); // left edge display->fillRect(x + w - r, y + r, r, h - 2 * r); // right edge
display->fillRect(x + w - r, y + r, r, h - 2 * r); // right edge
// Draw the rounded corners using filled circles // Draw the rounded corners using filled circles
display->fillCircle(x + r + 1, y + r, r); // top-left display->fillCircle(x + r + 1, y + r, r); // top-left
display->fillCircle(x + w - r - 1, y + r, r); // top-right display->fillCircle(x + w - r - 1, y + r, r); // top-right
display->fillCircle(x + r + 1, y + h - r - 1, r); // bottom-left display->fillCircle(x + r + 1, y + h - r - 1, r); // bottom-left
display->fillCircle(x + w - r - 1, y + h - r - 1, r); // bottom-right display->fillCircle(x + w - r - 1, y + h - r - 1, r); // bottom-right
} }
// ************************* // *************************
// * Common Header Drawing * // * Common Header Drawing *
// ************************* // *************************
void drawCommonHeader(OLEDDisplay *display, int16_t x, int16_t y, const char *titleStr, bool force_no_invert, bool show_date) void drawCommonHeader(OLEDDisplay *display, int16_t x, int16_t y, const char *titleStr, bool force_no_invert, bool show_date) {
{ constexpr int HEADER_OFFSET_Y = 1;
constexpr int HEADER_OFFSET_Y = 1; y += HEADER_OFFSET_Y;
y += HEADER_OFFSET_Y;
display->setFont(FONT_SMALL); display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_LEFT); display->setTextAlignment(TEXT_ALIGN_LEFT);
const int xOffset = 4; const int xOffset = 4;
const int highlightHeight = FONT_HEIGHT_SMALL - 1; const int highlightHeight = FONT_HEIGHT_SMALL - 1;
const bool isInverted = (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_INVERTED); const bool isInverted = (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_INVERTED);
const bool isBold = config.display.heading_bold; const bool isBold = config.display.heading_bold;
const int screenW = display->getWidth(); const int screenW = display->getWidth();
const int screenH = display->getHeight(); const int screenH = display->getHeight();
if (!force_no_invert) { if (!force_no_invert) {
// === Inverted Header Background === // === Inverted Header Background ===
if (isInverted) { if (isInverted) {
display->setColor(BLACK); display->setColor(BLACK);
display->fillRect(0, 0, screenW, highlightHeight + 2); display->fillRect(0, 0, screenW, highlightHeight + 2);
display->setColor(WHITE); display->setColor(WHITE);
drawRoundedHighlight(display, x, y, screenW, highlightHeight, 2); drawRoundedHighlight(display, x, y, screenW, highlightHeight, 2);
display->setColor(BLACK); display->setColor(BLACK);
} else { } else {
display->setColor(BLACK); display->setColor(BLACK);
display->fillRect(0, 0, screenW, highlightHeight + 2); display->fillRect(0, 0, screenW, highlightHeight + 2);
display->setColor(WHITE); display->setColor(WHITE);
if (currentResolution == ScreenResolution::High) { if (currentResolution == ScreenResolution::High) {
display->drawLine(0, 20, screenW, 20); display->drawLine(0, 20, screenW, 20);
} else { } else {
display->drawLine(0, 14, screenW, 14); display->drawLine(0, 14, screenW, 14);
} }
}
// === Screen Title ===
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(SCREEN_WIDTH / 2, y, titleStr);
if (config.display.heading_bold) {
display->drawString((SCREEN_WIDTH / 2) + 1, y, titleStr);
}
}
display->setTextAlignment(TEXT_ALIGN_LEFT);
// === Battery State ===
int chargePercent = powerStatus->getBatteryChargePercent();
bool isCharging = powerStatus->getIsCharging();
bool usbPowered = powerStatus->getHasUSB();
if (chargePercent >= 100) {
isCharging = false;
}
if (chargePercent == 101) {
usbPowered = true; // Forcing this flag on for the express purpose that some devices have no concept of having a USB cable
// plugged in
} }
uint32_t now = millis(); // === Screen Title ===
display->setTextAlignment(TEXT_ALIGN_CENTER);
display->drawString(SCREEN_WIDTH / 2, y, titleStr);
if (config.display.heading_bold) {
display->drawString((SCREEN_WIDTH / 2) + 1, y, titleStr);
}
}
display->setTextAlignment(TEXT_ALIGN_LEFT);
// === Battery State ===
int chargePercent = powerStatus->getBatteryChargePercent();
bool isCharging = powerStatus->getIsCharging();
bool usbPowered = powerStatus->getHasUSB();
if (chargePercent >= 100) {
isCharging = false;
}
if (chargePercent == 101) {
usbPowered = true; // Forcing this flag on for the express purpose that some devices have no concept of having a USB
// cable plugged in
}
uint32_t now = millis();
#ifndef USE_EINK #ifndef USE_EINK
if (isCharging && now - lastBlinkShared > 500) { if (isCharging && now - lastBlinkShared > 500) {
isBoltVisibleShared = !isBoltVisibleShared; isBoltVisibleShared = !isBoltVisibleShared;
lastBlinkShared = now; lastBlinkShared = now;
} }
#endif #endif
bool useHorizontalBattery = (currentResolution == ScreenResolution::High && screenW >= screenH); bool useHorizontalBattery = (currentResolution == ScreenResolution::High && screenW >= screenH);
const int textY = y + (highlightHeight - FONT_HEIGHT_SMALL) / 2; const int textY = y + (highlightHeight - FONT_HEIGHT_SMALL) / 2;
int batteryX = 1; int batteryX = 1;
int batteryY = HEADER_OFFSET_Y + 1; int batteryY = HEADER_OFFSET_Y + 1;
#if !defined(M5STACK_UNITC6L) #if !defined(M5STACK_UNITC6L)
// === Battery Icons === // === Battery Icons ===
if (usbPowered && !isCharging) { // This is a basic check to determine USB Powered is flagged but not charging if (usbPowered && !isCharging) { // This is a basic check to determine USB Powered is flagged but not charging
batteryX += 1; batteryX += 1;
batteryY += 2; batteryY += 2;
if (currentResolution == ScreenResolution::High) { if (currentResolution == ScreenResolution::High) {
display->drawXbm(batteryX, batteryY, 19, 12, imgUSB_HighResolution); display->drawXbm(batteryX, batteryY, 19, 12, imgUSB_HighResolution);
batteryX += 20; // Icon + 1 pixel batteryX += 20; // Icon + 1 pixel
} else { } else {
display->drawXbm(batteryX, batteryY, 10, 8, imgUSB); display->drawXbm(batteryX, batteryY, 10, 8, imgUSB);
batteryX += 11; // Icon + 1 pixel batteryX += 11; // Icon + 1 pixel
} }
} else {
if (useHorizontalBattery) {
batteryX += 1;
batteryY += 2;
display->drawXbm(batteryX, batteryY, 9, 13, batteryBitmap_h_bottom);
display->drawXbm(batteryX + 9, batteryY, 9, 13, batteryBitmap_h_top);
if (isCharging && isBoltVisibleShared)
display->drawXbm(batteryX + 4, batteryY, 9, 13, lightning_bolt_h);
else {
display->drawLine(batteryX + 5, batteryY, batteryX + 10, batteryY);
display->drawLine(batteryX + 5, batteryY + 12, batteryX + 10, batteryY + 12);
int fillWidth = 14 * chargePercent / 100;
display->fillRect(batteryX + 1, batteryY + 1, fillWidth, 11);
}
batteryX += 18; // Icon + 2 pixels
} else { } else {
if (useHorizontalBattery) {
batteryX += 1;
batteryY += 2;
display->drawXbm(batteryX, batteryY, 9, 13, batteryBitmap_h_bottom);
display->drawXbm(batteryX + 9, batteryY, 9, 13, batteryBitmap_h_top);
if (isCharging && isBoltVisibleShared)
display->drawXbm(batteryX + 4, batteryY, 9, 13, lightning_bolt_h);
else {
display->drawLine(batteryX + 5, batteryY, batteryX + 10, batteryY);
display->drawLine(batteryX + 5, batteryY + 12, batteryX + 10, batteryY + 12);
int fillWidth = 14 * chargePercent / 100;
display->fillRect(batteryX + 1, batteryY + 1, fillWidth, 11);
}
batteryX += 18; // Icon + 2 pixels
} else {
#ifdef USE_EINK #ifdef USE_EINK
batteryY += 2; batteryY += 2;
#endif #endif
display->drawXbm(batteryX, batteryY, 7, 11, batteryBitmap_v); display->drawXbm(batteryX, batteryY, 7, 11, batteryBitmap_v);
if (isCharging && isBoltVisibleShared) if (isCharging && isBoltVisibleShared)
display->drawXbm(batteryX + 1, batteryY + 3, 5, 5, lightning_bolt_v); display->drawXbm(batteryX + 1, batteryY + 3, 5, 5, lightning_bolt_v);
else { else {
display->drawXbm(batteryX - 1, batteryY + 4, 8, 3, batteryBitmap_sidegaps_v); display->drawXbm(batteryX - 1, batteryY + 4, 8, 3, batteryBitmap_sidegaps_v);
int fillHeight = 8 * chargePercent / 100; int fillHeight = 8 * chargePercent / 100;
int fillY = batteryY - fillHeight; int fillY = batteryY - fillHeight;
display->fillRect(batteryX + 1, fillY + 10, 5, fillHeight); display->fillRect(batteryX + 1, fillY + 10, 5, fillHeight);
} }
batteryX += 9; // Icon + 2 pixels batteryX += 9; // Icon + 2 pixels
}
} }
}
if (chargePercent != 101) { if (chargePercent != 101) {
// === Battery % Display === // === Battery % Display ===
char chargeStr[4]; char chargeStr[4];
snprintf(chargeStr, sizeof(chargeStr), "%d", chargePercent); snprintf(chargeStr, sizeof(chargeStr), "%d", chargePercent);
int chargeNumWidth = display->getStringWidth(chargeStr); int chargeNumWidth = display->getStringWidth(chargeStr);
display->drawString(batteryX, textY, chargeStr); display->drawString(batteryX, textY, chargeStr);
display->drawString(batteryX + chargeNumWidth - 1, textY, "%"); display->drawString(batteryX + chargeNumWidth - 1, textY, "%");
if (isBold) { if (isBold) {
display->drawString(batteryX + 1, textY, chargeStr); display->drawString(batteryX + 1, textY, chargeStr);
display->drawString(batteryX + chargeNumWidth, textY, "%"); display->drawString(batteryX + chargeNumWidth, textY, "%");
}
} }
}
// === Time and Right-aligned Icons === // === Time and Right-aligned Icons ===
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true);
char timeStr[10] = "--:--"; // Fallback display char timeStr[10] = "--:--"; // Fallback display
int timeStrWidth = display->getStringWidth("12:34"); // Default alignment int timeStrWidth = display->getStringWidth("12:34"); // Default alignment
int timeX = screenW - xOffset - timeStrWidth + 4; int timeX = screenW - xOffset - timeStrWidth + 4;
if (rtc_sec > 0) { if (rtc_sec > 0) {
// === Build Time String === // === Build Time String ===
long hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY; long hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY;
int hour, minute, second; int hour, minute, second;
graphics::decomposeTime(rtc_sec, hour, minute, second); graphics::decomposeTime(rtc_sec, hour, minute, second);
snprintf(timeStr, sizeof(timeStr), "%d:%02d", hour, minute); snprintf(timeStr, sizeof(timeStr), "%d:%02d", hour, minute);
// === Build Date String === // === Build Date String ===
char datetimeStr[25]; char datetimeStr[25];
UIRenderer::formatDateTime(datetimeStr, sizeof(datetimeStr), rtc_sec, display, false); UIRenderer::formatDateTime(datetimeStr, sizeof(datetimeStr), rtc_sec, display, false);
char dateLine[40]; char dateLine[40];
if (currentResolution == ScreenResolution::High) {
snprintf(dateLine, sizeof(dateLine), "%s", datetimeStr);
} else {
if (hasUnreadMessage) {
snprintf(dateLine, sizeof(dateLine), "%s", &datetimeStr[5]);
} else {
snprintf(dateLine, sizeof(dateLine), "%s", &datetimeStr[2]);
}
}
if (config.display.use_12h_clock) {
bool isPM = hour >= 12;
hour %= 12;
if (hour == 0)
hour = 12;
snprintf(timeStr, sizeof(timeStr), "%d:%02d%s", hour, minute, isPM ? "p" : "a");
}
if (show_date) {
timeStrWidth = display->getStringWidth(dateLine);
} else {
timeStrWidth = display->getStringWidth(timeStr);
}
timeX = screenW - xOffset - timeStrWidth + 3;
// === Show Mail or Mute Icon to the Left of Time ===
int iconRightEdge = timeX - 2;
bool showMail = false;
#ifndef USE_EINK
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
#else
if (hasUnreadMessage) {
showMail = true;
}
#endif
if (showMail) {
if (useHorizontalBattery) {
int iconW = 16, iconH = 12;
int iconX = iconRightEdge - iconW;
int iconY = textY + (FONT_HEIGHT_SMALL - iconH) / 2 - 1;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, iconW + 3, iconH + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, iconW + 3, iconH + 2);
display->setColor(WHITE);
}
display->drawRect(iconX, iconY, iconW + 1, iconH);
display->drawLine(iconX, iconY, iconX + iconW / 2, iconY + iconH - 4);
display->drawLine(iconX + iconW, iconY, iconX + iconW / 2, iconY + iconH - 4);
} else {
int iconX = iconRightEdge - (mail_width - 2);
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, mail_width + 2, mail_height + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, mail_width + 2, mail_height + 2);
display->setColor(WHITE);
}
display->drawXbm(iconX, iconY, mail_width, mail_height, mail);
}
} else if (externalNotificationModule->getMute()) {
if (currentResolution == ScreenResolution::High) {
int iconX = iconRightEdge - mute_symbol_big_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mute_symbol_big_height) / 2;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_big_width + 2, mute_symbol_big_height + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_big_width + 2, mute_symbol_big_height + 2);
display->setColor(WHITE);
}
display->drawXbm(iconX, iconY, mute_symbol_big_width, mute_symbol_big_height, mute_symbol_big);
} else {
int iconX = iconRightEdge - mute_symbol_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_width + 2, mute_symbol_height + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_width + 2, mute_symbol_height + 2);
display->setColor(WHITE);
}
display->drawXbm(iconX, iconY, mute_symbol_width, mute_symbol_height, mute_symbol);
}
}
if (show_date) {
// === Draw Date ===
display->drawString(timeX, textY, dateLine);
if (isBold)
display->drawString(timeX - 1, textY, dateLine);
} else {
// === Draw Time ===
display->drawString(timeX, textY, timeStr);
if (isBold)
display->drawString(timeX - 1, textY, timeStr);
}
} else {
// === No Time Available: Mail/Mute Icon Moves to Far Right ===
int iconRightEdge = screenW - xOffset;
bool showMail = false;
#ifndef USE_EINK
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
#else
if (hasUnreadMessage) {
showMail = true;
}
#endif
if (showMail) {
if (useHorizontalBattery) {
int iconW = 16, iconH = 12;
int iconX = iconRightEdge - iconW;
int iconY = textY + (FONT_HEIGHT_SMALL - iconH) / 2 - 1;
display->drawRect(iconX, iconY, iconW + 1, iconH);
display->drawLine(iconX, iconY, iconX + iconW / 2, iconY + iconH - 4);
display->drawLine(iconX + iconW, iconY, iconX + iconW / 2, iconY + iconH - 4);
} else {
int iconX = iconRightEdge - mail_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
display->drawXbm(iconX, iconY, mail_width, mail_height, mail);
}
} else if (externalNotificationModule->getMute()) {
if (currentResolution == ScreenResolution::High) {
int iconX = iconRightEdge - mute_symbol_big_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mute_symbol_big_height) / 2;
display->drawXbm(iconX, iconY, mute_symbol_big_width, mute_symbol_big_height, mute_symbol_big);
} else {
int iconX = iconRightEdge - mute_symbol_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
display->drawXbm(iconX, iconY, mute_symbol_width, mute_symbol_height, mute_symbol);
}
}
}
#endif
display->setColor(WHITE); // Reset for other UI
}
const int *getTextPositions(OLEDDisplay *display)
{
static int textPositions[7]; // Static array that persists beyond function scope
if (currentResolution == ScreenResolution::High) { if (currentResolution == ScreenResolution::High) {
textPositions[0] = textZeroLine; snprintf(dateLine, sizeof(dateLine), "%s", datetimeStr);
textPositions[1] = textFirstLine_medium;
textPositions[2] = textSecondLine_medium;
textPositions[3] = textThirdLine_medium;
textPositions[4] = textFourthLine_medium;
textPositions[5] = textFifthLine_medium;
textPositions[6] = textSixthLine_medium;
} else { } else {
textPositions[0] = textZeroLine; if (hasUnreadMessage) {
textPositions[1] = textFirstLine; snprintf(dateLine, sizeof(dateLine), "%s", &datetimeStr[5]);
textPositions[2] = textSecondLine; } else {
textPositions[3] = textThirdLine; snprintf(dateLine, sizeof(dateLine), "%s", &datetimeStr[2]);
textPositions[4] = textFourthLine; }
textPositions[5] = textFifthLine;
textPositions[6] = textSixthLine;
} }
return textPositions;
if (config.display.use_12h_clock) {
bool isPM = hour >= 12;
hour %= 12;
if (hour == 0)
hour = 12;
snprintf(timeStr, sizeof(timeStr), "%d:%02d%s", hour, minute, isPM ? "p" : "a");
}
if (show_date) {
timeStrWidth = display->getStringWidth(dateLine);
} else {
timeStrWidth = display->getStringWidth(timeStr);
}
timeX = screenW - xOffset - timeStrWidth + 3;
// === Show Mail or Mute Icon to the Left of Time ===
int iconRightEdge = timeX - 2;
bool showMail = false;
#ifndef USE_EINK
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
#else
if (hasUnreadMessage) {
showMail = true;
}
#endif
if (showMail) {
if (useHorizontalBattery) {
int iconW = 16, iconH = 12;
int iconX = iconRightEdge - iconW;
int iconY = textY + (FONT_HEIGHT_SMALL - iconH) / 2 - 1;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, iconW + 3, iconH + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, iconW + 3, iconH + 2);
display->setColor(WHITE);
}
display->drawRect(iconX, iconY, iconW + 1, iconH);
display->drawLine(iconX, iconY, iconX + iconW / 2, iconY + iconH - 4);
display->drawLine(iconX + iconW, iconY, iconX + iconW / 2, iconY + iconH - 4);
} else {
int iconX = iconRightEdge - (mail_width - 2);
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, mail_width + 2, mail_height + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, mail_width + 2, mail_height + 2);
display->setColor(WHITE);
}
display->drawXbm(iconX, iconY, mail_width, mail_height, mail);
}
} else if (externalNotificationModule->getMute()) {
if (currentResolution == ScreenResolution::High) {
int iconX = iconRightEdge - mute_symbol_big_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mute_symbol_big_height) / 2;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_big_width + 2, mute_symbol_big_height + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_big_width + 2, mute_symbol_big_height + 2);
display->setColor(WHITE);
}
display->drawXbm(iconX, iconY, mute_symbol_big_width, mute_symbol_big_height, mute_symbol_big);
} else {
int iconX = iconRightEdge - mute_symbol_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
if (isInverted && !force_no_invert) {
display->setColor(WHITE);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_width + 2, mute_symbol_height + 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(iconX - 1, iconY - 1, mute_symbol_width + 2, mute_symbol_height + 2);
display->setColor(WHITE);
}
display->drawXbm(iconX, iconY, mute_symbol_width, mute_symbol_height, mute_symbol);
}
}
if (show_date) {
// === Draw Date ===
display->drawString(timeX, textY, dateLine);
if (isBold)
display->drawString(timeX - 1, textY, dateLine);
} else {
// === Draw Time ===
display->drawString(timeX, textY, timeStr);
if (isBold)
display->drawString(timeX - 1, textY, timeStr);
}
} else {
// === No Time Available: Mail/Mute Icon Moves to Far Right ===
int iconRightEdge = screenW - xOffset;
bool showMail = false;
#ifndef USE_EINK
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
#else
if (hasUnreadMessage) {
showMail = true;
}
#endif
if (showMail) {
if (useHorizontalBattery) {
int iconW = 16, iconH = 12;
int iconX = iconRightEdge - iconW;
int iconY = textY + (FONT_HEIGHT_SMALL - iconH) / 2 - 1;
display->drawRect(iconX, iconY, iconW + 1, iconH);
display->drawLine(iconX, iconY, iconX + iconW / 2, iconY + iconH - 4);
display->drawLine(iconX + iconW, iconY, iconX + iconW / 2, iconY + iconH - 4);
} else {
int iconX = iconRightEdge - mail_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
display->drawXbm(iconX, iconY, mail_width, mail_height, mail);
}
} else if (externalNotificationModule->getMute()) {
if (currentResolution == ScreenResolution::High) {
int iconX = iconRightEdge - mute_symbol_big_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mute_symbol_big_height) / 2;
display->drawXbm(iconX, iconY, mute_symbol_big_width, mute_symbol_big_height, mute_symbol_big);
} else {
int iconX = iconRightEdge - mute_symbol_width;
int iconY = textY + (FONT_HEIGHT_SMALL - mail_height) / 2;
display->drawXbm(iconX, iconY, mute_symbol_width, mute_symbol_height, mute_symbol);
}
}
}
#endif
display->setColor(WHITE); // Reset for other UI
}
const int *getTextPositions(OLEDDisplay *display) {
static int textPositions[7]; // Static array that persists beyond function scope
if (currentResolution == ScreenResolution::High) {
textPositions[0] = textZeroLine;
textPositions[1] = textFirstLine_medium;
textPositions[2] = textSecondLine_medium;
textPositions[3] = textThirdLine_medium;
textPositions[4] = textFourthLine_medium;
textPositions[5] = textFifthLine_medium;
textPositions[6] = textSixthLine_medium;
} else {
textPositions[0] = textZeroLine;
textPositions[1] = textFirstLine;
textPositions[2] = textSecondLine;
textPositions[3] = textThirdLine;
textPositions[4] = textFourthLine;
textPositions[5] = textFifthLine;
textPositions[6] = textSixthLine;
}
return textPositions;
} }
// ************************* // *************************
// * Common Footer Drawing * // * Common Footer Drawing *
// ************************* // *************************
void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y) void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y) {
{ bool drawConnectionState = false;
bool drawConnectionState = false; if (service->api_state == service->STATE_BLE || service->api_state == service->STATE_WIFI || service->api_state == service->STATE_SERIAL ||
if (service->api_state == service->STATE_BLE || service->api_state == service->STATE_WIFI || service->api_state == service->STATE_PACKET || service->api_state == service->STATE_HTTP || service->api_state == service->STATE_ETH) {
service->api_state == service->STATE_SERIAL || service->api_state == service->STATE_PACKET || drawConnectionState = true;
service->api_state == service->STATE_HTTP || service->api_state == service->STATE_ETH) { }
drawConnectionState = true;
}
if (drawConnectionState) { if (drawConnectionState) {
const int scale = (currentResolution == ScreenResolution::High) ? 2 : 1; const int scale = (currentResolution == ScreenResolution::High) ? 2 : 1;
display->setColor(BLACK); display->setColor(BLACK);
display->fillRect(0, SCREEN_HEIGHT - (1 * scale) - (connection_icon_height * scale), (connection_icon_width * scale), display->fillRect(0, SCREEN_HEIGHT - (1 * scale) - (connection_icon_height * scale), (connection_icon_width * scale),
(connection_icon_height * scale) + (2 * scale)); (connection_icon_height * scale) + (2 * scale));
display->setColor(WHITE); display->setColor(WHITE);
if (currentResolution == ScreenResolution::High) { if (currentResolution == ScreenResolution::High) {
const int bytesPerRow = (connection_icon_width + 7) / 8; const int bytesPerRow = (connection_icon_width + 7) / 8;
int iconX = 0; int iconX = 0;
int iconY = SCREEN_HEIGHT - (connection_icon_height * 2); int iconY = SCREEN_HEIGHT - (connection_icon_height * 2);
for (int yy = 0; yy < connection_icon_height; ++yy) { for (int yy = 0; yy < connection_icon_height; ++yy) {
const uint8_t *rowPtr = connection_icon + yy * bytesPerRow; const uint8_t *rowPtr = connection_icon + yy * bytesPerRow;
for (int xx = 0; xx < connection_icon_width; ++xx) { for (int xx = 0; xx < connection_icon_width; ++xx) {
const uint8_t byteVal = pgm_read_byte(rowPtr + (xx >> 3)); const uint8_t byteVal = pgm_read_byte(rowPtr + (xx >> 3));
const uint8_t bitMask = 1U << (xx & 7); // XBM is LSB-first const uint8_t bitMask = 1U << (xx & 7); // XBM is LSB-first
if (byteVal & bitMask) { if (byteVal & bitMask) {
display->fillRect(iconX + xx * scale, iconY + yy * scale, scale, scale); display->fillRect(iconX + xx * scale, iconY + yy * scale, scale, scale);
} }
}
}
} else {
display->drawXbm(0, SCREEN_HEIGHT - connection_icon_height, connection_icon_width, connection_icon_height,
connection_icon);
} }
}
} else {
display->drawXbm(0, SCREEN_HEIGHT - connection_icon_height, connection_icon_width, connection_icon_height, connection_icon);
} }
}
} }
bool isAllowedPunctuation(char c) bool isAllowedPunctuation(char c) {
{ const std::string allowed = ".,!?;:-_()[]{}'\"@#$/\\&+=%~^ ";
const std::string allowed = ".,!?;:-_()[]{}'\"@#$/\\&+=%~^ "; return allowed.find(c) != std::string::npos;
return allowed.find(c) != std::string::npos;
} }
static void replaceAll(std::string &s, const std::string &from, const std::string &to) static void replaceAll(std::string &s, const std::string &from, const std::string &to) {
{ if (from.empty())
if (from.empty()) return;
return; size_t pos = 0;
size_t pos = 0; while ((pos = s.find(from, pos)) != std::string::npos) {
while ((pos = s.find(from, pos)) != std::string::npos) { s.replace(pos, from.size(), to);
s.replace(pos, from.size(), to); pos += to.size();
pos += to.size(); }
}
} }
std::string sanitizeString(const std::string &input) std::string sanitizeString(const std::string &input) {
{ std::string output;
std::string output; bool inReplacement = false;
bool inReplacement = false;
// Make a mutable copy so we can normalize UTF-8 “smart punctuation” into ASCII first. // Make a mutable copy so we can normalize UTF-8 “smart punctuation” into ASCII first.
std::string s = input; std::string s = input;
// Curly single quotes: ‘ ’ // Curly single quotes: ‘ ’
replaceAll(s, "\xE2\x80\x98", "'"); // U+2018 replaceAll(s, "\xE2\x80\x98", "'"); // U+2018
replaceAll(s, "\xE2\x80\x99", "'"); // U+2019 replaceAll(s, "\xE2\x80\x99", "'"); // U+2019
// Curly double quotes: “ ” // Curly double quotes: “ ”
replaceAll(s, "\xE2\x80\x9C", "\""); // U+201C replaceAll(s, "\xE2\x80\x9C", "\""); // U+201C
replaceAll(s, "\xE2\x80\x9D", "\""); // U+201D replaceAll(s, "\xE2\x80\x9D", "\""); // U+201D
// En dash / Em dash: – — // En dash / Em dash: – —
replaceAll(s, "\xE2\x80\x93", "-"); // U+2013 replaceAll(s, "\xE2\x80\x93", "-"); // U+2013
replaceAll(s, "\xE2\x80\x94", "-"); // U+2014 replaceAll(s, "\xE2\x80\x94", "-"); // U+2014
// Non-breaking space // Non-breaking space
replaceAll(s, "\xC2\xA0", " "); // U+00A0 replaceAll(s, "\xC2\xA0", " "); // U+00A0
// Now do your original sanitize pass over the normalized string. // Now do your original sanitize pass over the normalized string.
for (unsigned char uc : s) { for (unsigned char uc : s) {
char c = static_cast<char>(uc); char c = static_cast<char>(uc);
if (std::isalnum(uc) || isAllowedPunctuation(c)) { if (std::isalnum(uc) || isAllowedPunctuation(c)) {
output += c; output += c;
inReplacement = false; inReplacement = false;
} else { } else {
if (!inReplacement) { if (!inReplacement) {
output += static_cast<char>(0xBF); // ISO-8859-1 for inverted question mark output += static_cast<char>(0xBF); // ISO-8859-1 for inverted question mark
inReplacement = true; inReplacement = true;
} }
}
} }
}
return output; return output;
} }
} // namespace graphics } // namespace graphics
src/graphics/SharedUIDisplay.h
+2 -4
View File
@@ -3,8 +3,7 @@
#include <OLEDDisplay.h> #include <OLEDDisplay.h>
#include <string> #include <string>
namespace graphics namespace graphics {
{
// ======================= // =======================
// Shared UI Helpers // Shared UI Helpers
@@ -51,8 +50,7 @@ void decomposeTime(uint32_t rtc_sec, int &hour, int &minute, int &second);
void drawRoundedHighlight(OLEDDisplay *display, int16_t x, int16_t y, int16_t w, int16_t h, int16_t r); void drawRoundedHighlight(OLEDDisplay *display, int16_t x, int16_t y, int16_t w, int16_t h, int16_t r);
// Shared battery/time/mail header // Shared battery/time/mail header
void drawCommonHeader(OLEDDisplay *display, int16_t x, int16_t y, const char *titleStr = "", bool force_no_invert = false, void drawCommonHeader(OLEDDisplay *display, int16_t x, int16_t y, const char *titleStr = "", bool force_no_invert = false, bool show_date = false);
bool show_date = false);
// Shared battery/time/mail header // Shared battery/time/mail header
void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y); void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y);
src/graphics/TFTDisplay.cpp
+1109 -1160
View File
File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More