kevin/meshtastic-firmware
1
0
Fork 0
Code Issues Pull Requests Actions 4 Packages Projects Releases Wiki Activity

Revert "add a .clang-format file (#9154)" (#9172)

Browse Source 
I thought git would be smart enough to understand all the whitespace changes but even with all the flags I know to make it ignore theses it still blows up if there are identical changes on both sides.

I have a solution but it require creating a new commit at the merge base for each conflicting PR and merging it into develop.

I don't think blowing up all PRs is worth for now, maybe if we can coordinate this for V3 let's say.

This reverts commit 0d11331d18.
This commit is contained in:
Jorropo
2026-01-04 05:15:53 -06:00
committed by GitHub
co-authored by GitHub
parent 0d11331d18
commit beb268ff25
771 changed files with 83399 additions and 77967 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.clang-format
-2
View File
@@ -1,2 +0,0 @@
BasedOnStyle: LLVM
ColumnLimit: 150
.clusterfuzzlite/router_fuzzer.cpp
+135 -128
View File
@@ -16,7 +16,8 @@
#include "mesh/TypeConversions.h"
#include "mesh/mesh-pb-constants.h"
namespace {
namespace
{
constexpr uint32_t nodeId = 0x12345678;
// Set to true when lateInitVariant finishes. Used to ensure lateInitVariant was called during startup.
bool hasBeenConfigured = false;
@@ -34,130 +35,135 @@ std::condition_variable loopCV;
std::thread meshtasticThread;
// This exception is thrown when the portuino main thread should exit.
class ShouldExitException : public std::runtime_error {
public:
using std::runtime_error::runtime_error;
class ShouldExitException : public std::runtime_error
{
public:
using std::runtime_error::runtime_error;
};
// 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
// single, currently running, test case.
void runLoopOnce() {
realHardware = true; // Avoids delay(100) within portduino/main.cpp
std::unique_lock<std::mutex> lck(loopLock);
fuzzerRunning = true;
loopCanRun = true;
loopCV.notify_one();
loopCV.wait(lck, [] { return !loopCanRun && loopIsWaiting; });
void runLoopOnce()
{
realHardware = true; // Avoids delay(100) within portduino/main.cpp
std::unique_lock<std::mutex> lck(loopLock);
fuzzerRunning = true;
loopCanRun = true;
loopCV.notify_one();
loopCV.wait(lck, [] { return !loopCanRun && loopIsWaiting; });
}
} // namespace
// 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
// fuzzer input is ready.
bool loopCanSleep() {
std::unique_lock<std::mutex> lck(loopLock);
loopIsWaiting = true;
loopCV.notify_one();
loopCV.wait(lck, [] { return loopCanRun || loopShouldExit; });
loopIsWaiting = false;
if (loopShouldExit)
throw ShouldExitException("exit");
if (!fuzzerRunning)
return true; // The loop can sleep before the fuzzer starts.
loopCanRun = false; // Only run the loop once before waiting again.
return false;
bool loopCanSleep()
{
std::unique_lock<std::mutex> lck(loopLock);
loopIsWaiting = true;
loopCV.notify_one();
loopCV.wait(lck, [] { return loopCanRun || loopShouldExit; });
loopIsWaiting = false;
if (loopShouldExit)
throw ShouldExitException("exit");
if (!fuzzerRunning)
return true; // The loop can sleep before the fuzzer starts.
loopCanRun = false; // Only run the loop once before waiting again.
return false;
}
// Called just prior to starting Meshtastic. Allows for setting config values before startup.
void lateInitVariant() {
portduino_config.logoutputlevel = level_error;
channelFile.channels[0] = meshtastic_Channel{
.has_settings = true,
.settings =
meshtastic_ChannelSettings{
.psk = {.size = 1, .bytes = {/*defaultpskIndex=*/1}},
.name = "LongFast",
.uplink_enabled = true,
.has_module_settings = true,
.module_settings = {.position_precision = 16},
},
.role = meshtastic_Channel_Role_PRIMARY,
};
config.security.admin_key[0] = {
.size = 32,
.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},
};
config.security.admin_key_count = 1;
config.lora.region = meshtastic_Config_LoRaConfig_RegionCode_US;
moduleConfig.has_mqtt = true;
moduleConfig.mqtt = meshtastic_ModuleConfig_MQTTConfig{
.enabled = true,
.proxy_to_client_enabled = true,
};
moduleConfig.has_store_forward = true;
moduleConfig.store_forward = meshtastic_ModuleConfig_StoreForwardConfig{
.enabled = true,
.history_return_max = 4,
.history_return_window = 600,
.is_server = true,
};
meshtastic_Position fixedGPS = meshtastic_Position{
.has_latitude_i = true,
.latitude_i = static_cast<uint32_t>(1 * 1e7),
.has_longitude_i = true,
.longitude_i = static_cast<uint32_t>(3 * 1e7),
.has_altitude = true,
.altitude = 64,
.location_source = meshtastic_Position_LocSource_LOC_MANUAL,
};
nodeDB->setLocalPosition(fixedGPS);
config.has_position = true;
config.position.fixed_position = true;
meshtastic_NodeInfoLite *info = nodeDB->getMeshNode(nodeDB->getNodeNum());
info->has_position = true;
info->position = TypeConversions::ConvertToPositionLite(fixedGPS);
hasBeenConfigured = true;
void lateInitVariant()
{
portduino_config.logoutputlevel = level_error;
channelFile.channels[0] = meshtastic_Channel{
.has_settings = true,
.settings =
meshtastic_ChannelSettings{
.psk = {.size = 1, .bytes = {/*defaultpskIndex=*/1}},
.name = "LongFast",
.uplink_enabled = true,
.has_module_settings = true,
.module_settings = {.position_precision = 16},
},
.role = meshtastic_Channel_Role_PRIMARY,
};
config.security.admin_key[0] = {
.size = 32,
.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},
};
config.security.admin_key_count = 1;
config.lora.region = meshtastic_Config_LoRaConfig_RegionCode_US;
moduleConfig.has_mqtt = true;
moduleConfig.mqtt = meshtastic_ModuleConfig_MQTTConfig{
.enabled = true,
.proxy_to_client_enabled = true,
};
moduleConfig.has_store_forward = true;
moduleConfig.store_forward = meshtastic_ModuleConfig_StoreForwardConfig{
.enabled = true,
.history_return_max = 4,
.history_return_window = 600,
.is_server = true,
};
meshtastic_Position fixedGPS = meshtastic_Position{
.has_latitude_i = true,
.latitude_i = static_cast<uint32_t>(1 * 1e7),
.has_longitude_i = true,
.longitude_i = static_cast<uint32_t>(3 * 1e7),
.has_altitude = true,
.altitude = 64,
.location_source = meshtastic_Position_LocSource_LOC_MANUAL,
};
nodeDB->setLocalPosition(fixedGPS);
config.has_position = true;
config.position.fixed_position = true;
meshtastic_NodeInfoLite *info = nodeDB->getMeshNode(nodeDB->getNodeNum());
info->has_position = true;
info->position = TypeConversions::ConvertToPositionLite(fixedGPS);
hasBeenConfigured = true;
}
extern "C" {
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.
int LLVMFuzzerInitialize(int *argc, char ***argv) {
portduino_config.maxtophone = 5;
int LLVMFuzzerInitialize(int *argc, char ***argv)
{
portduino_config.maxtophone = 5;
meshtasticThread = std::thread([program = *argv[0]]() {
char nodeIdStr[12];
strcpy(nodeIdStr, std::to_string(nodeId).c_str());
int argc = 7;
char *argv[] = {program, "-d", "/tmp/meshtastic", "-h", nodeIdStr, "-p", "0", nullptr};
try {
portduino_main(argc, argv);
} catch (const ShouldExitException &) {
}
});
std::atexit([] {
{
const std::lock_guard<std::mutex> lck(loopLock);
loopShouldExit = true;
loopCV.notify_one();
}
meshtasticThread.join();
});
meshtasticThread = std::thread([program = *argv[0]]() {
char nodeIdStr[12];
strcpy(nodeIdStr, std::to_string(nodeId).c_str());
int argc = 7;
char *argv[] = {program, "-d", "/tmp/meshtastic", "-h", nodeIdStr, "-p", "0", nullptr};
try {
portduino_main(argc, argv);
} 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;
// 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));
}
std::this_thread::sleep_for(std::chrono::seconds(1));
}
return 1;
return 1;
}
// This is the main entrypoint for the fuzzer (the fuzz target). The fuzzer will provide an array of bytes to be
@@ -167,33 +173,34 @@ int LLVMFuzzerInitialize(int *argc, char ***argv) {
//
// This guide provides best practices for writing a fuzzer target.
// https://github.com/google/fuzzing/blob/master/docs/good-fuzz-target.md
int LLVMFuzzerTestOneInput(const uint8_t *data, size_t length) {
meshtastic_MeshPacket p = meshtastic_MeshPacket_init_default;
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.
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 ||
p.next_hop || p.relay_node || p.tx_after)
return -1; // Reject: The input will not be added to the corpus.
if (p.which_payload_variant == meshtastic_MeshPacket_decoded_tag) {
meshtastic_Data d;
stream = pb_istream_from_buffer(p.decoded.payload.bytes, p.decoded.payload.size);
if (!pb_decode(&stream, &meshtastic_Data_msg, &d))
return -1; // Reject: The input will not be added to the corpus.
}
int LLVMFuzzerTestOneInput(const uint8_t *data, size_t length)
{
meshtastic_MeshPacket p = meshtastic_MeshPacket_init_default;
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.
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 || p.next_hop || p.relay_node || p.tx_after)
return -1; // Reject: The input will not be added to the corpus.
if (p.which_payload_variant == meshtastic_MeshPacket_decoded_tag) {
meshtastic_Data d;
stream = pb_istream_from_buffer(p.decoded.payload.bytes, p.decoded.payload.size);
if (!pb_decode(&stream, &meshtastic_Data_msg, &d))
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.
if (p.from == 0)
p.from = nodeDB->getNodeNum();
if (p.to == 0)
p.to = nodeDB->getNodeNum();
static uint32_t packetId = 0;
if (p.id == 0)
p.id == ++packetId;
if (p.pki_encrypted && config.security.admin_key_count)
memcpy(&p.public_key, &config.security.admin_key[0], sizeof(p.public_key));
// Provide default values for a few fields so the fuzzer doesn't need to guess them.
if (p.from == 0)
p.from = nodeDB->getNodeNum();
if (p.to == 0)
p.to = nodeDB->getNodeNum();
static uint32_t packetId = 0;
if (p.id == 0)
p.id == ++packetId;
if (p.pki_encrypted && config.security.admin_key_count)
memcpy(&p.public_key, &config.security.admin_key[0], sizeof(p.public_key));
router->enqueueReceivedMessage(packetPool.allocCopy(p));
runLoopOnce();
return 0; // Accept: The input may be added to the corpus.
router->enqueueReceivedMessage(packetPool.allocCopy(p));
runLoopOnce();
return 0; // Accept: The input may be added to the corpus.
}
}
src/AmbientLightingThread.h
+124 -115
View File
@@ -21,183 +21,192 @@ Adafruit_NeoPixel pixels(NEOPIXEL_COUNT, NEOPIXEL_DATA, NEOPIXEL_TYPE);
extern unPhone unphone;
#endif
namespace concurrency {
class AmbientLightingThread : public concurrency::OSThread {
public:
explicit AmbientLightingThread(ScanI2C::DeviceType type) : OSThread("AmbientLighting") {
notifyDeepSleepObserver.observe(&notifyDeepSleep); // Let us know when shutdown() is issued.
namespace concurrency
{
class AmbientLightingThread : public concurrency::OSThread
{
public:
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.
#ifdef HAS_RGB_LED
#ifdef ENABLE_AMBIENTLIGHTING
moduleConfig.ambient_lighting.led_state = true;
moduleConfig.ambient_lighting.led_state = true;
#endif
#endif
// Uncomment to test module
// moduleConfig.ambient_lighting.led_state = true;
// moduleConfig.ambient_lighting.current = 10;
// Default to a color based on our node number
// moduleConfig.ambient_lighting.red = (myNodeInfo.my_node_num & 0xFF0000) >> 16;
// moduleConfig.ambient_lighting.green = (myNodeInfo.my_node_num & 0x00FF00) >> 8;
// moduleConfig.ambient_lighting.blue = myNodeInfo.my_node_num & 0x0000FF;
// Uncomment to test module
// moduleConfig.ambient_lighting.led_state = true;
// moduleConfig.ambient_lighting.current = 10;
// Default to a color based on our node number
// moduleConfig.ambient_lighting.red = (myNodeInfo.my_node_num & 0xFF0000) >> 16;
// moduleConfig.ambient_lighting.green = (myNodeInfo.my_node_num & 0x00FF00) >> 8;
// moduleConfig.ambient_lighting.blue = myNodeInfo.my_node_num & 0x0000FF;
#if defined(HAS_NCP5623) || defined(HAS_LP5562)
_type = type;
if (_type == ScanI2C::DeviceType::NONE) {
LOG_DEBUG("AmbientLighting Disable due to no RGB leds found on I2C bus");
disable();
return;
}
_type = type;
if (_type == ScanI2C::DeviceType::NONE) {
LOG_DEBUG("AmbientLighting Disable due to no RGB leds found on I2C bus");
disable();
return;
}
#endif
#ifdef HAS_RGB_LED
if (!moduleConfig.ambient_lighting.led_state) {
LOG_DEBUG("AmbientLighting Disable due to moduleConfig.ambient_lighting.led_state OFF");
disable();
return;
}
LOG_DEBUG("AmbientLighting init");
if (!moduleConfig.ambient_lighting.led_state) {
LOG_DEBUG("AmbientLighting Disable due to moduleConfig.ambient_lighting.led_state OFF");
disable();
return;
}
LOG_DEBUG("AmbientLighting init");
#ifdef HAS_NCP5623
if (_type == ScanI2C::NCP5623) {
rgb.begin();
if (_type == ScanI2C::NCP5623) {
rgb.begin();
#endif
#ifdef HAS_LP5562
if (_type == ScanI2C::LP5562) {
rgbw.begin();
if (_type == ScanI2C::LP5562) {
rgbw.begin();
#endif
#ifdef RGBLED_RED
pinMode(RGBLED_RED, OUTPUT);
pinMode(RGBLED_GREEN, OUTPUT);
pinMode(RGBLED_BLUE, OUTPUT);
pinMode(RGBLED_RED, OUTPUT);
pinMode(RGBLED_GREEN, OUTPUT);
pinMode(RGBLED_BLUE, OUTPUT);
#endif
#ifdef HAS_NEOPIXEL
pixels.begin(); // Initialise the pixel(s)
pixels.clear(); // Set all pixel colors to 'off'
pixels.setBrightness(moduleConfig.ambient_lighting.current);
pixels.begin(); // Initialise the pixel(s)
pixels.clear(); // Set all pixel colors to 'off'
pixels.setBrightness(moduleConfig.ambient_lighting.current);
#endif
setLighting();
setLighting();
#endif
#if defined(HAS_NCP5623) || defined(HAS_LP5562)
}
}
#endif
}
}
protected:
int32_t runOnce() override {
protected:
int32_t runOnce() override
{
#ifdef HAS_RGB_LED
#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
setLighting();
return 30000; // 30 seconds to reset from any animations that may have been running from Ext. Notification
setLighting();
return 30000; // 30 seconds to reset from any animations that may have been running from Ext. Notification
#if defined(HAS_NCP5623) || defined(HAS_LP5562)
}
}
#endif
#endif
return disable();
}
return disable();
}
// When shutdown() is issued, setLightingOff will be called.
CallbackObserver<AmbientLightingThread, void *> notifyDeepSleepObserver =
CallbackObserver<AmbientLightingThread, void *>(this, &AmbientLightingThread::setLightingOff);
// When shutdown() is issued, setLightingOff will be called.
CallbackObserver<AmbientLightingThread, void *> notifyDeepSleepObserver =
CallbackObserver<AmbientLightingThread, void *>(this, &AmbientLightingThread::setLightingOff);
private:
ScanI2C::DeviceType _type = ScanI2C::DeviceType::NONE;
private:
ScanI2C::DeviceType _type = ScanI2C::DeviceType::NONE;
// Turn RGB lighting off, is used in junction to shutdown()
int setLightingOff(void *unused) {
// Turn RGB lighting off, is used in junction to shutdown()
int setLightingOff(void *unused)
{
#ifdef HAS_NCP5623
rgb.setCurrent(0);
rgb.setRed(0);
rgb.setGreen(0);
rgb.setBlue(0);
LOG_INFO("OFF: NCP5623 Ambient lighting");
rgb.setCurrent(0);
rgb.setRed(0);
rgb.setGreen(0);
rgb.setBlue(0);
LOG_INFO("OFF: NCP5623 Ambient lighting");
#endif
#ifdef HAS_LP5562
rgbw.setCurrent(0);
rgbw.setRed(0);
rgbw.setGreen(0);
rgbw.setBlue(0);
rgbw.setWhite(0);
LOG_INFO("OFF: LP5562 Ambient lighting");
rgbw.setCurrent(0);
rgbw.setRed(0);
rgbw.setGreen(0);
rgbw.setBlue(0);
rgbw.setWhite(0);
LOG_INFO("OFF: LP5562 Ambient lighting");
#endif
#ifdef HAS_NEOPIXEL
pixels.clear();
pixels.show();
LOG_INFO("OFF: NeoPixel Ambient lighting");
pixels.clear();
pixels.show();
LOG_INFO("OFF: NeoPixel Ambient lighting");
#endif
#ifdef RGBLED_CA
analogWrite(RGBLED_RED, 255 - 0);
analogWrite(RGBLED_GREEN, 255 - 0);
analogWrite(RGBLED_BLUE, 255 - 0);
LOG_INFO("OFF: Ambient light RGB Common Anode");
analogWrite(RGBLED_RED, 255 - 0);
analogWrite(RGBLED_GREEN, 255 - 0);
analogWrite(RGBLED_BLUE, 255 - 0);
LOG_INFO("OFF: Ambient light RGB Common Anode");
#elif defined(RGBLED_RED)
analogWrite(RGBLED_RED, 0);
analogWrite(RGBLED_GREEN, 0);
analogWrite(RGBLED_BLUE, 0);
LOG_INFO("OFF: Ambient light RGB Common Cathode");
analogWrite(RGBLED_RED, 0);
analogWrite(RGBLED_GREEN, 0);
analogWrite(RGBLED_BLUE, 0);
LOG_INFO("OFF: Ambient light RGB Common Cathode");
#endif
#ifdef UNPHONE
unphone.rgb(0, 0, 0);
LOG_INFO("OFF: unPhone Ambient lighting");
unphone.rgb(0, 0, 0);
LOG_INFO("OFF: unPhone Ambient lighting");
#endif
return 0;
}
return 0;
}
void setLighting() {
void setLighting()
{
#ifdef HAS_NCP5623
rgb.setCurrent(moduleConfig.ambient_lighting.current);
rgb.setRed(moduleConfig.ambient_lighting.red);
rgb.setGreen(moduleConfig.ambient_lighting.green);
rgb.setBlue(moduleConfig.ambient_lighting.blue);
LOG_DEBUG("Init NCP5623 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);
rgb.setCurrent(moduleConfig.ambient_lighting.current);
rgb.setRed(moduleConfig.ambient_lighting.red);
rgb.setGreen(moduleConfig.ambient_lighting.green);
rgb.setBlue(moduleConfig.ambient_lighting.blue);
LOG_DEBUG("Init NCP5623 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);
#endif
#ifdef HAS_LP5562
rgbw.setCurrent(moduleConfig.ambient_lighting.current);
rgbw.setRed(moduleConfig.ambient_lighting.red);
rgbw.setGreen(moduleConfig.ambient_lighting.green);
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,
moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
rgbw.setCurrent(moduleConfig.ambient_lighting.current);
rgbw.setRed(moduleConfig.ambient_lighting.red);
rgbw.setGreen(moduleConfig.ambient_lighting.green);
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,
moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif
#ifdef HAS_NEOPIXEL
pixels.fill(pixels.Color(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue), 0,
NEOPIXEL_COUNT);
pixels.fill(pixels.Color(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green,
moduleConfig.ambient_lighting.blue),
0, NEOPIXEL_COUNT);
// RadioMaster Bandit has addressable LED at the two buttons
// this allow us to set different lighting for them in variant.h file.
#ifdef RADIOMASTER_900_BANDIT
#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
#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
pixels.show();
// 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.green, moduleConfig.ambient_lighting.blue);
pixels.show();
// 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.green, moduleConfig.ambient_lighting.blue);
#endif
#ifdef RGBLED_CA
analogWrite(RGBLED_RED, 255 - moduleConfig.ambient_lighting.red);
analogWrite(RGBLED_GREEN, 255 - moduleConfig.ambient_lighting.green);
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,
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
analogWrite(RGBLED_RED, 255 - moduleConfig.ambient_lighting.red);
analogWrite(RGBLED_GREEN, 255 - moduleConfig.ambient_lighting.green);
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,
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#elif defined(RGBLED_RED)
analogWrite(RGBLED_RED, moduleConfig.ambient_lighting.red);
analogWrite(RGBLED_GREEN, moduleConfig.ambient_lighting.green);
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,
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
analogWrite(RGBLED_RED, moduleConfig.ambient_lighting.red);
analogWrite(RGBLED_GREEN, moduleConfig.ambient_lighting.green);
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,
moduleConfig.ambient_lighting.green, moduleConfig.ambient_lighting.blue);
#endif
#ifdef UNPHONE
unphone.rgb(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green, 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,
moduleConfig.ambient_lighting.blue);
unphone.rgb(moduleConfig.ambient_lighting.red, moduleConfig.ambient_lighting.green,
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, moduleConfig.ambient_lighting.blue);
#endif
}
};
}
};
} // namespace concurrency
src/AudioThread.h
+68 -61
View File
@@ -18,86 +18,93 @@ extern ExtensionIOXL9555 io;
#define AUDIO_THREAD_INTERVAL_MS 100
class AudioThread : public concurrency::OSThread {
public:
AudioThread() : OSThread("Audio") { initOutput(); }
class AudioThread : public concurrency::OSThread
{
public:
AudioThread() : OSThread("Audio") { initOutput(); }
void beginRttl(const void *data, uint32_t len) {
void beginRttl(const void *data, uint32_t len)
{
#ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, HIGH);
io.digitalWrite(EXPANDS_AMP_EN, HIGH);
#endif
setCPUFast(true);
rtttlFile = new AudioFileSourcePROGMEM(data, len);
i2sRtttl = new AudioGeneratorRTTTL();
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;
setCPUFast(true);
rtttlFile = new AudioFileSourcePROGMEM(data, len);
i2sRtttl = new AudioGeneratorRTTTL();
i2sRtttl->begin(rtttlFile, audioOut);
}
if (rtttlFile != nullptr) {
delete rtttlFile;
rtttlFile = nullptr;
// Also handles actually playing the RTTTL, needs to be called in loop
bool isPlaying()
{
if (i2sRtttl != nullptr) {
return i2sRtttl->isRunning() && i2sRtttl->loop();
}
return false;
}
setCPUFast(false);
#ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, LOW);
#endif
}
void stop()
{
if (i2sRtttl != nullptr) {
i2sRtttl->stop();
delete i2sRtttl;
i2sRtttl = nullptr;
}
void readAloud(const char *text) {
if (i2sRtttl != nullptr) {
i2sRtttl->stop();
delete i2sRtttl;
i2sRtttl = nullptr;
if (rtttlFile != nullptr) {
delete rtttlFile;
rtttlFile = nullptr;
}
setCPUFast(false);
#ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, LOW);
#endif
}
void readAloud(const char *text)
{
if (i2sRtttl != nullptr) {
i2sRtttl->stop();
delete i2sRtttl;
i2sRtttl = nullptr;
}
#ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, HIGH);
io.digitalWrite(EXPANDS_AMP_EN, HIGH);
#endif
ESP8266SAM *sam = new ESP8266SAM;
sam->Say(audioOut, text);
delete sam;
setCPUFast(false);
ESP8266SAM *sam = new ESP8266SAM;
sam->Say(audioOut, text);
delete sam;
setCPUFast(false);
#ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, LOW);
io.digitalWrite(EXPANDS_AMP_EN, LOW);
#endif
}
}
protected:
int32_t runOnce() override {
canSleep = true; // Assume we should not keep the board awake
protected:
int32_t runOnce() override
{
canSleep = true; // Assume we should not keep the board awake
// if (i2sRtttl != nullptr && i2sRtttl->isRunning()) {
// i2sRtttl->loop();
// }
return AUDIO_THREAD_INTERVAL_MS;
}
// if (i2sRtttl != nullptr && i2sRtttl->isRunning()) {
// i2sRtttl->loop();
// }
return AUDIO_THREAD_INTERVAL_MS;
}
private:
void initOutput() {
audioOut = new AudioOutputI2S(1, AudioOutputI2S::EXTERNAL_I2S);
audioOut->SetPinout(DAC_I2S_BCK, DAC_I2S_WS, DAC_I2S_DOUT, DAC_I2S_MCLK);
audioOut->SetGain(0.2);
};
private:
void initOutput()
{
audioOut = new AudioOutputI2S(1, AudioOutputI2S::EXTERNAL_I2S);
audioOut->SetPinout(DAC_I2S_BCK, DAC_I2S_WS, DAC_I2S_DOUT, DAC_I2S_MCLK);
audioOut->SetGain(0.2);
};
AudioGeneratorRTTTL *i2sRtttl = nullptr;
AudioOutputI2S *audioOut = nullptr;
AudioGeneratorRTTTL *i2sRtttl = nullptr;
AudioOutputI2S *audioOut = nullptr;
AudioFileSourcePROGMEM *rtttlFile = nullptr;
AudioFileSourcePROGMEM *rtttlFile = nullptr;
};
#endif
src/BluetoothCommon.cpp
+12 -6
View File
@@ -3,9 +3,15 @@
// NRF52 wants these constants as byte arrays
// 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, 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 FROMRADIO_UUID_16[16u] = {0x02, 0x00, 0x12, 0xac, 0x42, 0x02, 0x78, 0xb8, 0xed, 0x11, 0x93, 0x49, 0x9e, 0xe6, 0x55, 0x2c};
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};
const uint8_t MESH_SERVICE_UUID_16[16u] = {0xfd, 0xea, 0x73, 0xe2, 0xca, 0x5d, 0xa8, 0x9f,
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 FROMRADIO_UUID_16[16u] = {0x02, 0x00, 0x12, 0xac, 0x42, 0x02, 0x78, 0xb8,
0xed, 0x11, 0x93, 0x49, 0x9e, 0xe6, 0x55, 0x2c};
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
+8 -7
View File
@@ -21,11 +21,12 @@ extern const uint8_t MESH_SERVICE_UUID_16[], TORADIO_UUID_16[16u], FROMRADIO_UUI
/// Given a level between 0-100, update the BLE attribute
void updateBatteryLevel(uint8_t level);
class BluetoothApi {
public:
virtual void setup();
virtual void shutdown();
virtual void clearBonds();
virtual bool isConnected();
virtual int getRssi() = 0;
class BluetoothApi
{
public:
virtual void setup();
virtual void shutdown();
virtual void clearBonds();
virtual bool isConnected();
virtual int getRssi() = 0;
};
src/BluetoothStatus.h
+96 -89
View File
@@ -5,106 +5,113 @@
#include "meshUtils.h"
#include <Arduino.h>
namespace meshtastic {
namespace meshtastic
{
// Describes the state of the Bluetooth connection
// Allows display to handle pairing events without each UI needing to explicitly hook the Bluefruit / NimBLE code
class BluetoothStatus : public Status {
public:
enum class ConnectionState {
DISCONNECTED,
PAIRING,
CONNECTED,
};
class BluetoothStatus : public Status
{
public:
enum class ConnectionState {
DISCONNECTED,
PAIRING,
CONNECTED,
};
private:
CallbackObserver<BluetoothStatus, const BluetoothStatus *> statusObserver =
CallbackObserver<BluetoothStatus, const BluetoothStatus *>(this, &BluetoothStatus::updateStatus);
private:
CallbackObserver<BluetoothStatus, const BluetoothStatus *> statusObserver =
CallbackObserver<BluetoothStatus, const BluetoothStatus *>(this, &BluetoothStatus::updateStatus);
ConnectionState state = ConnectionState::DISCONNECTED;
std::string passkey; // Stored as string, because Bluefruit allows passkeys with a leading zero
ConnectionState state = ConnectionState::DISCONNECTED;
std::string passkey; // Stored as string, because Bluefruit allows passkeys with a leading zero
public:
BluetoothStatus() { statusType = STATUS_TYPE_BLUETOOTH; }
public:
BluetoothStatus() { statusType = STATUS_TYPE_BLUETOOTH; }
// New BluetoothStatus: connected or disconnected
explicit BluetoothStatus(ConnectionState state) {
assert(state != ConnectionState::PAIRING); // If pairing, use constructor which specifies passkey
statusType = STATUS_TYPE_BLUETOOTH;
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: connected or disconnected
explicit BluetoothStatus(ConnectionState state)
{
assert(state != ConnectionState::PAIRING); // If pairing, use constructor which specifies passkey
statusType = STATUS_TYPE_BLUETOOTH;
this->state = state;
}
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_INVERTED
digitalWrite(BLE_LED, LOW);
#else
digitalWrite(BLE_LED, HIGH);
#endif
#endif
break;
case ConnectionState::DISCONNECTED:
LOG_DEBUG("BluetoothStatus DISCONNECTED");
#ifdef BLE_LED
#ifdef BLE_LED_INVERTED
digitalWrite(BLE_LED, HIGH);
#else
digitalWrite(BLE_LED, LOW);
#endif
#endif
break;
}
// New BluetoothStatus: pairing, with passkey
explicit BluetoothStatus(const std::string &passkey) : Status()
{
statusType = STATUS_TYPE_BLUETOOTH;
this->state = ConnectionState::PAIRING;
this->passkey = passkey;
}
return 0;
}
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;
}
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_INVERTED
digitalWrite(BLE_LED, LOW);
#else
digitalWrite(BLE_LED, HIGH);
#endif
#endif
break;
case ConnectionState::DISCONNECTED:
LOG_DEBUG("BluetoothStatus DISCONNECTED");
#ifdef BLE_LED
#ifdef BLE_LED_INVERTED
digitalWrite(BLE_LED, HIGH);
#else
digitalWrite(BLE_LED, LOW);
#endif
#endif
break;
}
}
return 0;
}
};
} // namespace meshtastic
src/DebugConfiguration.cpp
+126 -108
View File
@@ -31,150 +31,168 @@ SOFTWARE.*/
#endif
/// 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, ...) {
va_list args;
va_start(args, fmt);
if (console)
console->vprintf(level, fmt, args);
va_end(args);
extern "C" void logLegacy(const char *level, const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
if (console)
console->vprintf(level, fmt, args);
va_end(args);
}
#if HAS_NETWORKING
Syslog::Syslog(UDP &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)) {
Syslog::Syslog(UDP &client)
{
this->_client = &client;
this->_server = NULL;
} else {
this->_server = server;
}
this->_port = port;
return *this;
this->_port = 0;
this->_deviceHostname = SYSLOG_NILVALUE;
this->_appName = SYSLOG_NILVALUE;
this->_priDefault = LOGLEVEL_KERN;
}
Syslog &Syslog::server(IPAddress ip, uint16_t port) {
this->_ip = ip;
this->_server = NULL;
this->_port = port;
return *this;
Syslog &Syslog::server(const char *server, uint16_t port)
{
if (this->_ip.fromString(server)) {
this->_server = NULL;
} else {
this->_server = server;
}
this->_port = port;
return *this;
}
Syslog &Syslog::deviceHostname(const char *deviceHostname) {
this->_deviceHostname = (deviceHostname == NULL) ? SYSLOG_NILVALUE : deviceHostname;
return *this;
Syslog &Syslog::server(IPAddress ip, uint16_t port)
{
this->_ip = ip;
this->_server = NULL;
this->_port = port;
return *this;
}
Syslog &Syslog::appName(const char *appName) {
this->_appName = (appName == NULL) ? SYSLOG_NILVALUE : appName;
return *this;
Syslog &Syslog::deviceHostname(const char *deviceHostname)
{
this->_deviceHostname = (deviceHostname == NULL) ? SYSLOG_NILVALUE : deviceHostname;
return *this;
}
Syslog &Syslog::defaultPriority(uint16_t pri) {
this->_priDefault = pri;
return *this;
Syslog &Syslog::appName(const char *appName)
{
this->_appName = (appName == NULL) ? SYSLOG_NILVALUE : appName;
return *this;
}
Syslog &Syslog::logMask(uint8_t priMask) {
this->_priMask = priMask;
return *this;
Syslog &Syslog::defaultPriority(uint16_t pri)
{
this->_priDefault = pri;
return *this;
}
void Syslog::enable() {
this->_client->begin(this->_port);
this->_enabled = true;
Syslog &Syslog::logMask(uint8_t priMask)
{
this->_priMask = priMask;
return *this;
}
void Syslog::disable() {
this->_enabled = false;
this->_client->stop();
void Syslog::enable()
{
this->_client->begin(this->_port);
this->_enabled = true;
}
bool Syslog::isEnabled() { return this->_enabled; }
void Syslog::disable()
{
this->_enabled = false;
this->_client->stop();
}
bool Syslog::vlogf(uint16_t pri, const char *fmt, va_list args) { return this->vlogf(pri, this->_appName, fmt, args); }
bool Syslog::isEnabled()
{
return this->_enabled;
}
bool Syslog::vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args) {
char *message;
size_t initialLen;
size_t len;
bool result;
bool Syslog::vlogf(uint16_t pri, const char *fmt, va_list args)
{
return this->vlogf(pri, this->_appName, fmt, args);
}
initialLen = strlen(fmt);
bool Syslog::vlogf(uint16_t pri, const char *appName, const char *fmt, va_list args)
{
char *message;
size_t initialLen;
size_t len;
bool result;
message = new char[initialLen + 1];
initialLen = strlen(fmt);
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;
message = new char[len + 1];
vsnprintf(message, len + 1, fmt, args);
}
result = this->_sendLog(pri, appName, message);
delete[] message;
return result;
return result;
}
inline bool Syslog::_sendLog(uint16_t pri, const char *appName, const char *message) {
int result;
inline bool Syslog::_sendLog(uint16_t pri, const char *appName, const char *message)
{
int result;
#ifdef ARCH_PORTDUINO
bool utf = !portduino_config.ascii_logs;
bool utf = !portduino_config.ascii_logs;
#else
bool utf = true;
bool utf = true;
#endif
if (!this->_enabled)
return false;
if (!this->_enabled)
return false;
if ((this->_server == NULL && this->_ip == INADDR_NONE) || this->_port == 0)
return false;
if ((this->_server == NULL && this->_ip == INADDR_NONE) || this->_port == 0)
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;
// 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
src/DebugConfiguration.h
+33 -32
View File
@@ -74,13 +74,13 @@ extern MemGet memGet;
// Macro-based heap debugging
#define DEBUG_HEAP_BEFORE auto heapBefore = memGet.getFreeHeap();
#define DEBUG_HEAP_AFTER(context, ptr) \
do { \
auto heapAfter = memGet.getFreeHeap(); \
if (heapBefore != heapAfter) { \
LOG_HEAP("Alloc in %s pointer 0x%x, size: %u, free: %u", context, ptr, heapBefore - heapAfter, heapAfter); \
} \
} while (0)
#define DEBUG_HEAP_AFTER(context, ptr) \
do { \
auto heapAfter = memGet.getFreeHeap(); \
if (heapBefore != heapAfter) { \
LOG_HEAP("Alloc in %s pointer 0x%x, size: %u, free: %u", context, ptr, heapBefore - heapAfter, heapAfter); \
} \
} while (0)
#else
#define LOG_HEAP(...)
@@ -162,36 +162,37 @@ extern "C" void logLegacy(const char *level, const char *fmt, ...);
#if HAS_NETWORKING
class Syslog {
private:
UDP *_client;
IPAddress _ip;
const char *_server;
uint16_t _port;
const char *_deviceHostname;
const char *_appName;
uint16_t _priDefault;
uint8_t _priMask = 0xff;
bool _enabled = false;
class Syslog
{
private:
UDP *_client;
IPAddress _ip;
const char *_server;
uint16_t _port;
const char *_deviceHostname;
const char *_appName;
uint16_t _priDefault;
uint8_t _priMask = 0xff;
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:
explicit Syslog(UDP &client);
public:
explicit Syslog(UDP &client);
Syslog &server(const char *server, uint16_t port);
Syslog &server(IPAddress ip, uint16_t port);
Syslog &deviceHostname(const char *deviceHostname);
Syslog &appName(const char *appName);
Syslog &defaultPriority(uint16_t pri = LOGLEVEL_KERN);
Syslog &logMask(uint8_t priMask);
Syslog &server(const char *server, uint16_t port);
Syslog &server(IPAddress ip, uint16_t port);
Syslog &deviceHostname(const char *deviceHostname);
Syslog &appName(const char *appName);
Syslog &defaultPriority(uint16_t pri = LOGLEVEL_KERN);
Syslog &logMask(uint8_t priMask);
void enable();
void disable();
bool isEnabled();
void enable();
void disable();
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 *appName, 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)));
};
#endif // HAS_NETWORKING
src/DisplayFormatters.cpp
+79 -76
View File
@@ -1,83 +1,86 @@
#include "DisplayFormatters.h"
const char *DisplayFormatters::getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName, bool usePreset) {
const char *DisplayFormatters::getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName,
bool usePreset)
{
// If use_preset is false, always return "Custom"
if (!usePreset) {
return "Custom";
}
// If use_preset is false, always return "Custom"
if (!usePreset) {
return "Custom";
}
switch (preset) {
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_TURBO:
return useShortName ? "ShortT" : "ShortTurbo";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_SLOW:
return useShortName ? "ShortS" : "ShortSlow";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_FAST:
return useShortName ? "ShortF" : "ShortFast";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_SLOW:
return useShortName ? "MedS" : "MediumSlow";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_FAST:
return useShortName ? "MedF" : "MediumFast";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_SLOW:
return useShortName ? "LongS" : "LongSlow";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_FAST:
return useShortName ? "LongF" : "LongFast";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_TURBO:
return useShortName ? "LongT" : "LongTurbo";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_MODERATE:
return useShortName ? "LongM" : "LongMod";
break;
default:
return useShortName ? "Custom" : "Invalid";
break;
}
switch (preset) {
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_TURBO:
return useShortName ? "ShortT" : "ShortTurbo";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_SLOW:
return useShortName ? "ShortS" : "ShortSlow";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_SHORT_FAST:
return useShortName ? "ShortF" : "ShortFast";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_SLOW:
return useShortName ? "MedS" : "MediumSlow";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_MEDIUM_FAST:
return useShortName ? "MedF" : "MediumFast";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_SLOW:
return useShortName ? "LongS" : "LongSlow";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_FAST:
return useShortName ? "LongF" : "LongFast";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_TURBO:
return useShortName ? "LongT" : "LongTurbo";
break;
case meshtastic_Config_LoRaConfig_ModemPreset_LONG_MODERATE:
return useShortName ? "LongM" : "LongMod";
break;
default:
return useShortName ? "Custom" : "Invalid";
break;
}
}
const char *DisplayFormatters::getDeviceRole(meshtastic_Config_DeviceConfig_Role role) {
switch (role) {
case meshtastic_Config_DeviceConfig_Role_CLIENT:
return "Client";
break;
case meshtastic_Config_DeviceConfig_Role_CLIENT_MUTE:
return "Client Mute";
break;
case meshtastic_Config_DeviceConfig_Role_CLIENT_HIDDEN:
return "Client Hidden";
break;
case meshtastic_Config_DeviceConfig_Role_CLIENT_BASE:
return "Client Base";
break;
case meshtastic_Config_DeviceConfig_Role_LOST_AND_FOUND:
return "Lost and Found";
break;
case meshtastic_Config_DeviceConfig_Role_TRACKER:
return "Tracker";
break;
case meshtastic_Config_DeviceConfig_Role_SENSOR:
return "Sensor";
break;
case meshtastic_Config_DeviceConfig_Role_TAK:
return "TAK";
break;
case meshtastic_Config_DeviceConfig_Role_TAK_TRACKER:
return "TAK Tracker";
break;
case meshtastic_Config_DeviceConfig_Role_ROUTER:
return "Router";
break;
case meshtastic_Config_DeviceConfig_Role_ROUTER_LATE:
return "Router Late";
break;
default:
return "Unknown";
break;
}
const char *DisplayFormatters::getDeviceRole(meshtastic_Config_DeviceConfig_Role role)
{
switch (role) {
case meshtastic_Config_DeviceConfig_Role_CLIENT:
return "Client";
break;
case meshtastic_Config_DeviceConfig_Role_CLIENT_MUTE:
return "Client Mute";
break;
case meshtastic_Config_DeviceConfig_Role_CLIENT_HIDDEN:
return "Client Hidden";
break;
case meshtastic_Config_DeviceConfig_Role_CLIENT_BASE:
return "Client Base";
break;
case meshtastic_Config_DeviceConfig_Role_LOST_AND_FOUND:
return "Lost and Found";
break;
case meshtastic_Config_DeviceConfig_Role_TRACKER:
return "Tracker";
break;
case meshtastic_Config_DeviceConfig_Role_SENSOR:
return "Sensor";
break;
case meshtastic_Config_DeviceConfig_Role_TAK:
return "TAK";
break;
case meshtastic_Config_DeviceConfig_Role_TAK_TRACKER:
return "TAK Tracker";
break;
case meshtastic_Config_DeviceConfig_Role_ROUTER:
return "Router";
break;
case meshtastic_Config_DeviceConfig_Role_ROUTER_LATE:
return "Router Late";
break;
default:
return "Unknown";
break;
}
}
src/DisplayFormatters.h
+6 -4
View File
@@ -1,8 +1,10 @@
#pragma once
#include "NodeDB.h"
class DisplayFormatters {
public:
static const char *getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName, bool usePreset);
static const char *getDeviceRole(meshtastic_Config_DeviceConfig_Role role);
class DisplayFormatters
{
public:
static const char *getModemPresetDisplayName(meshtastic_Config_LoRaConfig_ModemPreset preset, bool useShortName,
bool usePreset);
static const char *getDeviceRole(meshtastic_Config_DeviceConfig_Role role);
};
src/FSCommon.cpp
+201 -193
View File
@@ -1,11 +1,11 @@
/**
* @file FSCommon.cpp
* @brief This file contains functions for common filesystem operations such as copying, renaming, listing and deleting
* files and directories.
* @brief This file contains functions for common filesystem operations such as copying, renaming, listing and deleting files and
* directories.
*
* The functions in this file are used to perform common filesystem operations such as copying, renaming, listing and
* deleting files and directories. These functions are used in the Meshtastic-device project to manage files and
* directories on the device's filesystem.
* The functions in this file are used to perform common filesystem operations such as copying, renaming, listing and deleting
* files and directories. These functions are used in the Meshtastic-device project to manage files and directories on the
* device's filesystem.
*
*/
#include "FSCommon.h"
@@ -37,33 +37,34 @@ SPIClass SPI_HSPI(HSPI);
* @param to The path of the destination file.
* @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
// take SPI Lock
concurrency::LockGuard g(spiLock);
unsigned char cbuffer[16];
// take SPI Lock
concurrency::LockGuard g(spiLock);
unsigned char cbuffer[16];
File f1 = FSCom.open(from, FILE_O_READ);
if (!f1) {
LOG_ERROR("Failed to open source file %s", from);
return false;
}
File f1 = FSCom.open(from, FILE_O_READ);
if (!f1) {
LOG_ERROR("Failed to open source file %s", from);
return false;
}
File f2 = FSCom.open(to, FILE_O_WRITE);
if (!f2) {
LOG_ERROR("Failed to open destination file %s", to);
return false;
}
File f2 = FSCom.open(to, FILE_O_WRITE);
if (!f2) {
LOG_ERROR("Failed to open destination file %s", to);
return false;
}
while (f1.available() > 0) {
byte i = f1.read(cbuffer, 16);
f2.write(cbuffer, i);
}
while (f1.available() > 0) {
byte i = f1.read(cbuffer, 16);
f2.write(cbuffer, i);
}
f2.flush();
f2.close();
f1.close();
return true;
f2.flush();
f2.close();
f1.close();
return true;
#endif
}
@@ -75,23 +76,24 @@ bool copyFile(const char *from, const char *to) {
*
* @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 ARCH_ESP32
// take SPI Lock
spiLock->lock();
// rename was fixed for ESP32 IDF LittleFS in April
bool result = FSCom.rename(pathFrom, pathTo);
spiLock->unlock();
return result;
// take SPI Lock
spiLock->lock();
// rename was fixed for ESP32 IDF LittleFS in April
bool result = FSCom.rename(pathFrom, pathTo);
spiLock->unlock();
return result;
#else
// copyFile does its own locking.
if (copyFile(pathFrom, pathTo) && FSCom.remove(pathFrom)) {
return true;
} else {
return false;
}
// copyFile does its own locking.
if (copyFile(pathFrom, pathTo) && FSCom.remove(pathFrom)) {
return true;
} else {
return false;
}
#endif
#endif
@@ -109,44 +111,45 @@ bool renameFile(const char *pathFrom, const char *pathTo) {
* @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.
*/
std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels) {
std::vector<meshtastic_FileInfo> filenames = {};
std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels)
{
std::vector<meshtastic_FileInfo> filenames = {};
#ifdef FSCom
File root = FSCom.open(dirname, FILE_O_READ);
if (!root)
return filenames;
if (!root.isDirectory())
return filenames;
File root = FSCom.open(dirname, FILE_O_READ);
if (!root)
return filenames;
if (!root.isDirectory())
return filenames;
File file = root.openNextFile();
while (file) {
if (file.isDirectory() && !String(file.name()).endsWith(".")) {
if (levels) {
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);
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.path(), levels - 1);
#else
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.name(), levels - 1);
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())};
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());
strcpy(fileInfo.file_name, file.path());
#else
strcpy(fileInfo.file_name, file.name());
strcpy(fileInfo.file_name, file.name());
#endif
if (!String(fileInfo.file_name).endsWith(".")) {
filenames.push_back(fileInfo);
}
file.close();
if (!String(fileInfo.file_name).endsWith(".")) {
filenames.push_back(fileInfo);
}
file.close();
}
file = root.openNextFile();
}
file = root.openNextFile();
}
root.close();
root.close();
#endif
return filenames;
return filenames;
}
/**
@@ -157,98 +160,100 @@ std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels) {
* @param levels The number of levels of subdirectories to list.
* @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
#if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
char buffer[255];
#endif
File root = FSCom.open(dirname, FILE_O_READ);
if (!root) {
return;
}
if (!root.isDirectory()) {
return;
}
File file = root.openNextFile();
while (file && 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 (levels) {
#ifdef ARCH_ESP32
listDir(file.path(), levels - 1, del);
if (del) {
LOG_DEBUG("Remove %s", file.path());
strncpy(buffer, file.path(), sizeof(buffer));
file.close();
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 {
#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();
char buffer[255];
#endif
File root = FSCom.open(dirname, FILE_O_READ);
if (!root) {
return;
}
file = root.openNextFile();
}
if (!root.isDirectory()) {
return;
}
File file = root.openNextFile();
while (
file &&
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 (levels) {
#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();
}
listDir(file.path(), levels - 1, del);
if (del) {
LOG_DEBUG("Remove %s", file.path());
strncpy(buffer, file.path(), sizeof(buffer));
file.close();
FSCom.rmdir(buffer);
} else {
file.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();
}
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
root.close();
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();
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
}
@@ -260,14 +265,15 @@ void listDir(const char *dirname, uint8_t levels, bool del) {
*
* @param dirname The name of the directory to remove.
*/
void rmDir(const char *dirname) {
void rmDir(const char *dirname)
{
#ifdef FSCom
#if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
listDir(dirname, 10, true);
listDir(dirname, 10, true);
#elif defined(ARCH_NRF52)
// nRF52 implementation of LittleFS has a recursive delete function
FSCom.rmdir_r(dirname);
// nRF52 implementation of LittleFS has a recursive delete function
FSCom.rmdir_r(dirname);
#endif
#endif
@@ -278,53 +284,55 @@ void rmDir(const char *dirname) {
*/
__attribute__((weak, noinline)) void preFSBegin() {}
void fsInit() {
void fsInit()
{
#ifdef FSCom
concurrency::LockGuard g(spiLock);
preFSBegin();
if (!FSBegin()) {
LOG_ERROR("Filesystem mount failed");
// assert(0); This auto-formats the partition, so no need to fail here.
}
concurrency::LockGuard g(spiLock);
preFSBegin();
if (!FSBegin()) {
LOG_ERROR("Filesystem mount failed");
// assert(0); This auto-formats the partition, so no need to fail here.
}
#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
LOG_DEBUG("Filesystem files:");
LOG_DEBUG("Filesystem files:");
#endif
listDir("/", 10);
listDir("/", 10);
#endif
}
/**
* Initializes the SD card and mounts the file system.
*/
void setupSDCard() {
void setupSDCard()
{
#if defined(HAS_SDCARD) && !defined(SDCARD_USE_SOFT_SPI)
concurrency::LockGuard g(spiLock);
SDHandler.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
if (!SD.begin(SDCARD_CS, SDHandler, SD_SPI_FREQUENCY)) {
LOG_DEBUG("No SD_MMC card detected");
return;
}
uint8_t cardType = SD.cardType();
if (cardType == CARD_NONE) {
LOG_DEBUG("No SD_MMC card attached");
return;
}
LOG_DEBUG("SD_MMC Card Type: ");
if (cardType == CARD_MMC) {
LOG_DEBUG("MMC");
} else if (cardType == CARD_SD) {
LOG_DEBUG("SDSC");
} else if (cardType == CARD_SDHC) {
LOG_DEBUG("SDHC");
} else {
LOG_DEBUG("UNKNOWN");
}
concurrency::LockGuard g(spiLock);
SDHandler.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
if (!SD.begin(SDCARD_CS, SDHandler, SD_SPI_FREQUENCY)) {
LOG_DEBUG("No SD_MMC card detected");
return;
}
uint8_t cardType = SD.cardType();
if (cardType == CARD_NONE) {
LOG_DEBUG("No SD_MMC card attached");
return;
}
LOG_DEBUG("SD_MMC Card Type: ");
if (cardType == CARD_MMC) {
LOG_DEBUG("MMC");
} else if (cardType == CARD_SD) {
LOG_DEBUG("SDSC");
} else if (cardType == CARD_SDHC) {
LOG_DEBUG("SDHC");
} else {
LOG_DEBUG("UNKNOWN");
}
uint64_t cardSize = SD.cardSize() / (1024 * 1024);
LOG_DEBUG("SD Card Size: %lu MB", (uint32_t)cardSize);
LOG_DEBUG("Total space: %lu MB", (uint32_t)(SD.totalBytes() / (1024 * 1024)));
LOG_DEBUG("Used space: %lu MB", (uint32_t)(SD.usedBytes() / (1024 * 1024)));
uint64_t cardSize = SD.cardSize() / (1024 * 1024);
LOG_DEBUG("SD Card Size: %lu MB", (uint32_t)cardSize);
LOG_DEBUG("Total space: %lu MB", (uint32_t)(SD.totalBytes() / (1024 * 1024)));
LOG_DEBUG("Used space: %lu MB", (uint32_t)(SD.usedBytes() / (1024 * 1024)));
#endif
}
src/Fusion/FusionAhrs.c
+327 -295
View File
@@ -43,17 +43,18 @@ static inline int Clamp(const int value, const int min, const int max);
* @brief Initialises the AHRS algorithm structure.
* @param ahrs AHRS algorithm structure.
*/
void FusionAhrsInitialise(FusionAhrs *const ahrs) {
const FusionAhrsSettings settings = {
.convention = FusionConventionNwu,
.gain = 0.5f,
.gyroscopeRange = 0.0f,
.accelerationRejection = 90.0f,
.magneticRejection = 90.0f,
.recoveryTriggerPeriod = 0,
};
FusionAhrsSetSettings(ahrs, &settings);
FusionAhrsReset(ahrs);
void FusionAhrsInitialise(FusionAhrs *const ahrs)
{
const FusionAhrsSettings settings = {
.convention = FusionConventionNwu,
.gain = 0.5f,
.gyroscopeRange = 0.0f,
.accelerationRejection = 90.0f,
.magneticRejection = 90.0f,
.recoveryTriggerPeriod = 0,
};
FusionAhrsSetSettings(ahrs, &settings);
FusionAhrsReset(ahrs);
}
/**
@@ -61,20 +62,21 @@ void FusionAhrsInitialise(FusionAhrs *const ahrs) {
* algorithm while maintaining the current settings.
* @param ahrs AHRS algorithm structure.
*/
void FusionAhrsReset(FusionAhrs *const ahrs) {
ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
ahrs->accelerometer = FUSION_VECTOR_ZERO;
ahrs->initialising = true;
ahrs->rampedGain = INITIAL_GAIN;
ahrs->angularRateRecovery = false;
ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
ahrs->accelerometerIgnored = false;
ahrs->accelerationRecoveryTrigger = 0;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magnetometerIgnored = false;
ahrs->magneticRecoveryTrigger = 0;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
void FusionAhrsReset(FusionAhrs *const ahrs)
{
ahrs->quaternion = FUSION_IDENTITY_QUATERNION;
ahrs->accelerometer = FUSION_VECTOR_ZERO;
ahrs->initialising = true;
ahrs->rampedGain = INITIAL_GAIN;
ahrs->angularRateRecovery = false;
ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO;
ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO;
ahrs->accelerometerIgnored = false;
ahrs->accelerationRecoveryTrigger = 0;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magnetometerIgnored = false;
ahrs->magneticRecoveryTrigger = 0;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
}
/**
@@ -82,25 +84,28 @@ void FusionAhrsReset(FusionAhrs *const ahrs) {
* @param ahrs AHRS algorithm structure.
* @param settings Settings.
*/
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) {
ahrs->settings.convention = settings->convention;
ahrs->settings.gain = settings->gain;
ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
ahrs->settings.accelerationRejection =
settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
ahrs->settings.magneticRejection =
settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
ahrs->settings.accelerationRejection = FLT_MAX;
ahrs->settings.magneticRejection = FLT_MAX;
}
if (ahrs->initialising == false) {
ahrs->rampedGain = ahrs->settings.gain;
}
ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings)
{
ahrs->settings.convention = settings->convention;
ahrs->settings.gain = settings->gain;
ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange;
ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f
? FLT_MAX
: powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2);
ahrs->settings.magneticRejection =
settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2);
ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod;
ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
if ((settings->gain == 0.0f) ||
(settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero
ahrs->settings.accelerationRejection = FLT_MAX;
ahrs->settings.magneticRejection = FLT_MAX;
}
if (ahrs->initialising == false) {
ahrs->rampedGain = ahrs->settings.gain;
}
ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD;
}
/**
@@ -112,113 +117,119 @@ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *con
* @param magnetometer Magnetometer measurement in arbitrary units.
* @param deltaTime Delta time in seconds.
*/
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer,
const float deltaTime) {
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const FusionVector magnetometer, const float deltaTime)
{
#define Q ahrs->quaternion.element
// Store accelerometer
ahrs->accelerometer = accelerometer;
// Store accelerometer
ahrs->accelerometer = accelerometer;
// Reinitialise if gyroscope range exceeded
if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) ||
(fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
const FusionQuaternion quaternion = ahrs->quaternion;
FusionAhrsReset(ahrs);
ahrs->quaternion = quaternion;
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;
// Reinitialise if gyroscope range exceeded
if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) ||
(fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) {
const FusionQuaternion quaternion = ahrs->quaternion;
FusionAhrsReset(ahrs);
ahrs->quaternion = quaternion;
ahrs->angularRateRecovery = true;
}
// 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;
}
}
// 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;
// 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;
}
}
// Don't ignore magnetometer during magnetic recovery
if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) {
ahrs->magneticRecoveryTimeout = 0;
ahrs->magnetometerIgnored = false;
} else {
ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod;
// 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;
}
// 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);
// Apply magnetometer feedback
if (ahrs->magnetometerIgnored == false) {
halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback;
// 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;
}
// 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
const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
// Convert gyroscope to radians per second scaled by 0.5
const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f));
// Apply feedback to gyroscope
const FusionVector adjustedHalfGyroscope = FusionVectorAdd(
halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
// Apply feedback to gyroscope
const FusionVector adjustedHalfGyroscope = FusionVectorAdd(
halfGyroscope,
FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain));
// Integrate rate of change of quaternion
ahrs->quaternion = FusionQuaternionAdd(
ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
// Integrate rate of change of quaternion
ahrs->quaternion = FusionQuaternionAdd(
ahrs->quaternion,
FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime)));
// Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
// Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
#undef Q
}
@@ -227,28 +238,29 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons
* @param ahrs AHRS algorithm structure.
* @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
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu: {
const FusionVector halfGravity = {.axis = {
.x = Q.x * Q.z - Q.w * Q.y,
.y = Q.y * Q.z + Q.w * Q.x,
.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by 0.5
return halfGravity;
}
case FusionConventionNed: {
const FusionVector halfGravity = {.axis = {
.x = Q.w * Q.y - Q.x * Q.z,
.y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 0.5f - Q.w * Q.w - Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by -0.5
return halfGravity;
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu: {
const FusionVector halfGravity = {.axis = {
.x = Q.x * Q.z - Q.w * Q.y,
.y = Q.y * Q.z + Q.w * Q.x,
.z = Q.w * Q.w - 0.5f + Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by 0.5
return halfGravity;
}
case FusionConventionNed: {
const FusionVector halfGravity = {.axis = {
.x = Q.w * Q.y - Q.x * Q.z,
.y = -1.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 0.5f - Q.w * Q.w - Q.z * Q.z,
}}; // third column of transposed rotation matrix scaled by -0.5
return halfGravity;
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q
}
@@ -257,35 +269,36 @@ static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) {
* @param ahrs AHRS algorithm structure.
* @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
switch (ahrs->settings.convention) {
case FusionConventionNwu: {
const FusionVector halfMagnetic = {.axis = {
.x = Q.x * Q.y + Q.w * Q.z,
.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
.z = Q.y * Q.z - Q.w * Q.x,
}}; // second column of transposed rotation matrix scaled by 0.5
return halfMagnetic;
}
case FusionConventionEnu: {
const FusionVector halfMagnetic = {.axis = {
.x = 0.5f - Q.w * Q.w - Q.x * Q.x,
.y = Q.w * Q.z - Q.x * Q.y,
.z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
}}; // first column of transposed rotation matrix scaled by -0.5
return halfMagnetic;
}
case FusionConventionNed: {
const FusionVector halfMagnetic = {.axis = {
.x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
.y = 0.5f - Q.w * Q.w - Q.y * Q.y,
.z = Q.w * Q.x - Q.y * Q.z,
}}; // second column of transposed rotation matrix scaled by -0.5
return halfMagnetic;
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
switch (ahrs->settings.convention) {
case FusionConventionNwu: {
const FusionVector halfMagnetic = {.axis = {
.x = Q.x * Q.y + Q.w * Q.z,
.y = Q.w * Q.w - 0.5f + Q.y * Q.y,
.z = Q.y * Q.z - Q.w * Q.x,
}}; // second column of transposed rotation matrix scaled by 0.5
return halfMagnetic;
}
case FusionConventionEnu: {
const FusionVector halfMagnetic = {.axis = {
.x = 0.5f - Q.w * Q.w - Q.x * Q.x,
.y = Q.w * Q.z - Q.x * Q.y,
.z = -1.0f * (Q.x * Q.z + Q.w * Q.y),
}}; // first column of transposed rotation matrix scaled by -0.5
return halfMagnetic;
}
case FusionConventionNed: {
const FusionVector halfMagnetic = {.axis = {
.x = -1.0f * (Q.x * Q.y + Q.w * Q.z),
.y = 0.5f - Q.w * Q.w - Q.y * Q.y,
.z = Q.w * Q.x - Q.y * Q.z,
}}; // second column of transposed rotation matrix scaled by -0.5
return halfMagnetic;
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q
}
@@ -295,11 +308,12 @@ static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) {
* @param reference Reference.
* @return Feedback.
*/
static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) {
if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
}
return FusionVectorCrossProduct(sensor, reference);
static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference)
{
if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees
return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference));
}
return FusionVectorCrossProduct(sensor, reference);
}
/**
@@ -309,14 +323,15 @@ static inline FusionVector Feedback(const FusionVector sensor, const FusionVecto
* @param max Maximum value.
* @return Value limited to maximum and minimum.
*/
static inline int Clamp(const int value, const int min, const int max) {
if (value < min) {
return min;
}
if (value > max) {
return max;
}
return value;
static inline int Clamp(const int value, const int min, const int max)
{
if (value < min) {
return min;
}
if (value > max) {
return max;
}
return value;
}
/**
@@ -327,15 +342,17 @@ static inline int Clamp(const int value, const int min, const int max) {
* @param accelerometer Accelerometer measurement in g.
* @param deltaTime Delta time in seconds.
*/
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) {
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float deltaTime)
{
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
// Zero heading during initialisation
if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f);
}
// Zero heading during initialisation
if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f);
}
}
/**
@@ -347,24 +364,25 @@ void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector g
* @param heading Heading measurement in degrees.
* @param deltaTime Delta time in seconds.
*/
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading,
const float deltaTime) {
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float heading, const float deltaTime)
{
#define Q ahrs->quaternion.element
// Calculate roll
const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);
// Calculate roll
const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x);
// Calculate magnetometer
const float headingRadians = FusionDegreesToRadians(heading);
const float sinHeadingRadians = sinf(headingRadians);
const FusionVector magnetometer = {.axis = {
.x = cosf(headingRadians),
.y = -1.0f * cosf(roll) * sinHeadingRadians,
.z = sinHeadingRadians * sinf(roll),
}};
// Calculate magnetometer
const float headingRadians = FusionDegreesToRadians(heading);
const float sinHeadingRadians = sinf(headingRadians);
const FusionVector magnetometer = {.axis = {
.x = cosf(headingRadians),
.y = -1.0f * cosf(roll) * sinHeadingRadians,
.z = sinHeadingRadians * sinf(roll),
}};
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime);
#undef Q
}
@@ -373,14 +391,20 @@ void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector
* @param ahrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth.
*/
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { return ahrs->quaternion; }
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs)
{
return ahrs->quaternion;
}
/**
* @brief Sets the quaternion describing the sensor relative to the Earth.
* @param ahrs AHRS algorithm structure.
* @param quaternion Quaternion describing the sensor relative to the Earth.
*/
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) { ahrs->quaternion = quaternion; }
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion)
{
ahrs->quaternion = quaternion;
}
/**
* @brief Returns the linear acceleration measurement equal to the accelerometer
@@ -388,27 +412,28 @@ void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quat
* @param ahrs AHRS algorithm structure.
* @return Linear acceleration measurement in g.
*/
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) {
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs)
{
#define Q ahrs->quaternion.element
// Calculate gravity in the sensor coordinate frame
const FusionVector gravity = {.axis = {
.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
.y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
}}; // third column of transposed rotation matrix
// Calculate gravity in the sensor coordinate frame
const FusionVector gravity = {.axis = {
.x = 2.0f * (Q.x * Q.z - Q.w * Q.y),
.y = 2.0f * (Q.y * Q.z + Q.w * Q.x),
.z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z),
}}; // third column of transposed rotation matrix
// Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu: {
return FusionVectorSubtract(ahrs->accelerometer, gravity);
}
case FusionConventionNed: {
return FusionVectorAdd(ahrs->accelerometer, gravity);
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
// Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu: {
return FusionVectorSubtract(ahrs->accelerometer, gravity);
}
case FusionConventionNed: {
return FusionVectorAdd(ahrs->accelerometer, gravity);
}
}
return FUSION_VECTOR_ZERO; // avoid compiler warning
#undef Q
}
@@ -418,35 +443,36 @@ FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) {
* @param ahrs AHRS algorithm structure.
* @return Earth acceleration measurement in g.
*/
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) {
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs)
{
#define Q ahrs->quaternion.element
#define A ahrs->accelerometer.axis
// Calculate accelerometer measurement in the Earth coordinate frame
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
FusionVector accelerometer = {.axis = {
.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),
.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
// Calculate accelerometer measurement in the Earth coordinate frame
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
FusionVector accelerometer = {.axis = {
.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),
.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
// Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu:
accelerometer.axis.z -= 1.0f;
break;
case FusionConventionNed:
accelerometer.axis.z += 1.0f;
break;
}
return accelerometer;
// Remove gravity from accelerometer measurement
switch (ahrs->settings.convention) {
case FusionConventionNwu:
case FusionConventionEnu:
accelerometer.axis.z -= 1.0f;
break;
case FusionConventionNed:
accelerometer.axis.z += 1.0f;
break;
}
return accelerometer;
#undef Q
#undef A
}
@@ -456,18 +482,22 @@ FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) {
* @param ahrs AHRS algorithm structure.
* @return AHRS algorithm internal states.
*/
FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) {
const FusionAhrsInternalStates internalStates = {
.accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
.accelerometerIgnored = ahrs->accelerometerIgnored,
.accelerationRecoveryTrigger =
ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
.magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
.magnetometerIgnored = ahrs->magnetometerIgnored,
.magneticRecoveryTrigger =
ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
};
return internalStates;
FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs)
{
const FusionAhrsInternalStates internalStates = {
.accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))),
.accelerometerIgnored = ahrs->accelerometerIgnored,
.accelerationRecoveryTrigger =
ahrs->settings.recoveryTriggerPeriod == 0
? 0.0f
: (float)ahrs->accelerationRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
.magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))),
.magnetometerIgnored = ahrs->magnetometerIgnored,
.magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0
? 0.0f
: (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod,
};
return internalStates;
}
/**
@@ -475,14 +505,15 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr
* @param ahrs AHRS algorithm structure.
* @return AHRS algorithm flags.
*/
FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) {
const FusionAhrsFlags flags = {
.initialising = ahrs->initialising,
.angularRateRecovery = ahrs->angularRateRecovery,
.accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
.magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
};
return flags;
FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs)
{
const FusionAhrsFlags flags = {
.initialising = ahrs->initialising,
.angularRateRecovery = ahrs->angularRateRecovery,
.accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout,
.magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout,
};
return flags;
}
/**
@@ -492,17 +523,18 @@ FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) {
* @param ahrs AHRS algorithm structure.
* @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
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 FusionQuaternion rotation = {.element = {
.w = cosf(halfYawMinusHeading),
.x = 0.0f,
.y = 0.0f,
.z = -1.0f * sinf(halfYawMinusHeading),
}};
ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
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 FusionQuaternion rotation = {.element = {
.w = cosf(halfYawMinusHeading),
.x = 0.0f,
.y = 0.0f,
.z = -1.0f * sinf(halfYawMinusHeading),
}};
ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion);
#undef Q
}
src/Fusion/FusionAhrs.h
+37 -36
View File
@@ -22,12 +22,12 @@
* @brief AHRS algorithm settings.
*/
typedef struct {
FusionConvention convention;
float gain;
float gyroscopeRange;
float accelerationRejection;
float magneticRejection;
unsigned int recoveryTriggerPeriod;
FusionConvention convention;
float gain;
float gyroscopeRange;
float accelerationRejection;
float magneticRejection;
unsigned int recoveryTriggerPeriod;
} FusionAhrsSettings;
/**
@@ -35,43 +35,43 @@ typedef struct {
* must not be accessed by the application.
*/
typedef struct {
FusionAhrsSettings settings;
FusionQuaternion quaternion;
FusionVector accelerometer;
bool initialising;
float rampedGain;
float rampedGainStep;
bool angularRateRecovery;
FusionVector halfAccelerometerFeedback;
FusionVector halfMagnetometerFeedback;
bool accelerometerIgnored;
int accelerationRecoveryTrigger;
int accelerationRecoveryTimeout;
bool magnetometerIgnored;
int magneticRecoveryTrigger;
int magneticRecoveryTimeout;
FusionAhrsSettings settings;
FusionQuaternion quaternion;
FusionVector accelerometer;
bool initialising;
float rampedGain;
float rampedGainStep;
bool angularRateRecovery;
FusionVector halfAccelerometerFeedback;
FusionVector halfMagnetometerFeedback;
bool accelerometerIgnored;
int accelerationRecoveryTrigger;
int accelerationRecoveryTimeout;
bool magnetometerIgnored;
int magneticRecoveryTrigger;
int magneticRecoveryTimeout;
} FusionAhrs;
/**
* @brief AHRS algorithm internal states.
*/
typedef struct {
float accelerationError;
bool accelerometerIgnored;
float accelerationRecoveryTrigger;
float magneticError;
bool magnetometerIgnored;
float magneticRecoveryTrigger;
float accelerationError;
bool accelerometerIgnored;
float accelerationRecoveryTrigger;
float magneticError;
bool magnetometerIgnored;
float magneticRecoveryTrigger;
} FusionAhrsInternalStates;
/**
* @brief AHRS algorithm flags.
*/
typedef struct {
bool initialising;
bool angularRateRecovery;
bool accelerationRecovery;
bool magneticRecovery;
bool initialising;
bool angularRateRecovery;
bool accelerationRecovery;
bool magneticRecovery;
} FusionAhrsFlags;
//------------------------------------------------------------------------------
@@ -83,13 +83,14 @@ void FusionAhrsReset(FusionAhrs *const ahrs);
void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings);
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer,
const float deltaTime);
void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const FusionVector magnetometer, const float deltaTime);
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime);
void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float deltaTime);
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading,
const float deltaTime);
void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer,
const float heading, const float deltaTime);
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);
src/Fusion/FusionAxes.h
+147 -146
View File
@@ -22,30 +22,30 @@
* then alignment is +Y-X+Z.
*/
typedef enum {
FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */
FusionAxesAlignmentPXNZPY, /* +X-Z+Y */
FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */
FusionAxesAlignmentPXPZNY, /* +X+Z-Y */
FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */
FusionAxesAlignmentNXPZPY, /* -X+Z+Y */
FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */
FusionAxesAlignmentNXNZNY, /* -X-Z-Y */
FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */
FusionAxesAlignmentPYNZNX, /* +Y-Z-X */
FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */
FusionAxesAlignmentPYPZPX, /* +Y+Z+X */
FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */
FusionAxesAlignmentNYNZPX, /* -Y-Z+X */
FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */
FusionAxesAlignmentNYPZNX, /* -Y+Z-X */
FusionAxesAlignmentPZPYNX, /* +Z+Y-X */
FusionAxesAlignmentPZPXPY, /* +Z+X+Y */
FusionAxesAlignmentPZNYPX, /* +Z-Y+X */
FusionAxesAlignmentPZNXNY, /* +Z-X-Y */
FusionAxesAlignmentNZPYPX, /* -Z+Y+X */
FusionAxesAlignmentNZNXPY, /* -Z-X+Y */
FusionAxesAlignmentNZNYNX, /* -Z-Y-X */
FusionAxesAlignmentNZPXNY, /* -Z+X-Y */
FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */
FusionAxesAlignmentPXNZPY, /* +X-Z+Y */
FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */
FusionAxesAlignmentPXPZNY, /* +X+Z-Y */
FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */
FusionAxesAlignmentNXPZPY, /* -X+Z+Y */
FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */
FusionAxesAlignmentNXNZNY, /* -X-Z-Y */
FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */
FusionAxesAlignmentPYNZNX, /* +Y-Z-X */
FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */
FusionAxesAlignmentPYPZPX, /* +Y+Z+X */
FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */
FusionAxesAlignmentNYNZPX, /* -Y-Z+X */
FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */
FusionAxesAlignmentNYPZNX, /* -Y+Z-X */
FusionAxesAlignmentPZPYNX, /* +Z+Y-X */
FusionAxesAlignmentPZPXPY, /* +Z+X+Y */
FusionAxesAlignmentPZNYPX, /* +Z-Y+X */
FusionAxesAlignmentPZNXNY, /* +Z-X-Y */
FusionAxesAlignmentNZPYPX, /* -Z+Y+X */
FusionAxesAlignmentNZNXPY, /* -Z-X+Y */
FusionAxesAlignmentNZNYNX, /* -Z-Y-X */
FusionAxesAlignmentNZPXNY, /* -Z+X-Y */
} FusionAxesAlignment;
//------------------------------------------------------------------------------
@@ -57,128 +57,129 @@ typedef enum {
* @param alignment Axes alignment.
* @return Sensor axes aligned with the body axes.
*/
static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) {
FusionVector result;
switch (alignment) {
case FusionAxesAlignmentPXPYPZ:
break;
case FusionAxesAlignmentPXNZPY:
result.axis.x = +sensor.axis.x;
result.axis.y = -sensor.axis.z;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentPXNYNZ:
result.axis.x = +sensor.axis.x;
result.axis.y = -sensor.axis.y;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentPXPZNY:
result.axis.x = +sensor.axis.x;
result.axis.y = +sensor.axis.z;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentNXPYNZ:
result.axis.x = -sensor.axis.x;
result.axis.y = +sensor.axis.y;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentNXPZPY:
result.axis.x = -sensor.axis.x;
result.axis.y = +sensor.axis.z;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentNXNYPZ:
result.axis.x = -sensor.axis.x;
result.axis.y = -sensor.axis.y;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentNXNZNY:
result.axis.x = -sensor.axis.x;
result.axis.y = -sensor.axis.z;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentPYNXPZ:
result.axis.x = +sensor.axis.y;
result.axis.y = -sensor.axis.x;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentPYNZNX:
result.axis.x = +sensor.axis.y;
result.axis.y = -sensor.axis.z;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPYPXNZ:
result.axis.x = +sensor.axis.y;
result.axis.y = +sensor.axis.x;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentPYPZPX:
result.axis.x = +sensor.axis.y;
result.axis.y = +sensor.axis.z;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNYPXPZ:
result.axis.x = -sensor.axis.y;
result.axis.y = +sensor.axis.x;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentNYNZPX:
result.axis.x = -sensor.axis.y;
result.axis.y = -sensor.axis.z;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNYNXNZ:
result.axis.x = -sensor.axis.y;
result.axis.y = -sensor.axis.x;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentNYPZNX:
result.axis.x = -sensor.axis.y;
result.axis.y = +sensor.axis.z;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPZPYNX:
result.axis.x = +sensor.axis.z;
result.axis.y = +sensor.axis.y;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPZPXPY:
result.axis.x = +sensor.axis.z;
result.axis.y = +sensor.axis.x;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentPZNYPX:
result.axis.x = +sensor.axis.z;
result.axis.y = -sensor.axis.y;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentPZNXNY:
result.axis.x = +sensor.axis.z;
result.axis.y = -sensor.axis.x;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentNZPYPX:
result.axis.x = -sensor.axis.z;
result.axis.y = +sensor.axis.y;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNZNXPY:
result.axis.x = -sensor.axis.z;
result.axis.y = -sensor.axis.x;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentNZNYNX:
result.axis.x = -sensor.axis.z;
result.axis.y = -sensor.axis.y;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentNZPXNY:
result.axis.x = -sensor.axis.z;
result.axis.y = +sensor.axis.x;
result.axis.z = -sensor.axis.y;
return result;
}
return sensor; // avoid compiler warning
static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment)
{
FusionVector result;
switch (alignment) {
case FusionAxesAlignmentPXPYPZ:
break;
case FusionAxesAlignmentPXNZPY:
result.axis.x = +sensor.axis.x;
result.axis.y = -sensor.axis.z;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentPXNYNZ:
result.axis.x = +sensor.axis.x;
result.axis.y = -sensor.axis.y;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentPXPZNY:
result.axis.x = +sensor.axis.x;
result.axis.y = +sensor.axis.z;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentNXPYNZ:
result.axis.x = -sensor.axis.x;
result.axis.y = +sensor.axis.y;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentNXPZPY:
result.axis.x = -sensor.axis.x;
result.axis.y = +sensor.axis.z;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentNXNYPZ:
result.axis.x = -sensor.axis.x;
result.axis.y = -sensor.axis.y;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentNXNZNY:
result.axis.x = -sensor.axis.x;
result.axis.y = -sensor.axis.z;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentPYNXPZ:
result.axis.x = +sensor.axis.y;
result.axis.y = -sensor.axis.x;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentPYNZNX:
result.axis.x = +sensor.axis.y;
result.axis.y = -sensor.axis.z;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPYPXNZ:
result.axis.x = +sensor.axis.y;
result.axis.y = +sensor.axis.x;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentPYPZPX:
result.axis.x = +sensor.axis.y;
result.axis.y = +sensor.axis.z;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNYPXPZ:
result.axis.x = -sensor.axis.y;
result.axis.y = +sensor.axis.x;
result.axis.z = +sensor.axis.z;
return result;
case FusionAxesAlignmentNYNZPX:
result.axis.x = -sensor.axis.y;
result.axis.y = -sensor.axis.z;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNYNXNZ:
result.axis.x = -sensor.axis.y;
result.axis.y = -sensor.axis.x;
result.axis.z = -sensor.axis.z;
return result;
case FusionAxesAlignmentNYPZNX:
result.axis.x = -sensor.axis.y;
result.axis.y = +sensor.axis.z;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPZPYNX:
result.axis.x = +sensor.axis.z;
result.axis.y = +sensor.axis.y;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentPZPXPY:
result.axis.x = +sensor.axis.z;
result.axis.y = +sensor.axis.x;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentPZNYPX:
result.axis.x = +sensor.axis.z;
result.axis.y = -sensor.axis.y;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentPZNXNY:
result.axis.x = +sensor.axis.z;
result.axis.y = -sensor.axis.x;
result.axis.z = -sensor.axis.y;
return result;
case FusionAxesAlignmentNZPYPX:
result.axis.x = -sensor.axis.z;
result.axis.y = +sensor.axis.y;
result.axis.z = +sensor.axis.x;
return result;
case FusionAxesAlignmentNZNXPY:
result.axis.x = -sensor.axis.z;
result.axis.y = -sensor.axis.x;
result.axis.z = +sensor.axis.y;
return result;
case FusionAxesAlignmentNZNYNX:
result.axis.x = -sensor.axis.z;
result.axis.y = -sensor.axis.y;
result.axis.z = -sensor.axis.x;
return result;
case FusionAxesAlignmentNZPXNY:
result.axis.x = -sensor.axis.z;
result.axis.y = +sensor.axis.x;
result.axis.z = -sensor.axis.y;
return result;
}
return sensor; // avoid compiler warning
}
#endif
src/Fusion/FusionCalibration.h
+8 -5
View File
@@ -23,9 +23,11 @@
* @param offset Offset.
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity,
const FusionVector offset) {
return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment,
const FusionVector sensitivity, const FusionVector offset)
{
return FusionMatrixMultiplyVector(misalignment,
FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity));
}
/**
@@ -36,8 +38,9 @@ static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibr
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
const FusionVector hardIronOffset) {
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
const FusionVector hardIronOffset)
{
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
}
#endif
src/Fusion/FusionCompass.c
+23 -21
View File
@@ -22,27 +22,29 @@
* @param magnetometer Magnetometer measurement in any calibrated units.
* @return Heading angle in degrees.
*/
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer) {
switch (convention) {
case FusionConventionNwu: {
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
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));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
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);
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
}
}
return 0; // avoid compiler warning
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
const FusionVector magnetometer)
{
switch (convention) {
case FusionConventionNwu: {
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer));
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));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer));
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);
const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer));
const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up));
return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x));
}
}
return 0; // avoid compiler warning
}
//------------------------------------------------------------------------------
src/Fusion/FusionCompass.h
+2 -1
View File
@@ -17,7 +17,8 @@
//------------------------------------------------------------------------------
// Function declarations
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer);
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
const FusionVector magnetometer);
#endif
src/Fusion/FusionConvention.h
+3 -3
View File
@@ -14,9 +14,9 @@
* @brief Earth axes convention.
*/
typedef enum {
FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */
FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */
} FusionConvention;
#endif
src/Fusion/FusionMath.h
+196 -164
View File
@@ -21,27 +21,27 @@
* @brief 3D vector.
*/
typedef union {
float array[3];
float array[3];
struct {
float x;
float y;
float z;
} axis;
struct {
float x;
float y;
float z;
} axis;
} FusionVector;
/**
* @brief Quaternion.
*/
typedef union {
float array[4];
float array[4];
struct {
float w;
float x;
float y;
float z;
} element;
struct {
float w;
float x;
float y;
float z;
} element;
} FusionQuaternion;
/**
@@ -49,19 +49,19 @@ typedef union {
* See http://en.wikipedia.org/wiki/Row-major_order
*/
typedef union {
float array[3][3];
float array[3][3];
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
} FusionMatrix;
/**
@@ -69,13 +69,13 @@ typedef union {
* X, Y, and Z respectively.
*/
typedef union {
float array[3];
float array[3];
struct {
float roll;
float pitch;
float yaw;
} angle;
struct {
float roll;
float pitch;
float yaw;
} angle;
} FusionEuler;
/**
@@ -124,14 +124,20 @@ typedef union {
* @param degrees Degrees.
* @return Radians.
*/
static inline float FusionDegreesToRadians(const float degrees) { return degrees * ((float)M_PI / 180.0f); }
static inline float FusionDegreesToRadians(const float degrees)
{
return degrees * ((float)M_PI / 180.0f);
}
/**
* @brief Converts radians to degrees.
* @param radians Radians.
* @return Degrees.
*/
static inline float FusionRadiansToDegrees(const float radians) { return radians * (180.0f / (float)M_PI); }
static inline float FusionRadiansToDegrees(const float radians)
{
return radians * (180.0f / (float)M_PI);
}
//------------------------------------------------------------------------------
// Inline functions - Arc sine
@@ -141,14 +147,15 @@ static inline float FusionRadiansToDegrees(const float radians) { return radians
* @param value Value.
* @return Arc sine of the value.
*/
static inline float FusionAsin(const float value) {
if (value <= -1.0f) {
return (float)M_PI / -2.0f;
}
if (value >= 1.0f) {
return (float)M_PI / 2.0f;
}
return asinf(value);
static inline float FusionAsin(const float value)
{
if (value <= -1.0f) {
return (float)M_PI / -2.0f;
}
if (value >= 1.0f) {
return (float)M_PI / 2.0f;
}
return asinf(value);
}
//------------------------------------------------------------------------------
@@ -162,16 +169,17 @@ static inline float FusionAsin(const float value) {
* @param x Operand.
* @return Reciprocal of the square root of x.
*/
static inline float FusionFastInverseSqrt(const float x) {
static inline float FusionFastInverseSqrt(const float x)
{
typedef union {
float f;
int32_t i;
} Union32;
typedef union {
float f;
int32_t i;
} Union32;
Union32 union32 = {.f = x};
union32.i = 0x5F1F1412 - (union32.i >> 1);
return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
Union32 union32 = {.f = x};
union32.i = 0x5F1F1412 - (union32.i >> 1);
return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f);
}
#endif
@@ -184,8 +192,9 @@ static inline float FusionFastInverseSqrt(const float x) {
* @param vector Vector.
* @return True if the vector is zero.
*/
static inline bool FusionVectorIsZero(const FusionVector vector) {
return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
static inline bool FusionVectorIsZero(const FusionVector vector)
{
return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f);
}
/**
@@ -194,13 +203,14 @@ static inline bool FusionVectorIsZero(const FusionVector vector) {
* @param vectorB Vector B.
* @return Sum of two vectors.
*/
static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) {
const FusionVector result = {.axis = {
.x = vectorA.axis.x + vectorB.axis.x,
.y = vectorA.axis.y + vectorB.axis.y,
.z = vectorA.axis.z + vectorB.axis.z,
}};
return result;
static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB)
{
const FusionVector result = {.axis = {
.x = vectorA.axis.x + vectorB.axis.x,
.y = vectorA.axis.y + vectorB.axis.y,
.z = vectorA.axis.z + vectorB.axis.z,
}};
return result;
}
/**
@@ -209,13 +219,14 @@ static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const Fus
* @param vectorB Vector B.
* @return Vector B subtracted from vector A.
*/
static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) {
const FusionVector result = {.axis = {
.x = vectorA.axis.x - vectorB.axis.x,
.y = vectorA.axis.y - vectorB.axis.y,
.z = vectorA.axis.z - vectorB.axis.z,
}};
return result;
static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB)
{
const FusionVector result = {.axis = {
.x = vectorA.axis.x - vectorB.axis.x,
.y = vectorA.axis.y - vectorB.axis.y,
.z = vectorA.axis.z - vectorB.axis.z,
}};
return result;
}
/**
@@ -223,7 +234,10 @@ static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, cons
* @param vector Vector.
* @return Sum of the elements.
*/
static inline float FusionVectorSum(const FusionVector vector) { return vector.axis.x + vector.axis.y + vector.axis.z; }
static inline float FusionVectorSum(const FusionVector vector)
{
return vector.axis.x + vector.axis.y + vector.axis.z;
}
/**
* @brief Returns the multiplication of a vector by a scalar.
@@ -231,13 +245,14 @@ static inline float FusionVectorSum(const FusionVector vector) { return vector.a
* @param scalar Scalar.
* @return Multiplication of a vector by a scalar.
*/
static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) {
const FusionVector result = {.axis = {
.x = vector.axis.x * scalar,
.y = vector.axis.y * scalar,
.z = vector.axis.z * scalar,
}};
return result;
static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar)
{
const FusionVector result = {.axis = {
.x = vector.axis.x * scalar,
.y = vector.axis.y * scalar,
.z = vector.axis.z * scalar,
}};
return result;
}
/**
@@ -246,13 +261,14 @@ static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector,
* @param vectorB Vector B.
* @return Hadamard product.
*/
static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) {
const FusionVector result = {.axis = {
.x = vectorA.axis.x * vectorB.axis.x,
.y = vectorA.axis.y * vectorB.axis.y,
.z = vectorA.axis.z * vectorB.axis.z,
}};
return result;
static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB)
{
const FusionVector result = {.axis = {
.x = vectorA.axis.x * vectorB.axis.x,
.y = vectorA.axis.y * vectorB.axis.y,
.z = vectorA.axis.z * vectorB.axis.z,
}};
return result;
}
/**
@@ -261,15 +277,16 @@ static inline FusionVector FusionVectorHadamardProduct(const FusionVector vector
* @param vectorB Vector B.
* @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 B vectorB.axis
const FusionVector result = {.axis = {
.x = A.y * B.z - A.z * B.y,
.y = A.z * B.x - A.x * B.z,
.z = A.x * B.y - A.y * B.x,
}};
return result;
const FusionVector result = {.axis = {
.x = A.y * B.z - A.z * B.y,
.y = A.z * B.x - A.x * B.z,
.z = A.x * B.y - A.y * B.x,
}};
return result;
#undef A
#undef B
}
@@ -280,8 +297,9 @@ static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA,
* @param vectorB Vector B.
* @return Dot product.
*/
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) {
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB)
{
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
}
/**
@@ -289,27 +307,34 @@ static inline float FusionVectorDotProduct(const FusionVector vectorA, const Fus
* @param vector Vector.
* @return Vector magnitude squared.
*/
static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); }
static inline float FusionVectorMagnitudeSquared(const FusionVector vector)
{
return FusionVectorSum(FusionVectorHadamardProduct(vector, vector));
}
/**
* @brief Returns the vector magnitude.
* @param vector Vector.
* @return Vector magnitude.
*/
static inline float FusionVectorMagnitude(const FusionVector vector) { return sqrtf(FusionVectorMagnitudeSquared(vector)); }
static inline float FusionVectorMagnitude(const FusionVector vector)
{
return sqrtf(FusionVectorMagnitudeSquared(vector));
}
/**
* @brief Returns the normalised vector.
* @param vector Vector.
* @return Normalised vector.
*/
static inline FusionVector FusionVectorNormalise(const FusionVector vector) {
static inline FusionVector FusionVectorNormalise(const FusionVector vector)
{
#ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
#else
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
#endif
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
}
//------------------------------------------------------------------------------
@@ -321,14 +346,15 @@ static inline FusionVector FusionVectorNormalise(const FusionVector vector) {
* @param quaternionB Quaternion B.
* @return Sum of two quaternions.
*/
static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) {
const FusionQuaternion result = {.element = {
.w = quaternionA.element.w + quaternionB.element.w,
.x = quaternionA.element.x + quaternionB.element.x,
.y = quaternionA.element.y + quaternionB.element.y,
.z = quaternionA.element.z + quaternionB.element.z,
}};
return result;
static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB)
{
const FusionQuaternion result = {.element = {
.w = quaternionA.element.w + quaternionB.element.w,
.x = quaternionA.element.x + quaternionB.element.x,
.y = quaternionA.element.y + quaternionB.element.y,
.z = quaternionA.element.z + quaternionB.element.z,
}};
return result;
}
/**
@@ -337,16 +363,17 @@ static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quater
* @param quaternionB Quaternion B (to be pre-multiplied).
* @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 B quaternionB.element
const FusionQuaternion result = {.element = {
.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,
.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,
}};
return result;
const FusionQuaternion result = {.element = {
.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,
.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,
}};
return result;
#undef A
#undef B
}
@@ -360,16 +387,17 @@ static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion q
* @param vector 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 V vector.axis
const FusionQuaternion result = {.element = {
.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,
.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,
}};
return result;
const FusionQuaternion result = {.element = {
.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,
.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,
}};
return result;
#undef Q
#undef V
}
@@ -379,20 +407,21 @@ static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuater
* @param quaternion Quaternion.
* @return Normalised quaternion.
*/
static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) {
static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion)
{
#define Q quaternion.element
#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
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
const FusionQuaternion result = {.element = {
.w = Q.w * magnitudeReciprocal,
.x = Q.x * magnitudeReciprocal,
.y = Q.y * magnitudeReciprocal,
.z = Q.z * magnitudeReciprocal,
}};
return result;
const FusionQuaternion result = {.element = {
.w = Q.w * magnitudeReciprocal,
.x = Q.x * magnitudeReciprocal,
.y = Q.y * magnitudeReciprocal,
.z = Q.z * magnitudeReciprocal,
}};
return result;
#undef Q
}
@@ -405,14 +434,15 @@ static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion
* @param vector 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
const FusionVector result = {.axis = {
.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,
.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z,
}};
return result;
const FusionVector result = {.axis = {
.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,
.z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z,
}};
return result;
#undef R
}
@@ -424,27 +454,28 @@ static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix,
* @param quaternion Quaternion.
* @return Rotation matrix.
*/
static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) {
static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion)
{
#define Q quaternion.element
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
const FusionMatrix matrix = {.element = {
.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x),
.xy = 2.0f * (qxqy - qwqz),
.xz = 2.0f * (qxqz + qwqy),
.yx = 2.0f * (qxqy + qwqz),
.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y),
.yz = 2.0f * (qyqz - qwqx),
.zx = 2.0f * (qxqz - qwqy),
.zy = 2.0f * (qyqz + qwqx),
.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z),
}};
return matrix;
const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations
const float qwqx = Q.w * Q.x;
const float qwqy = Q.w * Q.y;
const float qwqz = Q.w * Q.z;
const float qxqy = Q.x * Q.y;
const float qxqz = Q.x * Q.z;
const float qyqz = Q.y * Q.z;
const FusionMatrix matrix = {.element = {
.xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x),
.xy = 2.0f * (qxqy - qwqz),
.xz = 2.0f * (qxqz + qwqy),
.yx = 2.0f * (qxqy + qwqz),
.yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y),
.yz = 2.0f * (qyqz - qwqx),
.zx = 2.0f * (qxqz - qwqy),
.zy = 2.0f * (qyqz + qwqx),
.zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z),
}};
return matrix;
#undef Q
}
@@ -453,15 +484,16 @@ static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quate
* @param quaternion Quaternion.
* @return Euler angles in degrees.
*/
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) {
static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion)
{
#define Q quaternion.element
const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
const FusionEuler euler = {.angle = {
.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))),
.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)),
}};
return euler;
const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations
const FusionEuler euler = {.angle = {
.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))),
.yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)),
}};
return euler;
#undef Q
}
src/Fusion/FusionOffset.c
+25 -22
View File
@@ -37,11 +37,12 @@
* @param offset Gyroscope offset algorithm structure.
* @param sampleRate Sample rate in Hz.
*/
void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) {
offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate);
offset->timeout = TIMEOUT * sampleRate;
offset->timer = 0;
offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate)
{
offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate);
offset->timeout = TIMEOUT * sampleRate;
offset->timer = 0;
offset->gyroscopeOffset = FUSION_VECTOR_ZERO;
}
/**
@@ -51,26 +52,28 @@ void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampl
* @param gyroscope 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
gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
// Subtract offset from gyroscope measurement
gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset);
// Reset timer if gyroscope not stationary
if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) {
offset->timer = 0;
// Reset timer if gyroscope not stationary
if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) {
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;
}
// 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.
*/
typedef struct {
float filterCoefficient;
unsigned int timeout;
unsigned int timer;
FusionVector gyroscopeOffset;
float filterCoefficient;
unsigned int timeout;
unsigned int timer;
FusionVector gyroscopeOffset;
} FusionOffset;
//------------------------------------------------------------------------------
src/GPSStatus.h
+102 -90
View File
@@ -4,124 +4,136 @@
#include "configuration.h"
#include <Arduino.h>
namespace meshtastic {
namespace meshtastic
{
/// Describes the state of the GPS system.
class GPSStatus : public Status {
class GPSStatus : public Status
{
private:
CallbackObserver<GPSStatus, const GPSStatus *> statusObserver = CallbackObserver<GPSStatus, const GPSStatus *>(this, &GPSStatus::updateStatus);
private:
CallbackObserver<GPSStatus, const GPSStatus *> statusObserver =
CallbackObserver<GPSStatus, const GPSStatus *>(this, &GPSStatus::updateStatus);
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 hasLock = false; // default to false, until we complete our first read
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)
uint32_t lastFixMillis = 0;
/// Time of last valid GPS fix (millis since boot)
uint32_t lastFixMillis = 0;
public:
GPSStatus() { statusType = STATUS_TYPE_GPS; }
public:
GPSStatus() { statusType = STATUS_TYPE_GPS; }
// preferred method
GPSStatus(bool hasLock, bool isConnected, bool isPowerSaving, const meshtastic_Position &pos) : Status() {
this->hasLock = hasLock;
this->isConnected = isConnected;
this->isPowerSaving = isPowerSaving;
// preferred method
GPSStatus(bool hasLock, bool isConnected, bool isPowerSaving, const meshtastic_Position &pos) : Status()
{
this->hasLock = hasLock;
this->isConnected = isConnected;
this->isPowerSaving = isPowerSaving;
// all-in-one struct copy
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;
// all-in-one struct copy
this->p = pos;
}
}
int32_t getLongitude() const {
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.longitude_i;
} else {
return p.longitude_i;
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;
}
}
}
int32_t getAltitude() const {
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.altitude;
} else {
return p.altitude;
int32_t getLongitude() const
{
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.longitude_i;
} else {
return p.longitude_i;
}
}
}
uint32_t getDOP() const { return p.PDOP; }
int32_t getAltitude() const
{
if (config.position.fixed_position) {
meshtastic_NodeInfoLite *node = nodeDB->getMeshNode(nodeDB->getNodeNum());
return node->position.altitude;
} else {
return p.altitude;
}
}
uint32_t getHeading() const { return p.ground_track; }
uint32_t getDOP() const { return p.PDOP; }
uint32_t getNumSatellites() const { return p.sats_in_view; }
uint32_t getHeading() const { return p.ground_track; }
/// Return millis() when the last GPS fix occurred (0 = never)
uint32_t getLastFixMillis() const { return lastFixMillis; }
uint32_t getNumSatellites() const { return p.sats_in_view; }
bool matches(const GPSStatus *newStatus) const {
/// Return millis() when the last GPS fix occurred (0 = never)
uint32_t getLastFixMillis() const { return lastFixMillis; }
bool matches(const GPSStatus *newStatus) const
{
#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
return (newStatus->hasLock != hasLock || newStatus->isConnected != isConnected || newStatus->isPowerSaving != isPowerSaving ||
newStatus->p.latitude_i != p.latitude_i || 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.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");
return (newStatus->hasLock != hasLock || newStatus->isConnected != isConnected ||
newStatus->isPowerSaving != isPowerSaving || newStatus->p.latitude_i != p.latitude_i ||
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.ground_speed != p.ground_speed ||
newStatus->p.sats_in_view != p.sats_in_view);
}
initialized = true;
hasLock = newStatus->hasLock;
isConnected = newStatus->isConnected;
int updateStatus(const GPSStatus *newStatus)
{
// Only update the status if values have actually changed
bool isDirty = matches(newStatus);
p = newStatus->p;
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");
}
if (isDirty) {
if (hasLock) {
// Record time of last valid GPS fix
lastFixMillis = millis();
initialized = true;
hasLock = newStatus->hasLock;
isConnected = newStatus->isConnected;
// 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);
p = newStatus->p;
if (isDirty) {
if (hasLock) {
// Record time of last valid GPS fix
lastFixMillis = millis();
// 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
src/GpioLogic.cpp
+67 -55
View File
@@ -1,90 +1,102 @@
#include "GpioLogic.h"
#include <assert.h>
void GpioVirtPin::set(bool value) {
if (value != this->value) {
this->value = value ? PinState::On : PinState::Off;
if (dependentPin)
dependentPin->update();
}
void GpioVirtPin::set(bool value)
{
if (value != this->value) {
this->value = value ? PinState::On : PinState::Off;
if (dependentPin)
dependentPin->update();
}
}
void GpioHwPin::set(bool value) {
pinMode(num, OUTPUT);
digitalWrite(num, value);
void GpioHwPin::set(bool value)
{
pinMode(num, OUTPUT);
digitalWrite(num, value);
}
GpioTransformer::GpioTransformer(GpioPin *outPin) : outPin(outPin) {}
void GpioTransformer::set(bool value) { outPin->set(value); }
void GpioTransformer::set(bool value)
{
outPin->set(value);
}
GpioUnaryTransformer::GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioTransformer(outPin), inPin(inPin) {
assert(!inPin->dependentPin); // We only allow one dependent pin
inPin->dependentPin = this;
GpioUnaryTransformer::GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioTransformer(outPin), inPin(inPin)
{
assert(!inPin->dependentPin); // We only allow one dependent pin
inPin->dependentPin = this;
// Don't update at construction time, because various GpioPins might be global constructor based not yet initied
// because order of operations for global constructors is not defined. update();
// Don't update at construction time, because various GpioPins might be global constructor based not yet initied because
// order of operations for global constructors is not defined.
// update();
}
/**
* Update the output pin based on the current state of the input pin.
*/
void GpioUnaryTransformer::update() {
auto p = inPin->get();
if (p == GpioVirtPin::PinState::Unset)
return; // Not yet fully initialized
void GpioUnaryTransformer::update()
{
auto p = inPin->get();
if (p == GpioVirtPin::PinState::Unset)
return; // Not yet fully initialized
set(p);
set(p);
}
/**
* Update the output pin based on the current state of the input pin.
*/
void GpioNotTransformer::update() {
auto p = inPin->get();
if (p == GpioVirtPin::PinState::Unset)
return; // Not yet fully initialized
void GpioNotTransformer::update()
{
auto p = inPin->get();
if (p == GpioVirtPin::PinState::Unset)
return; // Not yet fully initialized
set(!p);
set(!p);
}
GpioBinaryTransformer::GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation)
: GpioTransformer(outPin), inPin1(inPin1), inPin2(inPin2), operation(operation) {
assert(!inPin1->dependentPin); // We only allow one dependent pin
inPin1->dependentPin = this;
assert(!inPin2->dependentPin); // We only allow one dependent pin
inPin2->dependentPin = this;
: GpioTransformer(outPin), inPin1(inPin1), inPin2(inPin2), operation(operation)
{
assert(!inPin1->dependentPin); // We only allow one dependent pin
inPin1->dependentPin = this;
assert(!inPin2->dependentPin); // We only allow one dependent pin
inPin2->dependentPin = this;
// Don't update at construction time, because various GpioPins might be global constructor based not yet initiated
// because order of operations for global constructors is not defined. update();
// Don't update at construction time, because various GpioPins might be global constructor based not yet initiated because
// order of operations for global constructors is not defined.
// update();
}
void GpioBinaryTransformer::update() {
auto p1 = inPin1->get(), p2 = inPin2->get();
GpioVirtPin::PinState newValue = GpioVirtPin::PinState::Unset;
void GpioBinaryTransformer::update()
{
auto p1 = inPin1->get(), p2 = inPin2->get();
GpioVirtPin::PinState newValue = GpioVirtPin::PinState::Unset;
if (p1 == GpioVirtPin::PinState::Unset)
newValue = p2; // Not yet fully initialized
else if (p2 == GpioVirtPin::PinState::Unset)
newValue = p1; // Not yet fully initialized
if (p1 == GpioVirtPin::PinState::Unset)
newValue = p2; // Not yet fully initialized
else if (p2 == GpioVirtPin::PinState::Unset)
newValue = p1; // Not yet fully initialized
// If we've already found our value just use it, otherwise need to do the operation
if (newValue == GpioVirtPin::PinState::Unset) {
switch (operation) {
case And:
newValue = (GpioVirtPin::PinState)(p1 && p2);
break;
case Or:
newValue = (GpioVirtPin::PinState)(p1 || p2);
break;
case Xor:
newValue = (GpioVirtPin::PinState)(p1 != p2);
break;
default:
assert(false);
// If we've already found our value just use it, otherwise need to do the operation
if (newValue == GpioVirtPin::PinState::Unset) {
switch (operation) {
case And:
newValue = (GpioVirtPin::PinState)(p1 && p2);
break;
case Or:
newValue = (GpioVirtPin::PinState)(p1 || p2);
break;
case Xor:
newValue = (GpioVirtPin::PinState)(p1 != p2);
break;
default:
assert(false);
}
}
}
set(newValue);
set(newValue);
}
GpioSplitter::GpioSplitter(GpioPin *outPin1, GpioPin *outPin2) : outPin1(outPin1), outPin2(outPin2) {}
src/GpioLogic.h
+85 -77
View File
@@ -3,9 +3,8 @@
#include "configuration.h"
/**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) then using these classes might be able to let you cleanly turn on that enable when either dependent
device is needed.
require these classes! But if your hardware has a GPIO that is 'shared' between multiple devices (i.e. a shared 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
requirements.
@@ -14,21 +13,23 @@
/**
* A logical GPIO pin (not necessary raw hardware).
*/
class GpioPin {
public:
virtual void set(bool value) = 0;
class GpioPin
{
public:
virtual void set(bool value) = 0;
};
/**
* A physical GPIO hw pin.
*/
class GpioHwPin : public GpioPin {
uint32_t num;
class GpioHwPin : public GpioPin
{
uint32_t num;
public:
explicit GpioHwPin(uint32_t num) : num(num) {}
public:
explicit GpioHwPin(uint32_t num) : num(num) {}
void set(bool value);
void set(bool value);
};
class GpioTransformer;
@@ -38,115 +39,122 @@ class GpioBinaryTransformer;
/**
* A virtual GPIO pin.
*/
class GpioVirtPin : public GpioPin {
friend class GpioBinaryTransformer;
friend class GpioUnaryTransformer;
class GpioVirtPin : public GpioPin
{
friend class GpioBinaryTransformer;
friend class GpioUnaryTransformer;
public:
enum PinState { On = true, Off = false, Unset = 2 };
public:
enum PinState { On = true, Off = false, Unset = 2 };
void set(bool value);
PinState get() const { return value; }
void set(bool value);
PinState get() const { return value; }
private:
PinState value = PinState::Unset;
GpioTransformer *dependentPin = NULL;
private:
PinState value = PinState::Unset;
GpioTransformer *dependentPin = NULL;
};
#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. notably: the set method is not public (because it always is calculated by a subclass)
* A 'smart' trigger that can depend in a fake GPIO and if that GPIO changes, drive some other downstream GPIO to change.
* notably: the set method is not public (because it always is calculated by a subclass)
*/
class GpioTransformer {
public:
/**
* Update the output pin based on the current state of the input pin.
*/
virtual void update() = 0;
class GpioTransformer
{
public:
/**
* Update the output pin based on the current state of the input pin.
*/
virtual void update() = 0;
protected:
GpioTransformer(GpioPin *outPin);
protected:
GpioTransformer(GpioPin *outPin);
void set(bool value);
void set(bool value);
private:
GpioPin *outPin;
private:
GpioPin *outPin;
};
/**
* A transformer that just drives a hw pin based on a virtual pin.
*/
class GpioUnaryTransformer : public GpioTransformer {
public:
GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin);
class GpioUnaryTransformer : public GpioTransformer
{
public:
GpioUnaryTransformer(GpioVirtPin *inPin, GpioPin *outPin);
protected:
friend class GpioVirtPin;
protected:
friend class GpioVirtPin;
/**
* Update the output pin based on the current state of the input pin.
*/
virtual void update();
/**
* Update the output pin based on the current state of the input pin.
*/
virtual void update();
GpioVirtPin *inPin;
GpioVirtPin *inPin;
};
/**
* A transformer that performs a unary NOT operation from an input.
*/
class GpioNotTransformer : public GpioUnaryTransformer {
public:
GpioNotTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioUnaryTransformer(inPin, outPin) {}
class GpioNotTransformer : public GpioUnaryTransformer
{
public:
GpioNotTransformer(GpioVirtPin *inPin, GpioPin *outPin) : GpioUnaryTransformer(inPin, outPin) {}
protected:
friend class GpioVirtPin;
protected:
friend class GpioVirtPin;
/**
* Update the output pin based on the current state of the input pin.
*/
void update();
/**
* Update the output pin based on the current state of the input pin.
*/
void update();
};
/**
* A transformer that combines multiple virtual pins to drive an output pin
*/
class GpioBinaryTransformer : public GpioTransformer {
class GpioBinaryTransformer : public GpioTransformer
{
public:
enum Operation { And, Or, Xor };
public:
enum Operation { And, Or, Xor };
GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation);
GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation);
protected:
friend class GpioVirtPin;
protected:
friend class GpioVirtPin;
/**
* Update the output pin based on the current state of the input pins.
*/
void update();
/**
* Update the output pin based on the current state of the input pins.
*/
void update();
private:
GpioVirtPin *inPin1;
GpioVirtPin *inPin2;
Operation operation;
private:
GpioVirtPin *inPin1;
GpioVirtPin *inPin2;
Operation operation;
};
/**
* 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:
GpioSplitter(GpioPin *outPin1, GpioPin *outPin2);
public:
GpioSplitter(GpioPin *outPin1, GpioPin *outPin2);
void set(bool value) {
outPin1->set(value);
outPin2->set(value);
}
void set(bool value)
{
outPin1->set(value);
outPin2->set(value);
}
private:
GpioPin *outPin1;
GpioPin *outPin2;
private:
GpioPin *outPin1;
GpioPin *outPin2;
};
src/Led.cpp
+21 -17
View File
@@ -23,14 +23,16 @@ static GpioPin &ledHwPin = ledRawHwPin;
/**
* A GPIO controlled by the PMU
*/
class GpioPmuPin : public GpioPin {
public:
void set(bool value) {
if (pmu_found && PMU) {
// blink the axp led
PMU->setChargingLedMode(value ? XPOWERS_CHG_LED_ON : XPOWERS_CHG_LED_OFF);
class GpioPmuPin : public GpioPin
{
public:
void set(bool value)
{
if (pmu_found && PMU) {
// blink the axp led
PMU->setChargingLedMode(value ? XPOWERS_CHG_LED_ON : XPOWERS_CHG_LED_OFF);
}
}
}
} ledPmuHwPin;
// In some cases we need to drive a PMU LED and a normal LED
@@ -43,17 +45,19 @@ 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.
*/
class MonitoredLedPin : public GpioPin {
public:
void set(bool value) {
if (powerMon) {
if (value)
powerMon->setState(meshtastic_PowerMon_State_LED_On);
else
powerMon->clearState(meshtastic_PowerMon_State_LED_On);
class MonitoredLedPin : public GpioPin
{
public:
void set(bool value)
{
if (powerMon) {
if (value)
powerMon->setState(meshtastic_PowerMon_State_LED_On);
else
powerMon->clearState(meshtastic_PowerMon_State_LED_On);
}
ledFinalPin.set(value);
}
ledFinalPin.set(value);
}
} monitoredLedPin;
#else
static GpioPin &monitoredLedPin = ledFinalPin;
src/MessageStore.cpp
+280 -249
View File
@@ -18,238 +18,257 @@ static char *g_messagePool = nullptr;
static size_t g_poolWritePos = 0;
// Reset pool (called on boot or clear)
static inline void resetMessagePool() {
if (!g_messagePool) {
g_messagePool = static_cast<char *>(malloc(MESSAGE_TEXT_POOL_SIZE));
static inline void resetMessagePool()
{
if (!g_messagePool) {
LOG_ERROR("MessageStore: Failed to allocate %d bytes for message pool", MESSAGE_TEXT_POOL_SIZE);
return;
g_messagePool = static_cast<char *>(malloc(MESSAGE_TEXT_POOL_SIZE));
if (!g_messagePool) {
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
// If not enough space remains, wrap around (ring buffer style)
static inline uint16_t storeTextInPool(const char *src, size_t len) {
if (len >= MAX_MESSAGE_SIZE)
len = MAX_MESSAGE_SIZE - 1;
static inline uint16_t storeTextInPool(const char *src, size_t len)
{
if (len >= MAX_MESSAGE_SIZE)
len = MAX_MESSAGE_SIZE - 1;
// Wrap pool if out of space
if (g_poolWritePos + len + 1 >= MESSAGE_TEXT_POOL_SIZE) {
g_poolWritePos = 0;
}
// Wrap pool if out of space
if (g_poolWritePos + len + 1 >= MESSAGE_TEXT_POOL_SIZE) {
g_poolWritePos = 0;
}
uint16_t offset = g_poolWritePos;
memcpy(&g_messagePool[g_poolWritePos], src, len);
g_messagePool[g_poolWritePos + len] = '\0';
g_poolWritePos += (len + 1);
return offset;
uint16_t offset = g_poolWritePos;
memcpy(&g_messagePool[g_poolWritePos], src, len);
g_messagePool[g_poolWritePos + len] = '\0';
g_poolWritePos += (len + 1);
return offset;
}
// Retrieve a const pointer to message text by offset
static inline const char *getTextFromPool(uint16_t offset) {
if (!g_messagePool || offset >= MESSAGE_TEXT_POOL_SIZE)
return "";
return &g_messagePool[offset];
static inline const char *getTextFromPool(uint16_t offset)
{
if (!g_messagePool || offset >= MESSAGE_TEXT_POOL_SIZE)
return "";
return &g_messagePool[offset];
}
// Helper: assign a timestamp (RTC if available, else boot-relative)
static inline void assignTimestamp(StoredMessage &sm) {
uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true);
if (nowSecs) {
sm.timestamp = nowSecs;
sm.isBootRelative = false;
} else {
sm.timestamp = millis() / 1000;
sm.isBootRelative = true;
}
static inline void assignTimestamp(StoredMessage &sm)
{
uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true);
if (nowSecs) {
sm.timestamp = nowSecs;
sm.isBootRelative = false;
} else {
sm.timestamp = millis() / 1000;
sm.isBootRelative = true;
}
}
// Generic push with cap (used by live + persisted queues)
template <typename T> static inline void pushWithLimit(std::deque<T> &queue, const T &msg) {
if (queue.size() >= MAX_MESSAGES_SAVED)
queue.pop_front();
queue.push_back(msg);
template <typename T> static inline void pushWithLimit(std::deque<T> &queue, const T &msg)
{
if (queue.size() >= MAX_MESSAGES_SAVED)
queue.pop_front();
queue.push_back(msg);
}
template <typename T> static inline void pushWithLimit(std::deque<T> &queue, T &&msg) {
if (queue.size() >= MAX_MESSAGES_SAVED)
queue.pop_front();
queue.emplace_back(std::move(msg));
template <typename T> static inline void pushWithLimit(std::deque<T> &queue, T &&msg)
{
if (queue.size() >= MAX_MESSAGES_SAVED)
queue.pop_front();
queue.emplace_back(std::move(msg));
}
MessageStore::MessageStore(const std::string &label) {
filename = "/Messages_" + label + ".msgs";
resetMessagePool(); // initialize text pool on boot
MessageStore::MessageStore(const std::string &label)
{
filename = "/Messages_" + label + ".msgs";
resetMessagePool(); // initialize text pool on boot
}
// Live message handling (RAM only)
void MessageStore::addLiveMessage(StoredMessage &&msg) { pushWithLimit(liveMessages, std::move(msg)); }
void MessageStore::addLiveMessage(const StoredMessage &msg) { pushWithLimit(liveMessages, msg); }
void MessageStore::addLiveMessage(StoredMessage &&msg)
{
pushWithLimit(liveMessages, std::move(msg));
}
void MessageStore::addLiveMessage(const StoredMessage &msg)
{
pushWithLimit(liveMessages, msg);
}
// Add from incoming/outgoing packet
const StoredMessage &MessageStore::addFromPacket(const meshtastic_MeshPacket &packet) {
StoredMessage sm;
assignTimestamp(sm);
sm.channelIndex = packet.channel;
const StoredMessage &MessageStore::addFromPacket(const meshtastic_MeshPacket &packet)
{
StoredMessage sm;
assignTimestamp(sm);
sm.channelIndex = packet.channel;
const char *payload = reinterpret_cast<const char *>(packet.decoded.payload.bytes);
size_t len = strnlen(payload, MAX_MESSAGE_SIZE - 1);
sm.textOffset = storeTextInPool(payload, len);
sm.textLength = len;
const char *payload = reinterpret_cast<const char *>(packet.decoded.payload.bytes);
size_t len = strnlen(payload, MAX_MESSAGE_SIZE - 1);
sm.textOffset = storeTextInPool(payload, len);
sm.textLength = len;
// Determine sender
uint32_t localNode = nodeDB->getNodeNum();
sm.sender = (packet.from == 0) ? localNode : packet.from;
// Determine sender
uint32_t localNode = nodeDB->getNodeNum();
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) {
sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST;
sm.ackStatus = AckStatus::NONE;
} else {
sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST;
sm.ackStatus = AckStatus::ACKED;
}
if (packet.from == 0) {
sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST;
sm.ackStatus = AckStatus::NONE;
} else {
sm.type = isDM ? MessageType::DM_TO_US : MessageType::BROADCAST;
sm.ackStatus = AckStatus::ACKED;
}
addLiveMessage(sm);
return liveMessages.back();
addLiveMessage(sm);
return liveMessages.back();
}
// Outgoing/manual message
void MessageStore::addFromString(uint32_t sender, uint8_t channelIndex, const std::string &text) {
StoredMessage sm;
void MessageStore::addFromString(uint32_t sender, uint8_t channelIndex, const std::string &text)
{
StoredMessage sm;
// Always use our local time (helper handles RTC vs boot time)
assignTimestamp(sm);
// Always use our local time (helper handles RTC vs boot time)
assignTimestamp(sm);
sm.sender = sender;
sm.channelIndex = channelIndex;
sm.textOffset = storeTextInPool(text.c_str(), text.size());
sm.textLength = text.size();
sm.sender = sender;
sm.channelIndex = channelIndex;
sm.textOffset = storeTextInPool(text.c_str(), text.size());
sm.textLength = text.size();
// Use the provided destination
sm.dest = sender;
sm.type = MessageType::DM_TO_US;
// Use the provided destination
sm.dest = sender;
sm.type = MessageType::DM_TO_US;
// Outgoing messages always start with unknown ack status
sm.ackStatus = AckStatus::NONE;
// Outgoing messages always start with unknown ack status
sm.ackStatus = AckStatus::NONE;
addLiveMessage(sm);
addLiveMessage(sm);
}
#if ENABLE_MESSAGE_PERSISTENCE
// Compact, fixed-size on-flash representation using offset + length
struct __attribute__((packed)) StoredMessageRecord {
uint32_t timestamp;
uint32_t sender;
uint8_t channelIndex;
uint32_t dest;
uint8_t isBootRelative;
uint8_t ackStatus; // static_cast<uint8_t>(AckStatus)
uint8_t type; // static_cast<uint8_t>(MessageType)
uint16_t textLength; // message length
char text[MAX_MESSAGE_SIZE]; // store actual text here
uint32_t timestamp;
uint32_t sender;
uint8_t channelIndex;
uint32_t dest;
uint8_t isBootRelative;
uint8_t ackStatus; // static_cast<uint8_t>(AckStatus)
uint8_t type; // static_cast<uint8_t>(MessageType)
uint16_t textLength; // message length
char text[MAX_MESSAGE_SIZE]; // store actual text here
};
// Serialize one StoredMessage to flash
static inline void writeMessageRecord(SafeFile &f, const StoredMessage &m) {
StoredMessageRecord rec = {};
rec.timestamp = m.timestamp;
rec.sender = m.sender;
rec.channelIndex = m.channelIndex;
rec.dest = m.dest;
rec.isBootRelative = m.isBootRelative;
rec.ackStatus = static_cast<uint8_t>(m.ackStatus);
rec.type = static_cast<uint8_t>(m.type);
rec.textLength = m.textLength;
static inline void writeMessageRecord(SafeFile &f, const StoredMessage &m)
{
StoredMessageRecord rec = {};
rec.timestamp = m.timestamp;
rec.sender = m.sender;
rec.channelIndex = m.channelIndex;
rec.dest = m.dest;
rec.isBootRelative = m.isBootRelative;
rec.ackStatus = static_cast<uint8_t>(m.ackStatus);
rec.type = static_cast<uint8_t>(m.type);
rec.textLength = m.textLength;
// Copy the actual text into the record from RAM pool
const char *txt = getTextFromPool(m.textOffset);
strncpy(rec.text, txt, MAX_MESSAGE_SIZE - 1);
rec.text[MAX_MESSAGE_SIZE - 1] = '\0';
// Copy the actual text into the record from RAM pool
const char *txt = getTextFromPool(m.textOffset);
strncpy(rec.text, txt, MAX_MESSAGE_SIZE - 1);
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
static inline bool readMessageRecord(File &f, StoredMessage &m) {
StoredMessageRecord rec = {};
if (f.readBytes(reinterpret_cast<char *>(&rec), sizeof(rec)) != sizeof(rec))
return false;
static inline bool readMessageRecord(File &f, StoredMessage &m)
{
StoredMessageRecord rec = {};
if (f.readBytes(reinterpret_cast<char *>(&rec), sizeof(rec)) != sizeof(rec))
return false;
m.timestamp = rec.timestamp;
m.sender = rec.sender;
m.channelIndex = rec.channelIndex;
m.dest = rec.dest;
m.isBootRelative = rec.isBootRelative;
m.ackStatus = static_cast<AckStatus>(rec.ackStatus);
m.type = static_cast<MessageType>(rec.type);
m.textLength = rec.textLength;
m.timestamp = rec.timestamp;
m.sender = rec.sender;
m.channelIndex = rec.channelIndex;
m.dest = rec.dest;
m.isBootRelative = rec.isBootRelative;
m.ackStatus = static_cast<AckStatus>(rec.ackStatus);
m.type = static_cast<MessageType>(rec.type);
m.textLength = rec.textLength;
// 💡 Re-store text into pool and update offset
m.textLength = strnlen(rec.text, MAX_MESSAGE_SIZE - 1);
m.textOffset = storeTextInPool(rec.text, m.textLength);
// 💡 Re-store text into pool and update offset
m.textLength = strnlen(rec.text, MAX_MESSAGE_SIZE - 1);
m.textOffset = storeTextInPool(rec.text, m.textLength);
return true;
return true;
}
void MessageStore::saveToFlash() {
void MessageStore::saveToFlash()
{
#ifdef FSCom
// Ensure root exists
spiLock->lock();
FSCom.mkdir("/");
spiLock->unlock();
// Ensure root exists
spiLock->lock();
FSCom.mkdir("/");
spiLock->unlock();
SafeFile f(filename.c_str(), false);
SafeFile f(filename.c_str(), false);
spiLock->lock();
uint8_t count = static_cast<uint8_t>(liveMessages.size());
if (count > MAX_MESSAGES_SAVED)
count = MAX_MESSAGES_SAVED;
f.write(&count, 1);
spiLock->lock();
uint8_t count = static_cast<uint8_t>(liveMessages.size());
if (count > MAX_MESSAGES_SAVED)
count = MAX_MESSAGES_SAVED;
f.write(&count, 1);
for (uint8_t i = 0; i < count; ++i) {
writeMessageRecord(f, liveMessages[i]);
}
spiLock->unlock();
for (uint8_t i = 0; i < count; ++i) {
writeMessageRecord(f, liveMessages[i]);
}
spiLock->unlock();
f.close();
f.close();
#endif
}
void MessageStore::loadFromFlash() {
std::deque<StoredMessage>().swap(liveMessages);
resetMessagePool(); // reset pool when loading
void MessageStore::loadFromFlash()
{
std::deque<StoredMessage>().swap(liveMessages);
resetMessagePool(); // reset pool when loading
#ifdef FSCom
concurrency::LockGuard guard(spiLock);
concurrency::LockGuard guard(spiLock);
if (!FSCom.exists(filename.c_str()))
return;
if (!FSCom.exists(filename.c_str()))
return;
auto f = FSCom.open(filename.c_str(), FILE_O_READ);
if (!f)
return;
auto f = FSCom.open(filename.c_str(), FILE_O_READ);
if (!f)
return;
uint8_t count = 0;
f.readBytes(reinterpret_cast<char *>(&count), 1);
if (count > MAX_MESSAGES_SAVED)
count = MAX_MESSAGES_SAVED;
uint8_t count = 0;
f.readBytes(reinterpret_cast<char *>(&count), 1);
if (count > MAX_MESSAGES_SAVED)
count = MAX_MESSAGES_SAVED;
for (uint8_t i = 0; i < count; ++i) {
StoredMessage m;
if (!readMessageRecord(f, m))
break;
liveMessages.push_back(m);
}
for (uint8_t i = 0; i < count; ++i) {
StoredMessage m;
if (!readMessageRecord(f, m))
break;
liveMessages.push_back(m);
}
f.close();
f.close();
#endif
}
@@ -260,134 +279,146 @@ void MessageStore::loadFromFlash() {}
#endif
// Clear all messages (RAM + persisted queue)
void MessageStore::clearAllMessages() {
std::deque<StoredMessage>().swap(liveMessages);
resetMessagePool();
void MessageStore::clearAllMessages()
{
std::deque<StoredMessage>().swap(liveMessages);
resetMessagePool();
#ifdef FSCom
SafeFile f(filename.c_str(), false);
uint8_t count = 0;
f.write(&count, 1); // write "0 messages"
f.close();
SafeFile f(filename.c_str(), false);
uint8_t count = 0;
f.write(&count, 1); // write "0 messages"
f.close();
#endif
}
// 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) {
if (fromBack) {
// Iterate from the back and erase all matches from the end
for (auto it = deque.rbegin(); it != deque.rend();) {
if (pred(*it)) {
it = std::deque<StoredMessage>::reverse_iterator(deque.erase(std::next(it).base()));
} else {
++it;
}
template <typename Predicate> static void eraseIf(std::deque<StoredMessage> &deque, Predicate pred, bool fromBack = false)
{
if (fromBack) {
// Iterate from the back and erase all matches from the end
for (auto it = deque.rbegin(); it != deque.rend();) {
if (pred(*it)) {
it = std::deque<StoredMessage>::reverse_iterator(deque.erase(std::next(it).base()));
} 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;
}
}
}
} 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)
void MessageStore::deleteOldestMessage() {
eraseIf(liveMessages, [](StoredMessage &) { return true; });
saveToFlash();
void MessageStore::deleteOldestMessage()
{
eraseIf(liveMessages, [](StoredMessage &) { return true; });
saveToFlash();
}
// Delete oldest message in a specific channel
void MessageStore::deleteOldestMessageInChannel(uint8_t channel) {
auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; };
eraseIf(liveMessages, pred);
saveToFlash();
void MessageStore::deleteOldestMessageInChannel(uint8_t channel)
{
auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; };
eraseIf(liveMessages, pred);
saveToFlash();
}
void MessageStore::deleteAllMessagesInChannel(uint8_t channel) {
auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; };
eraseIf(liveMessages, pred, false /* delete ALL, not just first */);
saveToFlash();
void MessageStore::deleteAllMessagesInChannel(uint8_t channel)
{
auto pred = [channel](const StoredMessage &m) { return m.type == MessageType::BROADCAST && m.channelIndex == channel; };
eraseIf(liveMessages, pred, false /* delete ALL, not just first */);
saveToFlash();
}
void MessageStore::deleteAllMessagesWithPeer(uint32_t peer) {
uint32_t local = nodeDB->getNodeNum();
auto pred = [&](const StoredMessage &m) {
if (m.type != MessageType::DM_TO_US)
return false;
uint32_t other = (m.sender == local) ? m.dest : m.sender;
return other == peer;
};
eraseIf(liveMessages, pred, false);
saveToFlash();
void MessageStore::deleteAllMessagesWithPeer(uint32_t peer)
{
uint32_t local = nodeDB->getNodeNum();
auto pred = [&](const StoredMessage &m) {
if (m.type != MessageType::DM_TO_US)
return false;
uint32_t other = (m.sender == local) ? m.dest : m.sender;
return other == peer;
};
eraseIf(liveMessages, pred, false);
saveToFlash();
}
// Delete oldest message in a direct chat with a node
void MessageStore::deleteOldestMessageWithPeer(uint32_t peer) {
auto pred = [peer](const StoredMessage &m) {
if (m.type != MessageType::DM_TO_US)
return false;
uint32_t other = (m.sender == nodeDB->getNodeNum()) ? m.dest : m.sender;
return other == peer;
};
eraseIf(liveMessages, pred);
saveToFlash();
void MessageStore::deleteOldestMessageWithPeer(uint32_t peer)
{
auto pred = [peer](const StoredMessage &m) {
if (m.type != MessageType::DM_TO_US)
return false;
uint32_t other = (m.sender == nodeDB->getNodeNum()) ? m.dest : m.sender;
return other == peer;
};
eraseIf(liveMessages, pred);
saveToFlash();
}
std::deque<StoredMessage> MessageStore::getChannelMessages(uint8_t channel) const {
std::deque<StoredMessage> result;
for (const auto &m : liveMessages) {
if (m.type == MessageType::BROADCAST && m.channelIndex == channel) {
result.push_back(m);
std::deque<StoredMessage> MessageStore::getChannelMessages(uint8_t channel) const
{
std::deque<StoredMessage> result;
for (const auto &m : liveMessages) {
if (m.type == MessageType::BROADCAST && m.channelIndex == channel) {
result.push_back(m);
}
}
}
return result;
return result;
}
std::deque<StoredMessage> MessageStore::getDirectMessages() const {
std::deque<StoredMessage> result;
for (const auto &m : liveMessages) {
if (m.type == MessageType::DM_TO_US) {
result.push_back(m);
std::deque<StoredMessage> MessageStore::getDirectMessages() const
{
std::deque<StoredMessage> result;
for (const auto &m : liveMessages) {
if (m.type == MessageType::DM_TO_US) {
result.push_back(m);
}
}
}
return result;
return result;
}
// Upgrade boot-relative timestamps once RTC is valid
// Only same-boot boot-relative messages are healed.
// Persisted boot-relative messages from old boots stay ??? forever.
void MessageStore::upgradeBootRelativeTimestamps() {
uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true);
if (nowSecs == 0)
return; // Still no valid RTC
void MessageStore::upgradeBootRelativeTimestamps()
{
uint32_t nowSecs = getValidTime(RTCQuality::RTCQualityDevice, true);
if (nowSecs == 0)
return; // Still no valid RTC
uint32_t bootNow = millis() / 1000;
uint32_t bootNow = millis() / 1000;
auto fix = [&](std::deque<StoredMessage> &dq) {
for (auto &m : dq) {
if (m.isBootRelative && m.timestamp <= bootNow) {
uint32_t bootOffset = nowSecs - bootNow;
m.timestamp += bootOffset;
m.isBootRelative = false;
}
}
};
fix(liveMessages);
auto fix = [&](std::deque<StoredMessage> &dq) {
for (auto &m : dq) {
if (m.isBootRelative && m.timestamp <= bootNow) {
uint32_t bootOffset = nowSecs - bootNow;
m.timestamp += bootOffset;
m.isBootRelative = false;
}
}
};
fix(liveMessages);
}
const char *MessageStore::getText(const StoredMessage &msg) {
// Wrapper around the internal helper
return getTextFromPool(msg.textOffset);
const char *MessageStore::getText(const StoredMessage &msg)
{
// Wrapper around the internal helper
return getTextFromPool(msg.textOffset);
}
uint16_t MessageStore::storeText(const char *src, size_t len) {
// Wrapper around the internal helper
return storeTextInPool(src, len);
uint16_t MessageStore::storeText(const char *src, size_t len)
{
// Wrapper around the internal helper
return storeTextInPool(src, len);
}
// Global definition
src/MessageStore.h
+61 -58
View File
@@ -39,87 +39,90 @@
// Explicit message classification
enum class MessageType : uint8_t {
BROADCAST = 0, // broadcast message
DM_TO_US = 1 // direct message addressed to this node
BROADCAST = 0, // broadcast message
DM_TO_US = 1 // direct message addressed to this node
};
// Delivery status for messages we sent
enum class AckStatus : uint8_t {
NONE = 0, // just sent, waiting (no symbol shown)
ACKED = 1, // got a valid ACK from destination
NACKED = 2, // explicitly failed
TIMEOUT = 3, // no ACK after retry window
RELAYED = 4 // got an ACK from relay, not destination
NONE = 0, // just sent, waiting (no symbol shown)
ACKED = 1, // got a valid ACK from destination
NACKED = 2, // explicitly failed
TIMEOUT = 3, // no ACK after retry window
RELAYED = 4 // got an ACK from relay, not destination
};
struct StoredMessage {
uint32_t timestamp; // When message was created (secs since boot or RTC)
uint32_t sender; // NodeNum of sender
uint8_t channelIndex; // Channel index used
uint32_t dest; // Destination node (broadcast or direct)
MessageType type; // Derived from dest (explicit classification)
bool isBootRelative; // true = millis()/1000 fallback; false = epoch/RTC absolute
AckStatus ackStatus; // Delivery status (only meaningful for our own sent messages)
uint32_t timestamp; // When message was created (secs since boot or RTC)
uint32_t sender; // NodeNum of sender
uint8_t channelIndex; // Channel index used
uint32_t dest; // Destination node (broadcast or direct)
MessageType type; // Derived from dest (explicit classification)
bool isBootRelative; // true = millis()/1000 fallback; false = epoch/RTC absolute
AckStatus ackStatus; // Delivery status (only meaningful for our own sent messages)
// Text storage metadata — rebuilt from flash at boot
uint16_t textOffset; // Offset into global text pool (valid only after loadFromFlash())
uint16_t textLength; // Length of text in bytes
// Text storage metadata — rebuilt from flash at boot
uint16_t textOffset; // Offset into global text pool (valid only after loadFromFlash())
uint16_t textLength; // Length of text in bytes
// Default constructor initializes all fields safely
StoredMessage()
: timestamp(0), sender(0), channelIndex(0), dest(0xffffffff), type(MessageType::BROADCAST), isBootRelative(false), ackStatus(AckStatus::NONE),
textOffset(0), textLength(0) {}
// Default constructor initializes all fields safely
StoredMessage()
: timestamp(0), sender(0), channelIndex(0), dest(0xffffffff), type(MessageType::BROADCAST), isBootRelative(false),
ackStatus(AckStatus::NONE), textOffset(0), textLength(0)
{
}
};
class MessageStore {
public:
explicit MessageStore(const std::string &label);
class MessageStore
{
public:
explicit MessageStore(const std::string &label);
// Live RAM methods (always current, used by UI and runtime)
void addLiveMessage(StoredMessage &&msg);
void addLiveMessage(const StoredMessage &msg); // convenience overload
const std::deque<StoredMessage> &getLiveMessages() const { return liveMessages; }
// Live RAM methods (always current, used by UI and runtime)
void addLiveMessage(StoredMessage &&msg);
void addLiveMessage(const StoredMessage &msg); // convenience overload
const std::deque<StoredMessage> &getLiveMessages() const { return liveMessages; }
// Add new messages from packets or manual input
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
// Add new messages from packets or manual input
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
// Persistence methods (used only on boot/shutdown)
void saveToFlash(); // Save messages to flash
void loadFromFlash(); // Load messages from flash
// Persistence methods (used only on boot/shutdown)
void saveToFlash(); // Save messages to flash
void loadFromFlash(); // Load messages from flash
// Clear all messages (RAM + persisted queue + text pool)
void clearAllMessages();
// Clear all messages (RAM + persisted queue + text pool)
void clearAllMessages();
// Delete helpers
void deleteOldestMessage(); // remove oldest from RAM (and flash on save)
void deleteOldestMessageInChannel(uint8_t channel);
void deleteOldestMessageWithPeer(uint32_t peer);
void deleteAllMessagesInChannel(uint8_t channel);
void deleteAllMessagesWithPeer(uint32_t peer);
// Delete helpers
void deleteOldestMessage(); // remove oldest from RAM (and flash on save)
void deleteOldestMessageInChannel(uint8_t channel);
void deleteOldestMessageWithPeer(uint32_t peer);
void deleteAllMessagesInChannel(uint8_t channel);
void deleteAllMessagesWithPeer(uint32_t peer);
// Unified accessor (for UI code, defaults to RAM buffer)
const std::deque<StoredMessage> &getMessages() const { return liveMessages; }
// Unified accessor (for UI code, defaults to RAM buffer)
const std::deque<StoredMessage> &getMessages() const { return liveMessages; }
// Helper filters for future use
std::deque<StoredMessage> getChannelMessages(uint8_t channel) const; // Only broadcast messages on a channel
std::deque<StoredMessage> getDirectMessages() const; // Only direct messages
// Helper filters for future use
std::deque<StoredMessage> getChannelMessages(uint8_t channel) const; // Only broadcast messages on a channel
std::deque<StoredMessage> getDirectMessages() const; // Only direct messages
// Upgrade boot-relative timestamps once RTC is valid
void upgradeBootRelativeTimestamps();
// Upgrade boot-relative timestamps once RTC is valid
void upgradeBootRelativeTimestamps();
// Retrieve the C-string text for a stored message
static const char *getText(const StoredMessage &msg);
// Retrieve the C-string text for a stored message
static const char *getText(const StoredMessage &msg);
// Allocate text into pool (used by sender-side code)
static uint16_t storeText(const char *src, size_t len);
// Allocate text into pool (used by sender-side code)
static uint16_t storeText(const char *src, size_t len);
// Used when loading from flash to rebuild the text pool
static uint16_t rebuildTextFromFlash(const char *src, size_t len);
// Used when loading from flash to rebuild the text pool
static uint16_t rebuildTextFromFlash(const char *src, size_t len);
private:
std::deque<StoredMessage> liveMessages; // Single in-RAM message buffer (also used for persistence)
std::string filename; // Flash filename for persistence
private:
std::deque<StoredMessage> liveMessages; // Single in-RAM message buffer (also used for persistence)
std::string filename; // Flash filename for persistence
};
// Global instance (defined in MessageStore.cpp)
src/NodeStatus.h
+48 -40
View File
@@ -3,56 +3,64 @@
#include "configuration.h"
#include <Arduino.h>
namespace meshtastic {
namespace meshtastic
{
/// Describes the state of the NodeDB system.
class NodeStatus : public Status {
class NodeStatus : public Status
{
private:
CallbackObserver<NodeStatus, const NodeStatus *> statusObserver = CallbackObserver<NodeStatus, const NodeStatus *>(this, &NodeStatus::updateStatus);
private:
CallbackObserver<NodeStatus, const NodeStatus *> statusObserver =
CallbackObserver<NodeStatus, const NodeStatus *>(this, &NodeStatus::updateStatus);
uint16_t numOnline = 0;
uint16_t numTotal = 0;
uint16_t numOnline = 0;
uint16_t numTotal = 0;
uint16_t lastNumTotal = 0;
uint16_t lastNumTotal = 0;
public:
bool forceUpdate = false;
public:
bool forceUpdate = false;
NodeStatus() { statusType = STATUS_TYPE_NODE; }
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;
NodeStatus() { statusType = STATUS_TYPE_NODE; }
NodeStatus(uint16_t numOnline, uint16_t numTotal, bool forceUpdate = false) : Status()
{
isDirty = matches(newStatus);
initialized = true;
numOnline = newStatus->getNumOnline();
numTotal = newStatus->getNumTotal();
this->forceUpdate = forceUpdate;
this->numOnline = numOnline;
this->numTotal = numTotal;
}
if (isDirty || newStatus->forceUpdate) {
LOG_DEBUG("Node status update: %u online, %u total", numOnline, numTotal);
onNewStatus.notifyObservers(this);
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;
{
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
src/Observer.h
+67 -58
View File
@@ -8,90 +8,99 @@ template <class T> class Observable;
/**
* An observer which can be mixed in as a baseclass. Implement onNotify as a method in your class.
*/
template <class T> class Observer {
std::list<Observable<T> *> observables;
template <class T> class Observer
{
std::list<Observable<T> *> observables;
public:
virtual ~Observer();
public:
virtual ~Observer();
/// Stop watching the observable
void unobserve(Observable<T> *o);
/// Stop watching the observable
void unobserve(Observable<T> *o);
/// Start watching a specified observable
void observe(Observable<T> *o);
/// Start watching a specified observable
void observe(Observable<T> *o);
private:
friend class Observable<T>;
private:
friend class Observable<T>;
protected:
/**
* 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
**/
virtual int onNotify(T arg) = 0;
protected:
/**
* 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
**/
virtual int onNotify(T arg) = 0;
};
/**
* An observer that calls an arbitrary method
*/
template <class Callback, class T> class CallbackObserver : public Observer<T> {
typedef int (Callback::*ObserverCallback)(T arg);
template <class Callback, class T> class CallbackObserver : public Observer<T>
{
typedef int (Callback::*ObserverCallback)(T arg);
Callback *objPtr;
ObserverCallback method;
Callback *objPtr;
ObserverCallback method;
public:
CallbackObserver(Callback *_objPtr, ObserverCallback _method) : objPtr(_objPtr), method(_method) {}
public:
CallbackObserver(Callback *_objPtr, ObserverCallback _method) : objPtr(_objPtr), method(_method) {}
protected:
virtual int onNotify(T arg) override { return (objPtr->*method)(arg); }
protected:
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 performance reasons a pointer or word sized object is recommended.
* An observable class that will notify observers anytime notifyObservers is called. Argument type T can be any type, but for
* performance reasons a pointer or word sized object is recommended.
*/
template <class T> class Observable {
std::list<Observer<T> *> observers;
template <class T> class Observable
{
std::list<Observer<T> *> observers;
public:
/**
* 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
*/
int notifyObservers(T arg) {
for (typename std::list<Observer<T> *>::const_iterator iterator = observers.begin(); iterator != observers.end(); ++iterator) {
int result = (*iterator)->onNotify(arg);
if (result != 0)
return result;
public:
/**
* 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
*/
int notifyObservers(T arg)
{
for (typename std::list<Observer<T> *>::const_iterator iterator = observers.begin(); iterator != observers.end();
++iterator) {
int result = (*iterator)->onNotify(arg);
if (result != 0)
return result;
}
return 0;
}
return 0;
}
private:
friend class Observer<T>;
private:
friend class Observer<T>;
// Not called directly, instead call observer.observe
void addObserver(Observer<T> *o) { observers.push_back(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); }
void removeObserver(Observer<T> *o) { observers.remove(o); }
};
template <class T> Observer<T>::~Observer() {
for (typename std::list<Observable<T> *>::const_iterator iterator = observables.begin(); iterator != observables.end(); ++iterator) {
(*iterator)->removeObserver(this);
}
observables.clear();
template <class T> Observer<T>::~Observer()
{
for (typename std::list<Observable<T> *>::const_iterator iterator = observables.begin(); iterator != observables.end();
++iterator) {
(*iterator)->removeObserver(this);
}
observables.clear();
}
template <class T> void Observer<T>::unobserve(Observable<T> *o) {
o->removeObserver(this);
observables.remove(o);
template <class T> void Observer<T>::unobserve(Observable<T> *o)
{
o->removeObserver(this);
observables.remove(o);
}
template <class T> void Observer<T>::observe(Observable<T> *o) {
observables.push_back(o);
o->addObserver(this);
template <class T> void Observer<T>::observe(Observable<T> *o)
{
observables.push_back(o);
o->addObserver(this);
}
src/Power.cpp
+999 -932
View File
File diff suppressed because it is too large Load Diff
src/PowerFSM.cpp
+267 -241
View File
@@ -31,202 +31,222 @@ FakeFsm powerFSM;
void PowerFSM_setup(){};
#else
/// 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
#if !defined(BATTERY_PIN) && !defined(HAS_AXP192) && !defined(HAS_AXP2101) && !defined(NRF_APM)
return true;
return true;
#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
// We assume routers might be powered all the time, but from a low current (solar) source
bool isPowerSavingMode = config.power.is_power_saving || isRouter;
// 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
bool isPowerSavingMode = config.power.is_power_saving || isRouter;
/* 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)
/* 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)
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 external power source (see `isVbusIn()` in `Power.cpp`)
*/
return !isPowerSavingMode && powerStatus && (!powerStatus->getHasBattery() || powerStatus->getHasUSB());
3) On some boards we don't have the power management chip (like AXPxxxx) so we use EXT_PWR_DETECT GPIO pin to detect
external power source (see `isVbusIn()` in `Power.cpp`)
*/
return !isPowerSavingMode && powerStatus && (!powerStatus->getHasBattery() || powerStatus->getHasUSB());
}
static void sdsEnter() {
LOG_POWERFSM("State: SDS");
// 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);
static void sdsEnter()
{
LOG_POWERFSM("State: SDS");
// 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);
}
static void lowBattSDSEnter() {
LOG_POWERFSM("State: Lower batt SDS");
doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, true);
static void lowBattSDSEnter()
{
LOG_POWERFSM("State: Lower batt SDS");
doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, true);
}
extern Power *power;
static void shutdownEnter() {
LOG_POWERFSM("State: SHUTDOWN");
shutdownAtMsec = millis();
static void shutdownEnter()
{
LOG_POWERFSM("State: SHUTDOWN");
shutdownAtMsec = millis();
}
#include "error.h"
static uint32_t secsSlept;
static void lsEnter() {
LOG_POWERFSM("lsEnter begin, ls_secs=%u", config.power.ls_secs);
if (screen)
screen->setOn(false);
secsSlept = 0; // How long have we been sleeping this time
static void lsEnter()
{
LOG_POWERFSM("lsEnter begin, ls_secs=%u", config.power.ls_secs);
if (screen)
screen->setOn(false);
secsSlept = 0; // How long have we been sleeping this time
// LOG_INFO("lsEnter end");
// LOG_INFO("lsEnter end");
}
static void lsIdle() {
// LOG_INFO("lsIdle begin ls_secs=%u", getPref_ls_secs());
static void lsIdle()
{
// LOG_INFO("lsIdle begin ls_secs=%u", getPref_ls_secs());
#ifdef ARCH_ESP32
// Do we have more sleeping to do?
if (secsSlept < config.power.ls_secs) {
// If some other service would stall sleep, don't let sleep happen yet
if (doPreflightSleep()) {
// Briefly come out of sleep long enough to blink the led once every few seconds
uint32_t sleepTime = SLEEP_TIME;
// Do we have more sleeping to do?
if (secsSlept < config.power.ls_secs) {
// If some other service would stall sleep, don't let sleep happen yet
if (doPreflightSleep()) {
// Briefly come out of sleep long enough to blink the led once every few seconds
uint32_t sleepTime = SLEEP_TIME;
powerMon->setState(meshtastic_PowerMon_State_CPU_LightSleep);
ledBlink.set(false); // Never leave led on while in light sleep
esp_sleep_source_t wakeCause2 = doLightSleep(sleepTime * 1000LL);
powerMon->clearState(meshtastic_PowerMon_State_CPU_LightSleep);
powerMon->setState(meshtastic_PowerMon_State_CPU_LightSleep);
ledBlink.set(false); // Never leave led on while in light sleep
esp_sleep_source_t wakeCause2 = doLightSleep(sleepTime * 1000LL);
powerMon->clearState(meshtastic_PowerMon_State_CPU_LightSleep);
switch (wakeCause2) {
case ESP_SLEEP_WAKEUP_TIMER:
// Normal case: timer expired, we should just go back to sleep ASAP
switch (wakeCause2) {
case ESP_SLEEP_WAKEUP_TIMER:
// Normal case: timer expired, we should just go back to sleep ASAP
ledBlink.set(true); // briefly turn on led
wakeCause2 = doLightSleep(100); // leave led on for 1ms
ledBlink.set(true); // briefly turn on led
wakeCause2 = doLightSleep(100); // leave led on for 1ms
secsSlept += sleepTime;
// LOG_INFO("Sleep, flash led!");
break;
secsSlept += sleepTime;
// LOG_INFO("Sleep, flash led!");
break;
case ESP_SLEEP_WAKEUP_UART:
// Not currently used (because uart triggers in hw have problems)
powerFSM.trigger(EVENT_SERIAL_CONNECTED);
break;
case ESP_SLEEP_WAKEUP_UART:
// Not currently used (because uart triggers in hw have problems)
powerFSM.trigger(EVENT_SERIAL_CONNECTED);
break;
default:
// We woke for some other reason (button press, device IRQ interrupt)
default:
// We woke for some other reason (button press, device IRQ interrupt)
#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
bool pressed = false;
bool pressed = false;
#endif
if (pressed) { // If we woke because of press, instead generate a PRESS event.
powerFSM.trigger(EVENT_PRESS);
if (pressed) { // If we woke because of press, instead generate a PRESS event.
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 {
// 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);
// Someone says we can't sleep now, so just save some power by sleeping the CPU for 100ms or so
delay(100);
}
break;
}
} else {
// Someone says we can't sleep now, so just save some power by sleeping the CPU for 100ms or so
delay(100);
// Time to stop sleeping!
ledBlink.set(false);
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
}
static void lsExit() { LOG_POWERFSM("State: lsExit"); }
static void nbEnter() {
LOG_POWERFSM("State: nbEnter");
if (screen)
screen->setOn(false);
#ifdef ARCH_ESP32
// Only ESP32 should turn off bluetooth
setBluetoothEnable(false);
#endif
// FIXME - check if we already have packets for phone and immediately trigger EVENT_PACKETS_FOR_PHONE
static void lsExit()
{
LOG_POWERFSM("State: lsExit");
}
static void darkEnter() {
LOG_POWERFSM("State: darkEnter");
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 {
static void nbEnter()
{
LOG_POWERFSM("State: nbEnter");
if (screen)
screen->setOn(true);
screen->setOn(false);
#ifdef ARCH_ESP32
// Only ESP32 should turn off bluetooth
setBluetoothEnable(false);
#endif
// FIXME - check if we already have packets for phone and immediately trigger EVENT_PACKETS_FOR_PHONE
}
static void darkEnter()
{
LOG_POWERFSM("State: darkEnter");
setBluetoothEnable(true);
// within enter() the function getState() returns the state we came from
}
if (screen)
screen->setOn(false);
}
static void powerIdle() {
// LOG_POWERFSM("State: powerIdle"); // very chatty
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 serialEnter()
{
LOG_POWERFSM("State: serialEnter");
setBluetoothEnable(false);
if (screen) {
screen->setOn(true);
}
}
static void powerExit() {
LOG_POWERFSM("State: powerExit");
setBluetoothEnable(true);
static void serialExit()
{
LOG_POWERFSM("State: serialExit");
// Turn bluetooth back on when we leave serial stream API
setBluetoothEnable(true);
}
static void onEnter() {
LOG_POWERFSM("State: onEnter");
if (screen)
screen->setOn(true);
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)
screen->setOn(true);
setBluetoothEnable(true);
// within enter() the function getState() returns the state we came from
}
}
static void onIdle() {
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 powerIdle()
{
// LOG_POWERFSM("State: powerIdle"); // very chatty
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 bootEnter() { LOG_POWERFSM("State: bootEnter"); }
static void powerExit()
{
LOG_POWERFSM("State: powerExit");
setBluetoothEnable(true);
}
static void onEnter()
{
LOG_POWERFSM("State: onEnter");
if (screen)
screen->setOn(true);
setBluetoothEnable(true);
}
static void onIdle()
{
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 stateSDS(sdsEnter, NULL, NULL, "SDS");
@@ -240,141 +260,147 @@ State stateON(onEnter, onIdle, NULL, "ON");
State statePOWER(powerEnter, powerIdle, powerExit, "POWER");
Fsm powerFSM(&stateBOOT);
void PowerFSM_setup() {
bool isRouter = (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER ? 1 : 0);
bool hasPower = isPowered();
void PowerFSM_setup()
{
bool isRouter = (config.device.role == meshtastic_Config_DeviceConfig_Role_ROUTER ? 1 : 0);
bool hasPower = isPowered();
LOG_INFO("PowerFSM init, USB power=%d", hasPower ? 1 : 0);
powerFSM.add_timed_transition(&stateBOOT, hasPower ? &statePOWER : &stateON, 3 * 1000, NULL, "boot timeout");
LOG_INFO("PowerFSM init, USB power=%d", hasPower ? 1 : 0);
powerFSM.add_timed_transition(&stateBOOT, hasPower ? &statePOWER : &stateON, 3 * 1000, NULL, "boot timeout");
// 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)
// 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)
#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
powerFSM.add_transition(&stateLS, &stateDARK, EVENT_WAKE_TIMER, NULL, "Wake timer");
powerFSM.add_transition(&stateLS, &stateDARK, EVENT_WAKE_TIMER, NULL, "Wake timer");
#endif
// We need this transition, because we might not transition if we were waiting to enter light-sleep, because when we
// wake from light sleep we _always_ transition to NB or dark and
powerFSM.add_transition(&stateLS, isRouter ? &stateNB : &stateDARK, EVENT_PACKET_FOR_PHONE, NULL, "Received packet, exiting light sleep");
powerFSM.add_transition(&stateNB, &stateNB, EVENT_PACKET_FOR_PHONE, NULL, "Received packet, resetting win wake");
// We need this transition, because we might not transition if we were waiting to enter light-sleep, because when we wake from
// light sleep we _always_ transition to NB or dark and
powerFSM.add_transition(&stateLS, isRouter ? &stateNB : &stateDARK, EVENT_PACKET_FOR_PHONE, NULL,
"Received packet, exiting light sleep");
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
powerFSM.add_transition(&stateLS, &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(&statePOWER, &statePOWER, EVENT_PRESS, NULL, "Press");
powerFSM.add_transition(&stateON, &stateON, EVENT_PRESS, NULL, "Press"); // reenter On to restart our timers
powerFSM.add_transition(&stateSERIAL, &stateSERIAL, EVENT_PRESS, NULL,
"Press"); // Allow button to work while in serial API
// Handle press events - note: we ignore button presses when in API mode
powerFSM.add_transition(&stateLS, &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(&statePOWER, &statePOWER, EVENT_PRESS, NULL, "Press");
powerFSM.add_transition(&stateON, &stateON, EVENT_PRESS, NULL, "Press"); // reenter On to restart our timers
powerFSM.add_transition(&stateSERIAL, &stateSERIAL, EVENT_PRESS, NULL,
"Press"); // Allow button to work while in serial API
// Handle critically low power battery by forcing deep sleep
powerFSM.add_transition(&stateBOOT, &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(&stateDARK, &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");
// Handle critically low power battery by forcing deep sleep
powerFSM.add_transition(&stateBOOT, &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(&stateDARK, &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");
// Handle being told to power off
powerFSM.add_transition(&stateBOOT, &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(&stateDARK, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateON, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateSERIAL, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
// Handle being told to power off
powerFSM.add_transition(&stateBOOT, &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(&stateDARK, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateON, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
powerFSM.add_transition(&stateSERIAL, &stateSHUTDOWN, EVENT_SHUTDOWN, NULL, "Shutdown");
// Inputbroker
powerFSM.add_transition(&stateLS, &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(&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
// Inputbroker
powerFSM.add_transition(&stateLS, &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(&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(&stateDARK, &stateON, EVENT_BLUETOOTH_PAIR, NULL, "Bluetooth pairing");
powerFSM.add_transition(&stateON, &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");
// if we are a router we don't turn the screen on for these things
if (!isRouter) {
// 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");
// if we are a router we don't turn the screen on for these things
if (!isRouter) {
// 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");
// Show the received text message
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(&stateDARK, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text");
powerFSM.add_transition(&stateON, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text"); // restarts the sleep timer
}
// Show the received text message
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(&stateDARK, &stateON, EVENT_RECEIVED_MSG, NULL, "Received text");
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
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(&stateDARK, &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");
// 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(&stateNB, &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(&statePOWER, &stateSERIAL, EVENT_SERIAL_CONNECTED, NULL, "serial API");
// 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(&stateNB, &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");
// 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(&stateNB, &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(&statePOWER, &stateON, EVENT_POWER_DISCONNECTED, NULL, "power disconnected");
// powerFSM.add_transition(&stateSERIAL, &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");
// 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)
powerFSM.add_transition(&stateSERIAL, &stateON, EVENT_SERIAL_DISCONNECTED, NULL, "serial disconnect");
// 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)
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
// Allow E-Ink devices to suppress the screensaver, if screen timeout set to 0
if (config.display.screen_on_secs > 0)
// Allow E-Ink devices to suppress the screensaver, if screen timeout set to 0
if (config.display.screen_on_secs > 0)
#endif
{
powerFSM.add_timed_transition(&stateON, &stateDARK, Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
NULL, "Screen-on timeout");
powerFSM.add_timed_transition(&statePOWER, &stateDARK, Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
NULL, "Screen-on timeout");
}
{
powerFSM.add_timed_transition(&stateON, &stateDARK,
Default::getConfiguredOrDefaultMs(config.display.screen_on_secs, default_screen_on_secs),
NULL, "Screen-on timeout");
powerFSM.add_timed_transition(&statePOWER, &stateDARK,
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)
#ifdef ARCH_ESP32
// 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 through the modules
// 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
// through the modules
#if HAS_WIFI && !defined(MESHTASTIC_EXCLUDE_WIFI)
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_SENSOR;
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_SENSOR;
if ((isRouter || config.power.is_power_saving) && !isWifiAvailable() && !isTrackerOrSensor) {
powerFSM.add_timed_transition(&stateNB, &stateLS, Default::getConfiguredOrDefaultMs(config.power.min_wake_secs, default_min_wake_secs), NULL,
"Min wake timeout");
if ((isRouter || config.power.is_power_saving) && !isWifiAvailable() && !isTrackerOrSensor) {
powerFSM.add_timed_transition(&stateNB, &stateLS,
Default::getConfiguredOrDefaultMs(config.power.min_wake_secs, default_min_wake_secs), NULL,
"Min wake timeout");
// 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
powerFSM.add_timed_transition(&stateDARK, &stateLS,
Default::getConfiguredOrDefaultMs(config.power.wait_bluetooth_secs, default_wait_bluetooth_secs), NULL,
"Bluetooth timeout");
} else {
// 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),
NULL, "Screen-on timeout");
}
// 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
powerFSM.add_timed_transition(
&stateDARK, &stateLS,
Default::getConfiguredOrDefaultMs(config.power.wait_bluetooth_secs, default_wait_bluetooth_secs), NULL,
"Bluetooth timeout");
} else {
// 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),
NULL, "Screen-on timeout");
}
#endif // HAS_WIFI || !defined(MESHTASTIC_EXCLUDE_WIFI)
#else // (not) ARCH_ESP32
// If not ESP32, light-sleep not used. 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),
NULL, "Screen-on timeout");
// If not ESP32, light-sleep not used. 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), NULL,
"Screen-on timeout");
#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
src/PowerFSM.h
+15 -14
View File
@@ -17,8 +17,7 @@
#define EVENT_RECEIVED_MSG 5
// #define EVENT_BOOT 6 // now done with a timed transition
#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_LOW_BATTERY 10 // Battery is critically low, go to sleep
#define EVENT_SERIAL_CONNECTED 11
@@ -30,19 +29,21 @@
#define EVENT_INPUT 17 // input broker wants something, we need to wake up and enable screen
#if MESHTASTIC_EXCLUDE_POWER_FSM
class FakeFsm {
public:
void trigger(int event) {
if (event == EVENT_SERIAL_CONNECTED) {
serialConnected = true;
} else if (event == EVENT_SERIAL_DISCONNECTED) {
serialConnected = false;
}
};
bool getState() { return serialConnected; };
class FakeFsm
{
public:
void trigger(int event)
{
if (event == EVENT_SERIAL_CONNECTED) {
serialConnected = true;
} else if (event == EVENT_SERIAL_DISCONNECTED) {
serialConnected = false;
}
};
bool getState() { return serialConnected; };
private:
bool serialConnected = false;
private:
bool serialConnected = false;
};
extern FakeFsm powerFSM;
void PowerFSM_setup();
src/PowerFSMThread.h
+26 -22
View File
@@ -6,36 +6,40 @@
#include "main.h"
#include "power.h"
namespace concurrency {
namespace concurrency
{
/// Wrapper to convert our powerFSM stuff into a 'thread'
class PowerFSMThread : public OSThread {
public:
// callback returns the period for the next callback invocation (or 0 if we should no longer be called)
PowerFSMThread() : OSThread("PowerFSM") {}
class PowerFSMThread : public OSThread
{
public:
// callback returns the period for the next callback invocation (or 0 if we should no longer be called)
PowerFSMThread() : OSThread("PowerFSM") {}
protected:
int32_t runOnce() override {
protected:
int32_t runOnce() override
{
#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
/// cpu for serial rx - FIXME)
const State *state = powerFSM.getState();
canSleep = (state != &statePOWER) && (state != &stateSERIAL);
/// 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)
const State *state = powerFSM.getState();
canSleep = (state != &statePOWER) && (state != &stateSERIAL);
if (powerStatus->getHasUSB()) {
timeLastPowered = millis();
} else if (config.power.on_battery_shutdown_after_secs > 0 && config.power.on_battery_shutdown_after_secs != UINT32_MAX &&
millis() > (timeLastPowered +
Default::getConfiguredOrDefaultMs(config.power.on_battery_shutdown_after_secs))) { // shutdown after 30 minutes unpowered
powerFSM.trigger(EVENT_SHUTDOWN);
}
if (powerStatus->getHasUSB()) {
timeLastPowered = millis();
} else if (config.power.on_battery_shutdown_after_secs > 0 && config.power.on_battery_shutdown_after_secs != UINT32_MAX &&
millis() > (timeLastPowered +
Default::getConfiguredOrDefaultMs(
config.power.on_battery_shutdown_after_secs))) { // shutdown after 30 minutes unpowered
powerFSM.trigger(EVENT_SHUTDOWN);
}
return 100;
return 100;
#else
return INT32_MAX;
return INT32_MAX;
#endif
}
}
};
} // namespace concurrency
src/PowerMon.cpp
+27 -20
View File
@@ -2,39 +2,46 @@
#include "NodeDB.h"
// Use the 'live' config flag to figure out if we should be showing this message
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
// bootloader as valid!!! Possibly a linker/gcc/bootloader bug somewhere?
return ((m & config.power.powermon_enables) ? true : false);
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 bootloader as
// valid!!! Possibly a linker/gcc/bootloader bug somewhere?
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
auto oldstates = states;
states |= state;
if (oldstates != states && is_power_enabled(state)) {
emitLog(reason);
}
auto oldstates = states;
states |= state;
if (oldstates != states && is_power_enabled(state)) {
emitLog(reason);
}
#endif
}
void PowerMon::clearState(_meshtastic_PowerMon_State state, const char *reason) {
void PowerMon::clearState(_meshtastic_PowerMon_State state, const char *reason)
{
#ifdef USE_POWERMON
auto oldstates = states;
states &= ~state;
if (oldstates != states && is_power_enabled(state)) {
emitLog(reason);
}
auto oldstates = states;
states &= ~state;
if (oldstates != states && is_power_enabled(state)) {
emitLog(reason);
}
#endif
}
void PowerMon::emitLog(const char *reason) {
void PowerMon::emitLog(const char *reason)
{
#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.
LOG_INFO("S:PM:0x%08lx,%s", (uint32_t)states, reason);
// 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);
#endif
}
PowerMon *powerMon;
void powerMonInit() { powerMon = new PowerMon(); }
void powerMonInit()
{
powerMon = new PowerMon();
}
src/PowerMon.h
+18 -17
View File
@@ -13,29 +13,30 @@
*
* For more information see the PowerMon docs.
*/
class PowerMon {
uint64_t states = 0UL;
class PowerMon
{
uint64_t states = 0UL;
friend class PowerStressModule;
friend class PowerStressModule;
/**
* If stress testing we always want all events logged
*/
bool force_enabled = false;
/**
* If stress testing we always want all events logged
*/
bool force_enabled = false;
public:
PowerMon() {}
public:
PowerMon() {}
// Mark entry/exit of a power consuming state
void setState(_meshtastic_PowerMon_State state, const char *reason = "");
void clearState(_meshtastic_PowerMon_State state, const char *reason = "");
// Mark entry/exit of a power consuming state
void setState(_meshtastic_PowerMon_State state, const char *reason = "");
void clearState(_meshtastic_PowerMon_State state, const char *reason = "");
private:
// Emit the coded log message
void emitLog(const char *reason);
private:
// Emit the coded log message
void emitLog(const char *reason);
// Use the 'live' config flag to figure out if we should be showing this message
bool is_power_enabled(uint64_t m);
// Use the 'live' config flag to figure out if we should be showing this message
bool is_power_enabled(uint64_t m);
};
extern PowerMon *powerMon;
src/PowerStatus.h
+80 -73
View File
@@ -3,7 +3,8 @@
#include "configuration.h"
#include <Arduino.h>
namespace meshtastic {
namespace meshtastic
{
/**
* A boolean where we have a third state of Unknown
@@ -11,84 +12,90 @@ namespace meshtastic {
enum OptionalBool { OptFalse = 0, OptTrue = 1, OptUnknown = 2 };
/// Describes the state of the Power system.
class PowerStatus : public Status {
class PowerStatus : public Status
{
private:
CallbackObserver<PowerStatus, const PowerStatus *> statusObserver =
CallbackObserver<PowerStatus, const PowerStatus *>(this, &PowerStatus::updateStatus);
private:
CallbackObserver<PowerStatus, const PowerStatus *> statusObserver =
CallbackObserver<PowerStatus, const PowerStatus *>(this, &PowerStatus::updateStatus);
/// Whether we have a battery connected
OptionalBool hasBattery = OptUnknown;
/// Battery voltage in mV, valid if haveBattery is true
int batteryVoltageMv = 0;
/// Battery charge percentage, either read directly or estimated
int8_t batteryChargePercent = 0;
/// Whether USB is connected
OptionalBool hasUSB = OptUnknown;
/// Whether we are charging the battery
OptionalBool isCharging = OptUnknown;
/// Whether we have a battery connected
OptionalBool hasBattery = OptUnknown;
/// Battery voltage in mV, valid if haveBattery is true
int batteryVoltageMv = 0;
/// Battery charge percentage, either read directly or estimated
int8_t batteryChargePercent = 0;
/// Whether USB is connected
OptionalBool hasUSB = OptUnknown;
/// Whether we are charging the battery
OptionalBool isCharging = OptUnknown;
public:
PowerStatus() { statusType = STATUS_TYPE_POWER; }
PowerStatus(OptionalBool hasBattery, OptionalBool hasUSB, OptionalBool isCharging, int batteryVoltageMv = -1, int8_t batteryChargePercent = 0)
: Status() {
this->hasBattery = hasBattery;
this->hasUSB = hasUSB;
this->isCharging = isCharging;
this->batteryVoltageMv = batteryVoltageMv;
this->batteryChargePercent = batteryChargePercent;
}
PowerStatus(const PowerStatus &);
PowerStatus &operator=(const PowerStatus &);
void observe(Observable<const PowerStatus *> *source) { statusObserver.observe(source); }
bool getHasBattery() const { return hasBattery == OptTrue; }
bool getHasUSB() const { return hasUSB == OptTrue; }
/// Can we even know if this board has USB power or not
bool knowsUSB() const { return hasUSB != OptUnknown; }
bool getIsCharging() const { return isCharging == OptTrue; }
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'
*/
#if defined(HAS_PMU) || defined(BATTERY_PIN)
uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 0; }
#endif
/**
* Note: for boards without battery pin and PMU, 101% battery means 'the board is using external power'
*/
#if !defined(HAS_PMU) && !defined(BATTERY_PIN)
uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 101; }
#endif
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;
public:
PowerStatus() { statusType = STATUS_TYPE_POWER; }
PowerStatus(OptionalBool hasBattery, OptionalBool hasUSB, OptionalBool isCharging, int batteryVoltageMv = -1,
int8_t batteryChargePercent = 0)
: Status()
{
isDirty = matches(newStatus);
initialized = true;
hasBattery = newStatus->hasBattery;
batteryVoltageMv = newStatus->getBatteryVoltageMv();
batteryChargePercent = newStatus->getBatteryChargePercent();
hasUSB = newStatus->hasUSB;
isCharging = newStatus->isCharging;
this->hasBattery = hasBattery;
this->hasUSB = hasUSB;
this->isCharging = isCharging;
this->batteryVoltageMv = batteryVoltageMv;
this->batteryChargePercent = batteryChargePercent;
}
if (isDirty) {
// LOG_DEBUG("Battery %dmV %d%%", batteryVoltageMv, batteryChargePercent);
onNewStatus.notifyObservers(this);
PowerStatus(const PowerStatus &);
PowerStatus &operator=(const PowerStatus &);
void observe(Observable<const PowerStatus *> *source) { statusObserver.observe(source); }
bool getHasBattery() const { return hasBattery == OptTrue; }
bool getHasUSB() const { return hasUSB == OptTrue; }
/// Can we even know if this board has USB power or not
bool knowsUSB() const { return hasUSB != OptUnknown; }
bool getIsCharging() const { return isCharging == OptTrue; }
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'
*/
#if defined(HAS_PMU) || defined(BATTERY_PIN)
uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 0; }
#endif
/**
* Note: for boards without battery pin and PMU, 101% battery means 'the board is using external power'
*/
#if !defined(HAS_PMU) && !defined(BATTERY_PIN)
uint8_t getBatteryChargePercent() const { return getHasBattery() ? batteryChargePercent : 101; }
#endif
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;
{
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
src/RedirectablePrint.cpp
+314 -303
View File
@@ -20,376 +20,387 @@
#if HAS_NETWORKING
extern Syslog syslog;
#endif
void RedirectablePrint::rpInit() {
void RedirectablePrint::rpInit()
{
#ifdef HAS_FREE_RTOS
inDebugPrint = xSemaphoreCreateMutexStatic(&this->_MutexStorageSpace);
inDebugPrint = xSemaphoreCreateMutexStatic(&this->_MutexStorageSpace);
#endif
}
void RedirectablePrint::setDestination(Print *_dest) {
assert(_dest);
dest = _dest;
void RedirectablePrint::setDestination(Print *_dest)
{
assert(_dest);
dest = _dest;
}
size_t RedirectablePrint::write(uint8_t c) {
// Always send the characters to our segger JTAG debugger
size_t RedirectablePrint::write(uint8_t c)
{
// Always send the characters to our segger JTAG debugger
#ifdef USE_SEGGER
SEGGER_RTT_PutChar(SEGGER_STDOUT_CH, c);
SEGGER_RTT_PutChar(SEGGER_STDOUT_CH, c);
#endif
// Account for legacy config transition
bool serialEnabled = config.has_security ? config.security.serial_enabled : config.device.serial_enabled;
if (!config.has_lora || serialEnabled)
dest->write(c);
// Account for legacy config transition
bool serialEnabled = config.has_security ? config.security.serial_enabled : config.device.serial_enabled;
if (!config.has_lora || serialEnabled)
dest->write(c);
return 1; // We always claim one was written, rather than trusting what the
// serial port said (which could be zero)
return 1; // We always claim one was written, rather than trusting what the
// serial port said (which could be zero)
}
size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_list arg) {
va_list copy;
size_t RedirectablePrint::vprintf(const char *logLevel, const char *format, va_list arg)
{
va_list copy;
#if ENABLE_JSON_LOGGING || ARCH_PORTDUINO
static char printBuf[512];
static char printBuf[512];
#else
static char printBuf[160];
static char printBuf[160];
#endif
#ifdef ARCH_PORTDUINO
bool color = !portduino_config.ascii_logs;
bool color = !portduino_config.ascii_logs;
#else
bool color = true;
bool color = true;
#endif
va_copy(copy, arg);
size_t len = vsnprintf(printBuf, sizeof(printBuf), format, copy);
va_end(copy);
va_copy(copy, arg);
size_t len = vsnprintf(printBuf, sizeof(printBuf), format, copy);
va_end(copy);
// If the resulting string is longer than sizeof(printBuf)-1 characters, the remaining characters are still counted
// for the return value
// If the resulting string is longer than sizeof(printBuf)-1 characters, the remaining characters are still counted for the
// return value
if (len > sizeof(printBuf) - 1) {
len = sizeof(printBuf) - 1;
printBuf[sizeof(printBuf) - 2] = '\n';
}
for (size_t f = 0; f < len; f++) {
if (!std::isprint(static_cast<unsigned char>(printBuf[f])) && printBuf[f] != '\n')
printBuf[f] = '#';
}
if (color && logLevel != nullptr) {
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);
}
len = Print::write(printBuf, len);
if (color && logLevel != nullptr) {
Print::write("\u001b[0m", 4);
}
return len;
if (len > sizeof(printBuf) - 1) {
len = sizeof(printBuf) - 1;
printBuf[sizeof(printBuf) - 2] = '\n';
}
for (size_t f = 0; f < len; f++) {
if (!std::isprint(static_cast<unsigned char>(printBuf[f])) && printBuf[f] != '\n')
printBuf[f] = '#';
}
if (color && logLevel != nullptr) {
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);
}
len = Print::write(printBuf, len);
if (color && logLevel != nullptr) {
Print::write("\u001b[0m", 4);
}
return len;
}
void RedirectablePrint::log_to_serial(const char *logLevel, const char *format, va_list arg) {
size_t r = 0;
void RedirectablePrint::log_to_serial(const char *logLevel, const char *format, va_list arg)
{
size_t r = 0;
#ifdef ARCH_PORTDUINO
bool color = !portduino_config.ascii_logs;
bool color = !portduino_config.ascii_logs;
#else
bool color = true;
bool color = true;
#endif
// 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);
}
// 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;
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
// 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);
::printf("%s ", logLevel);
if (color) {
::printf("\u001b[0m");
}
::printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
#else
printf("%s ", logLevel);
if (color) {
printf("\u001b[0m");
}
printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
printf("%s ", logLevel);
if (color) {
printf("\u001b[0m");
}
printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
#endif
} else {
} else {
#ifdef ARCH_PORTDUINO
::printf("%s ", logLevel);
if (color) {
::printf("\u001b[0m");
}
::printf("| ??:??:?? %u ", millis() / 1000);
::printf("%s ", logLevel);
if (color) {
::printf("\u001b[0m");
}
::printf("| ??:??:?? %u ", millis() / 1000);
#else
printf("%s ", logLevel);
if (color) {
printf("\u001b[0m");
}
printf("| ??:??:?? %u ", millis() / 1000);
printf("%s ", logLevel);
if (color) {
printf("\u001b[0m");
}
printf("| ??:??:?? %u ", millis() / 1000);
#endif
}
auto thread = concurrency::OSThread::currentThread;
if (thread) {
print("[");
// printf("%p ", thread);
// assert(thread->ThreadName.length());
print(thread->ThreadName);
print("] ");
}
#ifdef DEBUG_HEAP
// Add heap free space bytes prefix before every log message
#ifdef ARCH_PORTDUINO
::printf("[heap %u] ", memGet.getFreeHeap());
#else
printf("[heap %u] ", memGet.getFreeHeap());
#endif
#endif // DEBUG_HEAP
r += vprintf(logLevel, format, arg);
}
void RedirectablePrint::log_to_syslog(const char *logLevel, const char *format, va_list arg) {
#if HAS_NETWORKING && !defined(ARCH_PORTDUINO)
// if syslog is in use, collect the log messages and send them to syslog
if (syslog.isEnabled()) {
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);
print("[");
// printf("%p ", thread);
// assert(thread->ThreadName.length());
print(thread->ThreadName);
print("] ");
}
}
#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;
}
}
#ifdef DEBUG_HEAP
// Add heap free space bytes prefix before every log message
#ifdef ARCH_PORTDUINO
::printf("[heap %u] ", memGet.getFreeHeap());
#else
(void)logLevel;
(void)format;
(void)arg;
printf("[heap %u] ", memGet.getFreeHeap());
#endif
#endif // DEBUG_HEAP
r += vprintf(logLevel, format, arg);
}
void RedirectablePrint::log_to_syslog(const char *logLevel, const char *format, va_list arg)
{
#if HAS_NETWORKING && !defined(ARCH_PORTDUINO)
// if syslog is in use, collect the log messages and send them to syslog
if (syslog.isEnabled()) {
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
}
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_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
}
void RedirectablePrint::log(const char *logLevel, const char *format, ...) {
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;
}
// 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';
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
// level trace is special, two possible ways to handle it.
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) {
if (portduino_config.traceFilename != "") {
va_list arg;
va_start(arg, format);
try {
traceFile << va_arg(arg, char *) << std::endl;
} catch (const std::ios_base::failure &e) {
}
va_end(arg);
// level trace is special, two possible ways to handle it.
if (strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) {
if (portduino_config.traceFilename != "") {
va_list arg;
va_start(arg, format);
try {
traceFile << va_arg(arg, char *) << std::endl;
} catch (const std::ios_base::failure &e) {
}
va_end(arg);
}
if (portduino_config.logoutputlevel < level_trace && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_TRACE) == 0) {
delete[] newFormat;
return;
}
}
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) {
delete[] newFormat;
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;
}
}
if (portduino_config.logoutputlevel < level_debug && strcmp(logLevel, MESHTASTIC_LOG_LEVEL_DEBUG) == 0) {
delete[] newFormat;
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;
}
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) {
if (inDebugPrint != nullptr && xSemaphoreTake(inDebugPrint, portMAX_DELAY) == pdTRUE) {
#else
if (!inDebugPrint) {
inDebugPrint = true;
if (!inDebugPrint) {
inDebugPrint = true;
#endif
va_list arg;
va_start(arg, format);
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);
log_to_serial(logLevel, newFormat, arg);
log_to_syslog(logLevel, newFormat, arg);
log_to_ble(logLevel, newFormat, arg);
va_end(arg);
va_end(arg);
#ifdef HAS_FREE_RTOS
xSemaphoreGive(inDebugPrint);
xSemaphoreGive(inDebugPrint);
#else
inDebugPrint = false;
inDebugPrint = false;
#endif
}
}
delete[] newFormat;
return;
delete[] newFormat;
return;
}
void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len) {
const char alphabet[17] = "0123456789abcdef";
log(logLevel, " +------------------------------------------------+ +----------------+");
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) {
if (i % 128 == 0)
log(logLevel, " +------------------------------------------------+ +----------------+");
char s[] = " | | | |\n";
uint8_t ix = 5, iy = 56;
for (uint8_t j = 0; j < 16; j++) {
if (i + j < len) {
uint8_t c = buf[i + j];
s[ix++] = alphabet[(c >> 4) & 0x0F];
s[ix++] = alphabet[c & 0x0F];
ix++;
if (c > 31 && c < 128)
s[iy++] = c;
void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len)
{
const char alphabet[17] = "0123456789abcdef";
log(logLevel, " +------------------------------------------------+ +----------------+");
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) {
if (i % 128 == 0)
log(logLevel, " +------------------------------------------------+ +----------------+");
char s[] = " | | | |\n";
uint8_t ix = 5, iy = 56;
for (uint8_t j = 0; j < 16; j++) {
if (i + j < len) {
uint8_t c = buf[i + j];
s[ix++] = alphabet[(c >> 4) & 0x0F];
s[ix++] = alphabet[c & 0x0F];
ix++;
if (c > 31 && c < 128)
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
s[iy++] = '.';
}
break;
}
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());
return std::string(formatted.get());
}
src/RedirectablePrint.h
+33 -32
View File
@@ -11,48 +11,49 @@
* 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.
*/
class RedirectablePrint : public Print {
Print *dest;
class RedirectablePrint : public Print
{
Print *dest;
#ifdef HAS_FREE_RTOS
SemaphoreHandle_t inDebugPrint = nullptr;
StaticSemaphore_t _MutexStorageSpace;
SemaphoreHandle_t inDebugPrint = nullptr;
StaticSemaphore_t _MutexStorageSpace;
#else
volatile bool inDebugPrint = false;
volatile bool inDebugPrint = false;
#endif
public:
explicit RedirectablePrint(Print *_dest) : dest(_dest) {}
public:
explicit RedirectablePrint(Print *_dest) : dest(_dest) {}
/**
* Set a new destination
*/
void rpInit();
void setDestination(Print *dest);
/**
* Set a new destination
*/
void rpInit();
void setDestination(Print *dest);
virtual size_t write(uint8_t c);
virtual size_t write(uint8_t c);
/**
* Debug logging print message
*
* 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
* 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)));
/**
* Debug logging print message
*
* 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
* 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)));
/** like printf but va_list based */
size_t vprintf(const char *logLevel, const char *format, va_list arg);
/** like printf but va_list based */
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:
/// 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);
meshtastic_LogRecord_Level getLogLevel(const char *logLevel);
protected:
/// 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);
meshtastic_LogRecord_Level getLogLevel(const char *logLevel);
private:
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);
private:
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);
};
src/SPILock.cpp
+4 -3
View File
@@ -5,7 +5,8 @@
concurrency::Lock *spiLock;
void initSPI() {
assert(!spiLock);
spiLock = new concurrency::Lock();
void initSPI()
{
assert(!spiLock);
spiLock = new concurrency::Lock();
}
src/SafeFile.cpp
+83 -75
View File
@@ -3,48 +3,54 @@
#ifdef FSCom
// Only way to work on both esp32 and nrf52
static File openFile(const char *filename, bool fullAtomic) {
concurrency::LockGuard g(spiLock);
LOG_DEBUG("Opening %s, fullAtomic=%d", filename, fullAtomic);
static File openFile(const char *filename, bool fullAtomic)
{
concurrency::LockGuard g(spiLock);
LOG_DEBUG("Opening %s, fullAtomic=%d", filename, fullAtomic);
#ifdef ARCH_NRF52
FSCom.remove(filename);
return FSCom.open(filename, FILE_O_WRITE);
FSCom.remove(filename);
return FSCom.open(filename, FILE_O_WRITE);
#endif
if (!fullAtomic) {
FSCom.remove(filename); // Nuke the old file to make space (ignore if it !exists)
}
if (!fullAtomic) {
FSCom.remove(filename); // Nuke the old file to make space (ignore if it !exists)
}
String filenameTmp = filename;
filenameTmp += ".tmp";
String filenameTmp = filename;
filenameTmp += ".tmp";
// FIXME: If we are doing a full atomic write, we may need to remove the old tmp file now
// if (fullAtomic) {
// FSCom.remove(filename);
// }
// FIXME: If we are doing a full atomic write, we may need to remove the old tmp file now
// if (fullAtomic) {
// FSCom.remove(filename);
// }
// clear any previous LFS errors
return FSCom.open(filenameTmp.c_str(), FILE_O_WRITE);
// clear any previous LFS errors
return FSCom.open(filenameTmp.c_str(), FILE_O_WRITE);
}
SafeFile::SafeFile(const char *_filename, bool 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);
SafeFile::SafeFile(const char *_filename, bool fullAtomic)
: filename(_filename), f(openFile(_filename, fullAtomic)), fullAtomic(fullAtomic)
{
}
size_t SafeFile::write(const uint8_t *buffer, size_t size) {
if (!f)
return 0;
size_t SafeFile::write(uint8_t ch)
{
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)
hash ^= ch;
return f.write(ch);
}
size_t SafeFile::write(const uint8_t *buffer, size_t size)
{
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)
}
/**
@@ -52,64 +58,66 @@ size_t SafeFile::write(const uint8_t *buffer, size_t size) {
*
* @return false for failure
*/
bool SafeFile::close() {
if (!f)
return false;
bool SafeFile::close()
{
if (!f)
return false;
spiLock->lock();
f.close();
spiLock->unlock();
spiLock->lock();
f.close();
spiLock->unlock();
#ifdef ARCH_NRF52
return true;
return true;
#endif
if (!testReadback())
return false;
if (!testReadback())
return false;
{ // Scope for lock
concurrency::LockGuard g(spiLock);
// brief window of risk here ;-)
if (fullAtomic && FSCom.exists(filename.c_str()) && !FSCom.remove(filename.c_str())) {
LOG_ERROR("Can't remove old pref file");
return false;
{ // Scope for lock
concurrency::LockGuard g(spiLock);
// brief window of risk here ;-)
if (fullAtomic && FSCom.exists(filename.c_str()) && !FSCom.remove(filename.c_str())) {
LOG_ERROR("Can't remove old pref file");
return false;
}
}
}
String filenameTmp = filename;
filenameTmp += ".tmp";
if (!renameFile(filenameTmp.c_str(), filename.c_str())) {
LOG_ERROR("Error: can't rename new pref file");
return false;
}
String filenameTmp = filename;
filenameTmp += ".tmp";
if (!renameFile(filenameTmp.c_str(), filename.c_str())) {
LOG_ERROR("Error: can't rename new pref file");
return false;
}
return true;
return true;
}
/// Read our (closed) tempfile back in and compare the hash
bool SafeFile::testReadback() {
concurrency::LockGuard g(spiLock);
bool SafeFile::testReadback()
{
concurrency::LockGuard g(spiLock);
String filenameTmp = filename;
filenameTmp += ".tmp";
auto f2 = FSCom.open(filenameTmp.c_str(), FILE_O_READ);
if (!f2) {
LOG_ERROR("Can't open tmp file for readback");
return false;
}
String filenameTmp = filename;
filenameTmp += ".tmp";
auto f2 = FSCom.open(filenameTmp.c_str(), FILE_O_READ);
if (!f2) {
LOG_ERROR("Can't open tmp file for readback");
return false;
}
int c = 0;
uint8_t test_hash = 0;
while ((c = f2.read()) >= 0) {
test_hash ^= (uint8_t)c;
}
f2.close();
int c = 0;
uint8_t test_hash = 0;
while ((c = f2.read()) >= 0) {
test_hash ^= (uint8_t)c;
}
f2.close();
if (test_hash != hash) {
LOG_ERROR("Readback failed hash mismatch");
return false;
}
if (test_hash != hash) {
LOG_ERROR("Readback failed hash mismatch");
return false;
}
return true;
return true;
}
#endif
src/SafeFile.h
+26 -26
View File
@@ -10,41 +10,41 @@
* 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
* 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:
* - 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 provide an close() method which is similar to close but returns false if we were unable to successfully write
* 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 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 then we still do the readback to verify file is valid so higher level code can handle failures.
* - we provide an close() method which is similar to close but returns false if we were unable to successfully write 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
* 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
* then we still do the readback to verify file is valid so higher level code can handle failures.
*/
class SafeFile : public Print {
public:
explicit SafeFile(char const *filepath, bool fullAtomic = false);
class SafeFile : public Print
{
public:
explicit SafeFile(char const *filepath, bool fullAtomic = false);
virtual size_t write(uint8_t);
virtual size_t write(const uint8_t *buffer, size_t size);
virtual size_t write(uint8_t);
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
*
* @return false for failure
*/
bool close();
/**
* Atomically close the file (deleting any old versions) and readback the contents to confirm the hash matches
*
* @return false for failure
*/
bool close();
private:
/// Read our (closed) tempfile back in and compare the hash
bool testReadback();
private:
/// Read our (closed) tempfile back in and compare the hash
bool testReadback();
String filename;
File f;
bool fullAtomic;
uint8_t hash = 0;
String filename;
File f;
bool fullAtomic;
uint8_t hash = 0;
};
#endif
src/SerialConsole.cpp
+78 -62
View File
@@ -28,105 +28,121 @@
SerialConsole *console;
void consoleInit() {
auto sc = new SerialConsole(); // Must be dynamically allocated because we are now inheriting from thread
void consoleInit()
{
auto sc = new SerialConsole(); // Must be dynamically allocated because we are now inheriting from thread
#if defined(SERIAL_HAS_ON_RECEIVE)
// onReceive does only exist for HardwareSerial not for USB CDC serial
Port.onReceive([sc]() { sc->rxInt(); });
// onReceive does only exist for HardwareSerial not for USB CDC serial
Port.onReceive([sc]() { sc->rxInt(); });
#endif
DEBUG_PORT.rpInit(); // Simply sets up semaphore
DEBUG_PORT.rpInit(); // Simply sets up semaphore
}
void consolePrintf(const char *format, ...) {
va_list arg;
va_start(arg, format);
console->vprintf(nullptr, format, arg);
va_end(arg);
console->flush();
void consolePrintf(const char *format, ...)
{
va_list arg;
va_start(arg, format);
console->vprintf(nullptr, format, arg);
va_end(arg);
console->flush();
}
SerialConsole::SerialConsole() : StreamAPI(&Port), RedirectablePrint(&Port), concurrency::OSThread("SerialConsole") {
api_type = TYPE_SERIAL;
assert(!console);
console = this;
canWrite = false; // We don't send packets to our port until it has talked to us first
SerialConsole::SerialConsole() : StreamAPI(&Port), RedirectablePrint(&Port), concurrency::OSThread("SerialConsole")
{
api_type = TYPE_SERIAL;
assert(!console);
console = this;
canWrite = false; // We don't send packets to our port until it has talked to us first
#ifdef RP2040_SLOW_CLOCK
Port.setTX(SERIAL2_TX);
Port.setRX(SERIAL2_RX);
Port.setTX(SERIAL2_TX);
Port.setRX(SERIAL2_RX);
#endif
Port.begin(SERIAL_BAUD);
#if defined(ARCH_NRF52) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) || defined(ARCH_RP2040) || \
Port.begin(SERIAL_BAUD);
#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)
time_t timeout = millis();
while (!Port) {
if (Throttle::isWithinTimespanMs(timeout, FIVE_SECONDS_MS)) {
delay(100);
} else {
break;
time_t timeout = millis();
while (!Port) {
if (Throttle::isWithinTimespanMs(timeout, FIVE_SECONDS_MS)) {
delay(100);
} else {
break;
}
}
}
#endif
#if !ARCH_PORTDUINO
emitRebooted();
emitRebooted();
#endif
}
int32_t SerialConsole::runOnce() {
int32_t SerialConsole::runOnce()
{
#ifdef HELTEC_MESH_SOLAR
// After enabling the mesh solar serial port module configuration, command processing is handled by the serial port
// module.
if (moduleConfig.serial.enabled && moduleConfig.serial.override_console_serial_port &&
moduleConfig.serial.mode == meshtastic_ModuleConfig_SerialConfig_Serial_Mode_MS_CONFIG) {
return 250;
}
// After enabling the mesh solar serial port module configuration, command processing is handled by the serial port module.
if (moduleConfig.serial.enabled && moduleConfig.serial.override_console_serial_port &&
moduleConfig.serial.mode == meshtastic_ModuleConfig_SerialConfig_Serial_Mode_MS_CONFIG) {
return 250;
}
#endif
int32_t delay = runOncePart();
int32_t delay = runOncePart();
#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)
return HWCDC::isPlugged() ? delay : (1000 * 20);
return HWCDC::isPlugged() ? delay : (1000 * 20);
#else
return delay;
return delay;
#endif
}
void SerialConsole::flush() { Port.flush(); }
void SerialConsole::flush()
{
Port.flush();
}
// trigger tx of serial data
void SerialConsole::onNowHasData(uint32_t fromRadioNum) { setIntervalFromNow(0); }
void SerialConsole::onNowHasData(uint32_t fromRadioNum)
{
setIntervalFromNow(0);
}
// trigger rx of serial data
void SerialConsole::rxInt() { setIntervalFromNow(0); }
void SerialConsole::rxInt()
{
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
bool SerialConsole::checkIsConnected() { return Throttle::isWithinTimespanMs(lastContactMsec, SERIAL_CONNECTION_TIMEOUT); }
// For the serial port we can't really detect if any client is on the other side, so instead just look for recent messages
bool SerialConsole::checkIsConnected()
{
return Throttle::isWithinTimespanMs(lastContactMsec, SERIAL_CONNECTION_TIMEOUT);
}
/**
* we override this to notice when we've received a protobuf over the serial
* stream. Then we shut off debug serial output.
*/
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.
if (config.has_lora && config.security.serial_enabled) {
// Switch to protobufs for log messages
usingProtobufs = true;
canWrite = true;
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.
if (config.has_lora && config.security.serial_enabled) {
// Switch to protobufs for log messages
usingProtobufs = true;
canWrite = true;
return StreamAPI::handleToRadio(buf, len);
} else {
return false;
}
return StreamAPI::handleToRadio(buf, len);
} else {
return false;
}
}
void SerialConsole::log_to_serial(const char *logLevel, const char *format, va_list arg) {
if (usingProtobufs && config.security.debug_log_api_enabled) {
meshtastic_LogRecord_Level ll = RedirectablePrint::getLogLevel(logLevel);
auto thread = concurrency::OSThread::currentThread;
emitLogRecord(ll, thread ? thread->ThreadName.c_str() : "", format, arg);
} else
RedirectablePrint::log_to_serial(logLevel, format, arg);
void SerialConsole::log_to_serial(const char *logLevel, const char *format, va_list arg)
{
if (usingProtobufs && config.security.debug_log_api_enabled) {
meshtastic_LogRecord_Level ll = RedirectablePrint::getLogLevel(logLevel);
auto thread = concurrency::OSThread::currentThread;
emitLogRecord(ll, thread ? thread->ThreadName.c_str() : "", format, arg);
} else
RedirectablePrint::log_to_serial(logLevel, format, arg);
}
src/SerialConsole.h
+28 -26
View File
@@ -6,40 +6,42 @@
* 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).
*/
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.
*/
bool usingProtobufs = false;
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.
*/
bool usingProtobufs = false;
public:
SerialConsole();
public:
SerialConsole();
/**
* we override this to notice when we've received a protobuf over the serial stream. Then we shunt off
* debug serial output.
*/
virtual bool handleToRadio(const uint8_t *buf, size_t len) override;
/**
* we override this to notice when we've received a protobuf over the serial stream. Then we shunt off
* debug serial output.
*/
virtual bool handleToRadio(const uint8_t *buf, size_t len) override;
virtual size_t write(uint8_t c) override {
if (c == '\n') // prefix any newlines with carriage return
RedirectablePrint::write('\r');
return RedirectablePrint::write(c);
}
virtual size_t write(uint8_t c) override
{
if (c == '\n') // prefix any newlines with carriage return
RedirectablePrint::write('\r');
return RedirectablePrint::write(c);
}
virtual int32_t runOnce() override;
virtual int32_t runOnce() override;
void flush();
void rxInt();
void flush();
void rxInt();
protected:
/// Check the current underlying physical link to see if the client is currently connected
virtual bool checkIsConnected() override;
protected:
/// Check the current underlying physical link to see if the client is currently connected
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
virtual void log_to_serial(const char *logLevel, const char *format, va_list arg);
/// 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);
};
// A simple wrapper to allow non class aware code write to the console
src/Status.h
+32 -28
View File
@@ -9,45 +9,49 @@
#define STATUS_TYPE_NODE 3
#define STATUS_TYPE_BLUETOOTH 4
namespace meshtastic {
namespace meshtastic
{
// A base class for observable status
class Status {
protected:
// Allows us to observe an Observable
CallbackObserver<Status, const Status *> statusObserver = CallbackObserver<Status, const Status *>(this, &Status::updateStatus);
bool initialized = false;
// Workaround for no typeid support
int statusType = 0;
class Status
{
protected:
// Allows us to observe an Observable
CallbackObserver<Status, const Status *> statusObserver =
CallbackObserver<Status, const Status *>(this, &Status::updateStatus);
bool initialized = false;
// Workaround for no typeid support
int statusType = 0;
public:
// Allows us to generate observable events
Observable<const Status *> onNewStatus;
public:
// Allows us to generate observable events
Observable<const Status *> onNewStatus;
// Enable polymorphism ?
virtual ~Status() = default;
// Enable polymorphism ?
virtual ~Status() = default;
Status() {
if (!statusType) {
statusType = STATUS_TYPE_BASE;
Status()
{
if (!statusType) {
statusType = STATUS_TYPE_BASE;
}
}
}
// Prevent object copy/move
Status(const Status &) = delete;
Status &operator=(const Status &) = delete;
// Prevent object copy/move
Status(const Status &) = delete;
Status &operator=(const Status &) = delete;
// Start observing a source of data
void observe(Observable<const Status *> *source) { statusObserver.observe(source); }
// Start observing a source of data
void observe(Observable<const Status *> *source) { statusObserver.observe(source); }
// Determines whether or not existing data matches the data in another Status instance
bool matches(const Status *otherStatus) const { return true; }
// Determines whether or not existing data matches the data in another Status instance
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
int updateStatus(const Status *newStatus) { return 0; }
// Called when the Observable we're observing generates a new notification
int updateStatus(const Status *newStatus) { return 0; }
};
}; // namespace meshtastic
src/airtime.cpp
+170 -143
View File
@@ -9,175 +9,202 @@ AirTime *airTime = NULL;
uint32_t air_period_tx[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) {
LOG_DEBUG("Packet TX: %ums", airtime_ms);
this->airtimes.periodTX[0] = this->airtimes.periodTX[0] + airtime_ms;
air_period_tx[0] = air_period_tx[0] + airtime_ms;
if (reportType == TX_LOG) {
LOG_DEBUG("Packet TX: %ums", airtime_ms);
this->airtimes.periodTX[0] = this->airtimes.periodTX[0] + airtime_ms;
air_period_tx[0] = air_period_tx[0] + airtime_ms;
this->utilizationTX[this->getPeriodUtilHour()] = this->utilizationTX[this->getPeriodUtilHour()] + airtime_ms;
} else if (reportType == RX_LOG) {
LOG_DEBUG("Packet RX: %ums", airtime_ms);
this->airtimes.periodRX[0] = this->airtimes.periodRX[0] + airtime_ms;
air_period_rx[0] = air_period_rx[0] + airtime_ms;
} else if (reportType == RX_ALL_LOG) {
LOG_DEBUG("Packet RX (noise?) : %ums", 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];
this->utilizationTX[this->getPeriodUtilHour()] = this->utilizationTX[this->getPeriodUtilHour()] + airtime_ms;
} else if (reportType == RX_LOG) {
LOG_DEBUG("Packet RX: %ums", airtime_ms);
this->airtimes.periodRX[0] = this->airtimes.periodRX[0] + airtime_ms;
air_period_rx[0] = air_period_rx[0] + airtime_ms;
} else if (reportType == RX_ALL_LOG) {
LOG_DEBUG("Packet RX (noise?) : %ums", airtime_ms);
this->airtimes.periodRX_ALL[0] = this->airtimes.periodRX_ALL[0] + airtime_ms;
}
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();
}
// Log all airtime type for channel utilization
this->channelUtilization[this->getPeriodUtilMinute()] = channelUtilization[this->getPeriodUtilMinute()] + airtime_ms;
}
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::currentPeriodIndex()
{
return ((getSecondsSinceBoot() / SECONDS_PER_PERIOD) % PERIODS_TO_LOG);
}
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;
uint8_t AirTime::getPeriodUtilMinute()
{
return (getSecondsSinceBoot() / 10) % CHANNEL_UTILIZATION_PERIODS;
}
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;
uint8_t AirTime::getPeriodUtilHour()
{
return (getSecondsSinceBoot() / 60) % MINUTES_IN_HOUR;
}
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;
}
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];
}
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();
}
}
bool AirTime::isTxAllowedAirUtil() {
if (!config.lora.override_duty_cycle && myRegion->dutyCycle < 100) {
if (utilizationTXPercent() < myRegion->dutyCycle * polite_duty_cycle_percent / 100) {
return true;
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("TX air util. >%f%%. Skip send", myRegion->dutyCycle * polite_duty_cycle_percent / 100);
return false;
LOG_WARN("Ch. util >%d%%. Skip send", percentage);
return false;
}
}
return true;
}
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
uint8_t AirTime::getSilentMinutes(float txPercent, float dutyCycle) {
float newTxPercent = txPercent;
for (int8_t i = MINUTES_IN_HOUR - 1; i >= 0; --i) {
newTxPercent -= ((float)this->utilizationTX[i] / (MS_IN_MINUTE * MINUTES_IN_HOUR / 100));
if (newTxPercent < dutyCycle)
return MINUTES_IN_HOUR - 1 - i;
}
uint8_t AirTime::getSilentMinutes(float txPercent, float dutyCycle)
{
float newTxPercent = txPercent;
for (int8_t i = MINUTES_IN_HOUR - 1; i >= 0; --i) {
newTxPercent -= ((float)this->utilizationTX[i] / (MS_IN_MINUTE * MINUTES_IN_HOUR / 100));
if (newTxPercent < dutyCycle)
return MINUTES_IN_HOUR - 1 - i;
}
return MINUTES_IN_HOUR;
return MINUTES_IN_HOUR;
}
AirTime::AirTime() : concurrency::OSThread("AirTime"), airtimes({}) {}
int32_t AirTime::runOnce() {
secSinceBoot++;
int32_t AirTime::runOnce()
{
secSinceBoot++;
uint8_t utilPeriod = this->getPeriodUtilMinute();
uint8_t utilPeriodTX = this->getPeriodUtilHour();
uint8_t utilPeriod = this->getPeriodUtilMinute();
uint8_t utilPeriodTX = this->getPeriodUtilHour();
if (firstTime) {
if (firstTime) {
// Init utilizationTX window to all 0
for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) {
this->utilizationTX[i] = 0;
// Init utilizationTX window to all 0
for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) {
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;
}
}
// 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);
return (1000 * 1);
}
src/airtime.h
+37 -36
View File
@@ -39,50 +39,51 @@ void logAirtime(reportTypes reportType, uint32_t airtime_ms);
uint32_t *airtimeReport(reportTypes reportType);
class AirTime : private concurrency::OSThread {
class AirTime : private concurrency::OSThread
{
public:
AirTime();
public:
AirTime();
void logAirtime(reportTypes reportType, uint32_t airtime_ms);
float channelUtilizationPercent();
float utilizationTXPercent();
void logAirtime(reportTypes reportType, uint32_t airtime_ms);
float channelUtilizationPercent();
float utilizationTXPercent();
float UtilizationPercentTX();
uint32_t channelUtilization[CHANNEL_UTILIZATION_PERIODS] = {0};
uint32_t utilizationTX[MINUTES_IN_HOUR] = {0};
float UtilizationPercentTX();
uint32_t channelUtilization[CHANNEL_UTILIZATION_PERIODS] = {0};
uint32_t utilizationTX[MINUTES_IN_HOUR] = {0};
void airtimeRotatePeriod();
uint8_t getPeriodsToLog();
uint32_t getSecondsPerPeriod();
uint32_t getSecondsSinceBoot();
uint32_t *airtimeReport(reportTypes reportType);
uint8_t getSilentMinutes(float txPercent, float dutyCycle);
bool isTxAllowedChannelUtil(bool polite = false);
bool isTxAllowedAirUtil();
void airtimeRotatePeriod();
uint8_t getPeriodsToLog();
uint32_t getSecondsPerPeriod();
uint32_t getSecondsSinceBoot();
uint32_t *airtimeReport(reportTypes reportType);
uint8_t getSilentMinutes(float txPercent, float dutyCycle);
bool isTxAllowedChannelUtil(bool polite = false);
bool isTxAllowedAirUtil();
private:
bool firstTime = true;
uint8_t lastUtilPeriod = 0;
uint8_t lastUtilPeriodTX = 0;
uint32_t secSinceBoot = 0;
uint8_t max_channel_util_percent = 40;
uint8_t polite_channel_util_percent = 25;
uint8_t polite_duty_cycle_percent = 50; // half of Duty Cycle allowance is ok for metadata
private:
bool firstTime = true;
uint8_t lastUtilPeriod = 0;
uint8_t lastUtilPeriodTX = 0;
uint32_t secSinceBoot = 0;
uint8_t max_channel_util_percent = 40;
uint8_t polite_channel_util_percent = 25;
uint8_t polite_duty_cycle_percent = 50; // half of Duty Cycle allowance is ok for metadata
struct airtimeStruct {
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_ALL[PERIODS_TO_LOG]; // AirTime received regardless of valid mesh packet. Could include noise.
uint8_t lastPeriodIndex;
} airtimes;
struct airtimeStruct {
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_ALL[PERIODS_TO_LOG]; // AirTime received regardless of valid mesh packet. Could include noise.
uint8_t lastPeriodIndex;
} airtimes;
uint8_t getPeriodUtilMinute();
uint8_t getPeriodUtilHour();
uint8_t currentPeriodIndex();
uint8_t getPeriodUtilMinute();
uint8_t getPeriodUtilHour();
uint8_t currentPeriodIndex();
protected:
virtual int32_t runOnce() override;
protected:
virtual int32_t runOnce() override;
};
extern AirTime *airTime;
src/buzz/BuzzerFeedbackThread.cpp
+52 -50
View File
@@ -5,58 +5,60 @@
BuzzerFeedbackThread *buzzerFeedbackThread;
BuzzerFeedbackThread::BuzzerFeedbackThread() {
if (inputBroker)
inputObserver.observe(inputBroker);
BuzzerFeedbackThread::BuzzerFeedbackThread()
{
if (inputBroker)
inputObserver.observe(inputBroker);
}
int BuzzerFeedbackThread::handleInputEvent(const InputEvent *event) {
// Only provide feedback if buzzer is enabled for notifications
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_DIRECT_MSG_ONLY) {
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();
int BuzzerFeedbackThread::handleInputEvent(const InputEvent *event)
{
// Only provide feedback if buzzer is enabled for notifications
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_DIRECT_MSG_ONLY) {
return 0; // Let other handlers process the event
}
break;
}
return 0; // Allow other handlers to 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;
}
return 0; // Allow other handlers to process the event
}
src/buzz/BuzzerFeedbackThread.h
+7 -6
View File
@@ -4,13 +4,14 @@
#include "concurrency/OSThread.h"
#include "input/InputBroker.h"
class BuzzerFeedbackThread {
CallbackObserver<BuzzerFeedbackThread, const InputEvent *> inputObserver =
CallbackObserver<BuzzerFeedbackThread, const InputEvent *>(this, &BuzzerFeedbackThread::handleInputEvent);
class BuzzerFeedbackThread
{
CallbackObserver<BuzzerFeedbackThread, const InputEvent *> inputObserver =
CallbackObserver<BuzzerFeedbackThread, const InputEvent *>(this, &BuzzerFeedbackThread::handleInputEvent);
public:
BuzzerFeedbackThread();
int handleInputEvent(const InputEvent *event);
public:
BuzzerFeedbackThread();
int handleInputEvent(const InputEvent *event);
};
extern BuzzerFeedbackThread *buzzerFeedbackThread;
src/buzz/buzz.cpp
+112 -94
View File
@@ -11,8 +11,8 @@ extern "C" void delay(uint32_t dwMs);
#endif
struct ToneDuration {
int frequency_khz;
int duration_ms;
int frequency_khz;
int duration_ms;
};
// Some common frequencies.
@@ -42,92 +42,105 @@ const int DURATION_1_2 = 500; // 1/2 note
const int DURATION_3_4 = 750; // 3/4 note
const int DURATION_1_1 = 1000; // 1/1 note
void playTones(const ToneDuration *tone_durations, int size) {
if (config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DISABLED ||
config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_NOTIFICATIONS_ONLY) {
// Buzzer is disabled or not set to system tones
return;
}
#ifdef PIN_BUZZER
if (!config.device.buzzer_gpio)
config.device.buzzer_gpio = PIN_BUZZER;
#endif
if (config.device.buzzer_gpio) {
for (int i = 0; i < size; i++) {
const auto &tone_duration = tone_durations[i];
tone(config.device.buzzer_gpio, tone_duration.frequency_khz, tone_duration.duration_ms);
// to distinguish the notes, set a minimum time between them.
delay(1.3 * tone_duration.duration_ms);
void playTones(const ToneDuration *tone_durations, int size)
{
if (config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_DISABLED ||
config.device.buzzer_mode == meshtastic_Config_DeviceConfig_BuzzerMode_NOTIFICATIONS_ONLY) {
// Buzzer is disabled or not set to system tones
return;
}
}
}
void playBeep() {
ToneDuration melody[] = {{NOTE_B3, DURATION_1_8}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playLongBeep() {
ToneDuration melody[] = {{NOTE_B3, DURATION_1_1}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playGPSEnableBeep() {
#if defined(R1_NEO) || defined(MUZI_BASE)
ToneDuration melody[] = {{NOTE_F5, DURATION_1_2}, {NOTE_G6, DURATION_1_8}, {NOTE_E7, DURATION_1_4}, {NOTE_SILENT, DURATION_1_2}};
#else
ToneDuration melody[] = {{NOTE_C3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_CS4, DURATION_1_4}};
#ifdef PIN_BUZZER
if (!config.device.buzzer_gpio)
config.device.buzzer_gpio = PIN_BUZZER;
#endif
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
if (config.device.buzzer_gpio) {
for (int i = 0; i < size; i++) {
const auto &tone_duration = tone_durations[i];
tone(config.device.buzzer_gpio, tone_duration.frequency_khz, tone_duration.duration_ms);
// to distinguish the notes, set a minimum time between them.
delay(1.3 * tone_duration.duration_ms);
}
}
}
void playGPSDisableBeep() {
void playBeep()
{
ToneDuration melody[] = {{NOTE_B3, DURATION_1_8}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playLongBeep()
{
ToneDuration melody[] = {{NOTE_B3, DURATION_1_1}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playGPSEnableBeep()
{
#if defined(R1_NEO) || defined(MUZI_BASE)
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_SILENT, DURATION_1_8}, {NOTE_C3, DURATION_1_1}, {NOTE_SILENT, DURATION_1_1}};
ToneDuration melody[] = {
{NOTE_F5, DURATION_1_2}, {NOTE_G6, DURATION_1_8}, {NOTE_E7, DURATION_1_4}, {NOTE_SILENT, DURATION_1_2}};
#else
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_C3, DURATION_1_4}};
ToneDuration melody[] = {{NOTE_C3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_CS4, DURATION_1_4}};
#endif
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playStartMelody() {
ToneDuration melody[] = {{NOTE_FS3, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_CS4, DURATION_1_4}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void playGPSDisableBeep()
{
#if defined(R1_NEO) || defined(MUZI_BASE)
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_SILENT, DURATION_1_8},
{NOTE_C3, DURATION_1_1}, {NOTE_SILENT, DURATION_1_1}};
#else
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_C3, DURATION_1_4}};
#endif
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playShutdownMelody() {
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void playStartMelody()
{
ToneDuration melody[] = {{NOTE_FS3, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_CS4, DURATION_1_4}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playChirp() {
// A short, friendly "chirp" sound for key presses
ToneDuration melody[] = {{NOTE_AS3, 20}}; // Short AS3 note
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void playShutdownMelody()
{
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_AS3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playClick() {
// A very short "click" sound with minimum delay; ideal for rotary encoder events
ToneDuration melody[] = {{NOTE_AS3, 1}}; // Very Short AS3
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void playChirp()
{
// A short, friendly "chirp" sound for key presses
ToneDuration melody[] = {{NOTE_AS3, 20}}; // Short AS3 note
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playBoop() {
// A short, friendly "boop" sound for button presses
ToneDuration melody[] = {{NOTE_A3, 50}}; // Very short A3 note
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void playClick()
{
// A very short "click" sound with minimum delay; ideal for rotary encoder events
ToneDuration melody[] = {{NOTE_AS3, 1}}; // Very Short AS3
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playLongPressLeadUp() {
// An ascending lead-up sequence for long press - builds anticipation
ToneDuration melody[] = {
{NOTE_C3, 100}, // Start low
{NOTE_E3, 100}, // Step up
{NOTE_G3, 100}, // Keep climbing
{NOTE_B3, 150} // Peak with longer note for emphasis
};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void playBoop()
{
// A short, friendly "boop" sound for button presses
ToneDuration melody[] = {{NOTE_A3, 50}}; // Very short A3 note
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playLongPressLeadUp()
{
// An ascending lead-up sequence for long press - builds anticipation
ToneDuration melody[] = {
{NOTE_C3, 100}, // Start low
{NOTE_E3, 100}, // Step up
{NOTE_G3, 100}, // Keep climbing
{NOTE_B3, 150} // Peak with longer note for emphasis
};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
// Static state for progressive lead-up notes
@@ -140,34 +153,39 @@ static const ToneDuration leadUpNotes[] = {
};
static const int leadUpNotesCount = sizeof(leadUpNotes) / sizeof(ToneDuration);
bool playNextLeadUpNote() {
if (leadUpNoteIndex >= leadUpNotesCount) {
return false; // All notes have been played
}
bool playNextLeadUpNote()
{
if (leadUpNoteIndex >= leadUpNotesCount) {
return false; // All notes have been played
}
// Use playTones to handle buzzer logic consistently
const auto &note = leadUpNotes[leadUpNoteIndex];
playTones(&note, 1); // Play single note using existing playTones function
// Use playTones to handle buzzer logic consistently
const auto &note = leadUpNotes[leadUpNoteIndex];
playTones(&note, 1); // Play single note using existing playTones function
leadUpNoteIndex++;
leadUpNoteIndex++;
if (leadUpNoteIndex >= leadUpNotesCount) {
return false; // this was the final note
}
return true; // Note was played (playTones handles buzzer availability internally)
if (leadUpNoteIndex >= leadUpNotesCount) {
return false; // this was the final note
}
return true; // Note was played (playTones handles buzzer availability internally)
}
void resetLeadUpSequence() { leadUpNoteIndex = 0; }
void playComboTune() {
// Quick high-pitched notes with trills
ToneDuration melody[] = {
{NOTE_G3, 80}, // Quick chirp
{NOTE_B3, 60}, // Higher chirp
{NOTE_CS4, 80}, // Even higher
{NOTE_G3, 60}, // Quick trill down
{NOTE_CS4, 60}, // Quick trill up
{NOTE_B3, 120} // Ending chirp
};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
void resetLeadUpSequence()
{
leadUpNoteIndex = 0;
}
void playComboTune()
{
// Quick high-pitched notes with trills
ToneDuration melody[] = {
{NOTE_G3, 80}, // Quick chirp
{NOTE_B3, 60}, // Higher chirp
{NOTE_CS4, 80}, // Even higher
{NOTE_G3, 60}, // Quick trill down
{NOTE_CS4, 60}, // Quick trill up
{NOTE_B3, 120} // Ending chirp
};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
src/commands.h
+11 -11
View File
@@ -4,15 +4,15 @@
*/
enum class Cmd {
INVALID,
SET_ON,
SET_OFF,
ON_PRESS,
START_ALERT_FRAME,
STOP_ALERT_FRAME,
START_FIRMWARE_UPDATE_SCREEN,
STOP_BOOT_SCREEN,
SHOW_PREV_FRAME,
SHOW_NEXT_FRAME,
NOOP
INVALID,
SET_ON,
SET_OFF,
ON_PRESS,
START_ALERT_FRAME,
STOP_ALERT_FRAME,
START_FIRMWARE_UPDATE_SCREEN,
STOP_BOOT_SCREEN,
SHOW_PREV_FRAME,
SHOW_NEXT_FRAME,
NOOP
};
src/concurrency/BinarySemaphoreFreeRTOS.cpp
+21 -7
View File
@@ -4,21 +4,35 @@
#ifdef HAS_FREE_RTOS
namespace concurrency {
namespace concurrency
{
BinarySemaphoreFreeRTOS::BinarySemaphoreFreeRTOS() : semaphore(xSemaphoreCreateBinary()) { assert(semaphore); }
BinarySemaphoreFreeRTOS::BinarySemaphoreFreeRTOS() : semaphore(xSemaphoreCreateBinary())
{
assert(semaphore);
}
BinarySemaphoreFreeRTOS::~BinarySemaphoreFreeRTOS() { vSemaphoreDelete(semaphore); }
BinarySemaphoreFreeRTOS::~BinarySemaphoreFreeRTOS()
{
vSemaphoreDelete(semaphore);
}
/**
* Returns false if we were interrupted
*/
bool BinarySemaphoreFreeRTOS::take(uint32_t msec) { return xSemaphoreTake(semaphore, pdMS_TO_TICKS(msec)); }
bool BinarySemaphoreFreeRTOS::take(uint32_t msec)
{
return xSemaphoreTake(semaphore, pdMS_TO_TICKS(msec));
}
void BinarySemaphoreFreeRTOS::give() { xSemaphoreGive(semaphore); }
void BinarySemaphoreFreeRTOS::give()
{
xSemaphoreGive(semaphore);
}
IRAM_ATTR void BinarySemaphoreFreeRTOS::giveFromISR(BaseType_t *pxHigherPriorityTaskWoken) {
xSemaphoreGiveFromISR(semaphore, pxHigherPriorityTaskWoken);
IRAM_ATTR void BinarySemaphoreFreeRTOS::giveFromISR(BaseType_t *pxHigherPriorityTaskWoken)
{
xSemaphoreGiveFromISR(semaphore, pxHigherPriorityTaskWoken);
}
} // namespace concurrency
src/concurrency/BinarySemaphoreFreeRTOS.h
+14 -12
View File
@@ -2,25 +2,27 @@
#include "../freertosinc.h"
namespace concurrency {
namespace concurrency
{
#ifdef HAS_FREE_RTOS
class BinarySemaphoreFreeRTOS {
SemaphoreHandle_t semaphore;
class BinarySemaphoreFreeRTOS
{
SemaphoreHandle_t semaphore;
public:
BinarySemaphoreFreeRTOS();
~BinarySemaphoreFreeRTOS();
public:
BinarySemaphoreFreeRTOS();
~BinarySemaphoreFreeRTOS();
/**
* Returns false if we timed out
*/
bool take(uint32_t msec);
/**
* Returns false if we timed out
*/
bool take(uint32_t msec);
void give();
void give();
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
};
#endif
src/concurrency/BinarySemaphorePosix.cpp
+6 -4
View File
@@ -3,7 +3,8 @@
#ifndef HAS_FREE_RTOS
namespace concurrency {
namespace concurrency
{
BinarySemaphorePosix::BinarySemaphorePosix() {}
@@ -12,9 +13,10 @@ BinarySemaphorePosix::~BinarySemaphorePosix() {}
/**
* Returns false if we timed out
*/
bool BinarySemaphorePosix::take(uint32_t msec) {
delay(msec); // FIXME
return false;
bool BinarySemaphorePosix::take(uint32_t msec)
{
delay(msec); // FIXME
return false;
}
void BinarySemaphorePosix::give() {}
src/concurrency/BinarySemaphorePosix.h
+14 -12
View File
@@ -2,25 +2,27 @@
#include "../freertosinc.h"
namespace concurrency {
namespace concurrency
{
#ifndef HAS_FREE_RTOS
class BinarySemaphorePosix {
// SemaphoreHandle_t semaphore;
class BinarySemaphorePosix
{
// SemaphoreHandle_t semaphore;
public:
BinarySemaphorePosix();
~BinarySemaphorePosix();
public:
BinarySemaphorePosix();
~BinarySemaphorePosix();
/**
* Returns false if we timed out
*/
bool take(uint32_t msec);
/**
* Returns false if we timed out
*/
bool take(uint32_t msec);
void give();
void give();
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
};
#endif
src/concurrency/InterruptableDelay.cpp
+17 -9
View File
@@ -1,7 +1,8 @@
#include "concurrency/InterruptableDelay.h"
#include "configuration.h"
namespace concurrency {
namespace concurrency
{
InterruptableDelay::InterruptableDelay() {}
@@ -10,18 +11,25 @@ InterruptableDelay::~InterruptableDelay() {}
/**
* Returns false if we were interrupted
*/
bool InterruptableDelay::delay(uint32_t msec) {
// LOG_DEBUG("delay %u ", msec);
bool InterruptableDelay::delay(uint32_t msec)
{
// LOG_DEBUG("delay %u ", msec);
// sem take will return false if we timed out (i.e. were not interrupted)
bool r = semaphore.take(msec);
// sem take will return false if we timed out (i.e. were not interrupted)
bool r = semaphore.take(msec);
// LOG_DEBUG("interrupt=%d", r);
return !r;
// LOG_DEBUG("interrupt=%d", r);
return !r;
}
void InterruptableDelay::interrupt() { semaphore.give(); }
void InterruptableDelay::interrupt()
{
semaphore.give();
}
IRAM_ATTR void InterruptableDelay::interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken) { semaphore.giveFromISR(pxHigherPriorityTaskWoken); }
IRAM_ATTR void InterruptableDelay::interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken)
{
semaphore.giveFromISR(pxHigherPriorityTaskWoken);
}
} // namespace concurrency
src/concurrency/InterruptableDelay.h
+15 -14
View File
@@ -10,31 +10,32 @@
#define BinarySemaphore BinarySemaphorePosix
#endif
namespace concurrency {
namespace concurrency
{
/**
* 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.
*/
class InterruptableDelay {
BinarySemaphore semaphore;
class InterruptableDelay
{
BinarySemaphore semaphore;
public:
InterruptableDelay();
~InterruptableDelay();
public:
InterruptableDelay();
~InterruptableDelay();
/**
* Returns false if we were interrupted
*/
bool delay(uint32_t msec);
/**
* Returns false if we were interrupted
*/
bool delay(uint32_t msec);
void interrupt();
void interrupt();
void interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken);
void interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken);
};
} // namespace concurrency
src/concurrency/Lock.cpp
+18 -14
View File
@@ -2,26 +2,30 @@
#include "configuration.h"
#include <cassert>
namespace concurrency {
namespace concurrency
{
#ifdef HAS_FREE_RTOS
Lock::Lock() : handle(xSemaphoreCreateBinary()) {
assert(handle);
if (xSemaphoreGive(handle) == false) {
abort();
}
Lock::Lock() : handle(xSemaphoreCreateBinary())
{
assert(handle);
if (xSemaphoreGive(handle) == false) {
abort();
}
}
void Lock::lock() {
if (xSemaphoreTake(handle, portMAX_DELAY) == false) {
abort();
}
void Lock::lock()
{
if (xSemaphoreTake(handle, portMAX_DELAY) == false) {
abort();
}
}
void Lock::unlock() {
if (xSemaphoreGive(handle) == false) {
abort();
}
void Lock::unlock()
{
if (xSemaphoreGive(handle) == false) {
abort();
}
}
#else
Lock::Lock() {}
src/concurrency/Lock.h
+18 -16
View File
@@ -2,31 +2,33 @@
#include "../freertosinc.h"
namespace concurrency {
namespace concurrency
{
/**
* @brief Simple wrapper around FreeRTOS API for implementing a mutex lock
*/
class Lock {
public:
Lock();
class Lock
{
public:
Lock();
Lock(const Lock &) = delete;
Lock &operator=(const Lock &) = delete;
Lock(const Lock &) = delete;
Lock &operator=(const Lock &) = delete;
/// Locks the lock.
//
// Must not be called from an ISR.
void lock();
/// Locks the lock.
//
// Must not be called from an ISR.
void lock();
// Unlocks the lock.
//
// Must not be called from an ISR.
void unlock();
// Unlocks the lock.
//
// Must not be called from an ISR.
void unlock();
private:
private:
#ifdef HAS_FREE_RTOS
SemaphoreHandle_t handle;
SemaphoreHandle_t handle;
#endif
};
src/concurrency/LockGuard.cpp
+10 -3
View File
@@ -1,10 +1,17 @@
#include "LockGuard.h"
#include "configuration.h"
namespace concurrency {
namespace concurrency
{
LockGuard::LockGuard(Lock *lock) : lock(lock) { lock->lock(); }
LockGuard::LockGuard(Lock *lock) : lock(lock)
{
lock->lock();
}
LockGuard::~LockGuard() { lock->unlock(); }
LockGuard::~LockGuard()
{
lock->unlock();
}
} // namespace concurrency
src/concurrency/LockGuard.h
+11 -9
View File
@@ -2,21 +2,23 @@
#include "Lock.h"
namespace concurrency {
namespace concurrency
{
/**
* @brief RAII lock guard
*/
class LockGuard {
public:
explicit LockGuard(Lock *lock);
~LockGuard();
class LockGuard
{
public:
explicit LockGuard(Lock *lock);
~LockGuard();
LockGuard(const LockGuard &) = delete;
LockGuard &operator=(const LockGuard &) = delete;
LockGuard(const LockGuard &) = delete;
LockGuard &operator=(const LockGuard &) = delete;
private:
Lock *lock;
private:
Lock *lock;
};
} // namespace concurrency
src/concurrency/NotifiedWorkerThread.cpp
+51 -44
View File
@@ -2,42 +2,45 @@
#include "configuration.h"
#include "main.h"
namespace concurrency {
namespace concurrency
{
static bool debugNotification;
/**
* Notify this thread so it can run
*/
bool NotifiedWorkerThread::notify(uint32_t v, bool overwrite) {
bool r = notifyCommon(v, overwrite);
bool NotifiedWorkerThread::notify(uint32_t v, bool overwrite)
{
bool r = notifyCommon(v, overwrite);
if (r)
mainDelay.interrupt();
if (r)
mainDelay.interrupt();
return r;
return r;
}
/**
* Notify this thread so it can run
*/
IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite) {
if (overwrite || notification == 0) {
enabled = true;
setInterval(0); // Run ASAP
runASAP = true;
IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite)
{
if (overwrite || notification == 0) {
enabled = true;
setInterval(0); // Run ASAP
runASAP = true;
notification = v;
if (debugNotification) {
LOG_DEBUG("Set notification %d", v);
notification = v;
if (debugNotification) {
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;
}
}
/**
@@ -45,43 +48,47 @@ IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite) {
*
* This must be inline or IRAM_ATTR on ESP32
*/
IRAM_ATTR bool NotifiedWorkerThread::notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite) {
bool r = notifyCommon(v, overwrite);
if (r)
mainDelay.interruptFromISR(highPriWoken);
IRAM_ATTR bool NotifiedWorkerThread::notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite)
{
bool r = notifyCommon(v, overwrite);
if (r)
mainDelay.interruptFromISR(highPriWoken);
return r;
return r;
}
/**
* Schedule a notification to fire in delay msecs
*/
bool NotifiedWorkerThread::notifyLater(uint32_t delay, uint32_t v, bool overwrite) {
bool didIt = notify(v, overwrite);
bool NotifiedWorkerThread::notifyLater(uint32_t delay, uint32_t v, bool overwrite)
{
bool didIt = notify(v, overwrite);
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
if (debugNotification) {
LOG_DEBUG("Delay notification %u", delay);
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
if (debugNotification) {
LOG_DEBUG("Delay notification %u", delay);
}
}
}
return didIt;
return didIt;
}
void NotifiedWorkerThread::checkNotification() {
auto n = notification;
notification = 0; // clear notification
if (n) {
onNotify(n);
}
void NotifiedWorkerThread::checkNotification()
{
auto n = notification;
notification = 0; // clear notification
if (n) {
onNotify(n);
}
}
int32_t NotifiedWorkerThread::runOnce() {
enabled = false; // Only run once per notification
checkNotification();
int32_t NotifiedWorkerThread::runOnce()
{
enabled = false; // Only run once per notification
checkNotification();
return RUN_SAME;
return RUN_SAME;
}
} // namespace concurrency
src/concurrency/NotifiedWorkerThread.h
+37 -35
View File
@@ -2,53 +2,55 @@
#include "OSThread.h"
namespace concurrency {
namespace concurrency
{
/**
* @brief A worker thread that waits on a freertos notification
*/
class NotifiedWorkerThread : public OSThread {
/**
* The notification that was most recently used to wake the thread. Read from runOnce()
*/
uint32_t notification = 0;
class NotifiedWorkerThread : public OSThread
{
/**
* The notification that was most recently used to wake the thread. Read from runOnce()
*/
uint32_t notification = 0;
public:
NotifiedWorkerThread(const char *name) : OSThread(name) {}
public:
NotifiedWorkerThread(const char *name) : OSThread(name) {}
/**
* Notify this thread so it can run
*/
bool notify(uint32_t v, bool overwrite);
/**
* Notify this thread so it can run
*/
bool notify(uint32_t v, bool overwrite);
/**
* Notify from an ISR
*
* This must be inline or IRAM_ATTR on ESP32
*/
bool notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite);
/**
* Notify from an ISR
*
* This must be inline or IRAM_ATTR on ESP32
*/
bool notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite);
/**
* Schedule a notification to fire in delay msecs
*/
bool notifyLater(uint32_t delay, uint32_t v, bool overwrite);
/**
* Schedule a notification to fire in delay msecs
*/
bool notifyLater(uint32_t delay, uint32_t v, bool overwrite);
protected:
virtual void onNotify(uint32_t notification) = 0;
protected:
virtual void onNotify(uint32_t notification) = 0;
/// just calls checkNotification()
virtual int32_t runOnce() override;
/// just calls checkNotification()
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 to change radio transmit/receive modes we want to handle any pending interrupts first). You can call
/// this method and if any notifications are currently pending they will be handled immediately.
void checkNotification();
/// Sometimes we might want to check notifications independently of when our thread was getting woken up (i.e. if we are about
/// to change radio transmit/receive modes we want to handle any pending interrupts first). You can call this method and if
/// any notifications are currently pending they will be handled immediately.
void checkNotification();
private:
/**
* Notify this thread so it can run
*/
bool notifyCommon(uint32_t v, bool overwrite);
private:
/**
* Notify this thread so it can run
*/
bool notifyCommon(uint32_t v, bool overwrite);
};
} // namespace concurrency
src/concurrency/OSThread.cpp
+74 -64
View File
@@ -3,7 +3,8 @@
#include "memGet.h"
#include <assert.h>
namespace concurrency {
namespace concurrency
{
/// Show debugging info for disabled threads
bool OSThread::showDisabled;
@@ -19,85 +20,93 @@ const OSThread *OSThread::currentThread;
ThreadController mainController, timerController;
InterruptableDelay mainDelay;
void OSThread::setup() {
mainController.ThreadName = "mainController";
timerController.ThreadName = "timerController";
void OSThread::setup()
{
mainController.ThreadName = "mainController";
timerController.ThreadName = "timerController";
}
OSThread::OSThread(const char *_name, uint32_t period, ThreadController *_controller) : Thread(NULL, period), controller(_controller) {
assertIsSetup();
OSThread::OSThread(const char *_name, uint32_t period, ThreadController *_controller)
: Thread(NULL, period), controller(_controller)
{
assertIsSetup();
ThreadName = _name;
ThreadName = _name;
if (controller) {
bool added = controller->add(this);
assert(added);
}
if (controller) {
bool added = controller->add(this);
assert(added);
}
}
OSThread::~OSThread() {
if (controller)
controller->remove(this);
OSThread::~OSThread()
{
if (controller)
controller->remove(this);
}
/**
* Wait a specified number msecs starting from the current time (rather than the last time we were run)
*/
void OSThread::setIntervalFromNow(unsigned long _interval) {
// Save interval
interval = _interval;
void OSThread::setIntervalFromNow(unsigned long _interval)
{
// Save interval
interval = _interval;
// Cache the next run based on the last_run
_cached_next_run = millis() + interval;
// Cache the next run based on the last_run
_cached_next_run = millis() + interval;
}
bool OSThread::shouldRun(unsigned long time) {
bool r = Thread::shouldRun(time);
bool OSThread::shouldRun(unsigned long time)
{
bool r = Thread::shouldRun(time);
if (showRun && r) {
LOG_DEBUG("Thread %s: run", ThreadName.c_str());
}
if (showRun && r) {
LOG_DEBUG("Thread %s: run", ThreadName.c_str());
}
if (showWaiting && enabled && !r) {
LOG_DEBUG("Thread %s: wait %lu", ThreadName.c_str(), interval);
}
if (showWaiting && enabled && !r) {
LOG_DEBUG("Thread %s: wait %lu", ThreadName.c_str(), interval);
}
if (showDisabled && !enabled) {
LOG_DEBUG("Thread %s: disabled", ThreadName.c_str());
}
if (showDisabled && !enabled) {
LOG_DEBUG("Thread %s: disabled", ThreadName.c_str());
}
return r;
return r;
}
void OSThread::run() {
void OSThread::run()
{
#ifdef DEBUG_HEAP
auto heap = memGet.getFreeHeap();
auto heap = memGet.getFreeHeap();
#endif
currentThread = this;
auto newDelay = runOnce();
currentThread = this;
auto newDelay = runOnce();
#ifdef DEBUG_HEAP
auto newHeap = memGet.getFreeHeap();
if (newHeap < heap)
LOG_HEAP("------ Thread %s leaked heap %d -> %d (%d) ------", ThreadName.c_str(), heap, newHeap, newHeap - heap);
if (heap < newHeap)
LOG_HEAP("++++++ Thread %s freed heap %d -> %d (%d) ++++++", ThreadName.c_str(), heap, newHeap, newHeap - heap);
auto newHeap = memGet.getFreeHeap();
if (newHeap < heap)
LOG_HEAP("------ Thread %s leaked heap %d -> %d (%d) ------", ThreadName.c_str(), heap, newHeap, newHeap - heap);
if (heap < newHeap)
LOG_HEAP("++++++ Thread %s freed heap %d -> %d (%d) ++++++", ThreadName.c_str(), heap, newHeap, newHeap - heap);
#endif
#ifdef DEBUG_LOOP_TIMING
LOG_DEBUG("====== Thread next run in: %d", newDelay);
LOG_DEBUG("====== Thread next run in: %d", newDelay);
#endif
runned();
runned();
if (newDelay >= 0)
setInterval(newDelay);
if (newDelay >= 0)
setInterval(newDelay);
currentThread = NULL;
currentThread = NULL;
}
int32_t OSThread::disable() {
enabled = false;
setInterval(INT32_MAX);
int32_t OSThread::disable()
{
enabled = false;
setInterval(INT32_MAX);
return INT32_MAX;
return INT32_MAX;
}
/**
@@ -113,22 +122,23 @@ int32_t OSThread::disable() {
*/
bool hasBeenSetup;
void assertIsSetup() {
void assertIsSetup()
{
/**
* 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. 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
* new them at a point where you are guaranteed that other objects that this instance
* depends on have already been created.
*
* 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.
*/
assert(hasBeenSetup);
/**
* 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.
* 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
* new them at a point where you are guaranteed that other objects that this instance
* depends on have already been created.
*
* 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.
*/
assert(hasBeenSetup);
}
} // namespace concurrency
src/concurrency/OSThread.h
+34 -33
View File
@@ -7,7 +7,8 @@
#include "ThreadController.h"
#include "concurrency/InterruptableDelay.h"
namespace concurrency {
namespace concurrency
{
extern ThreadController mainController, timerController;
extern InterruptableDelay mainDelay;
@@ -17,8 +18,7 @@ extern InterruptableDelay mainDelay;
/**
* @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
*
@@ -28,48 +28,49 @@ extern InterruptableDelay mainDelay;
* move typedQueue into concurrency
* remove freertos from typedqueue
*/
class OSThread : public Thread {
ThreadController *controller;
class OSThread : public Thread
{
ThreadController *controller;
/// Show debugging info for disabled threads
static bool showDisabled;
/// Show debugging info for disabled threads
static bool showDisabled;
/// Show debugging info for threads when we run them
static bool showRun;
/// Show debugging info for threads when we run them
static bool showRun;
/// Show debugging info for threads we decide not to run;
static bool showWaiting;
/// Show debugging info for threads we decide not to run;
static bool showWaiting;
public:
/// For debug printing only (might be null)
static const OSThread *currentThread;
public:
/// For debug printing only (might be null)
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)
*/
void setIntervalFromNow(unsigned long _interval);
/**
* Wait a specified number msecs starting from the current time (rather than the last time we were run)
*/
void setIntervalFromNow(unsigned long _interval);
protected:
/**
* 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)
*/
virtual int32_t runOnce() = 0;
bool sleepOnNextExecution = false;
protected:
/**
* 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)
*/
virtual int32_t runOnce() = 0;
bool sleepOnNextExecution = false;
// Do not override this
virtual void run();
// Do not override this
virtual void run();
};
/**
src/concurrency/Periodic.h
+10 -8
View File
@@ -2,21 +2,23 @@
#include "concurrency/OSThread.h"
namespace concurrency {
namespace concurrency
{
/**
* @brief Periodically invoke a callback. This just provides C-style callback conventions
* rather than a virtual function - FIXME, remove?
*/
class Periodic : public OSThread {
int32_t (*callback)();
class Periodic : public OSThread
{
int32_t (*callback)();
public:
// 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) {}
public:
// 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) {}
protected:
int32_t runOnce() override { return callback(); }
protected:
int32_t runOnce() override { return callback(); }
};
} // 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
#endif
// FIXME: This is still needed by the Bluetooth Stack and needs to be replaced by something better. Remnant of the old
// versioning system.
// FIXME: This is still needed by the Bluetooth Stack and needs to be replaced by something better. Remnant of the old versioning
// system.
#ifndef HW_VERSION
#define HW_VERSION "1.0"
#endif
src/detect/LoRaRadioType.h
+11 -11
View File
@@ -1,17 +1,17 @@
#pragma once
enum LoRaRadioType {
NO_RADIO,
STM32WLx_RADIO,
SIM_RADIO,
RF95_RADIO,
SX1262_RADIO,
SX1268_RADIO,
LLCC68_RADIO,
SX1280_RADIO,
LR1110_RADIO,
LR1120_RADIO,
LR1121_RADIO
NO_RADIO,
STM32WLx_RADIO,
SIM_RADIO,
RF95_RADIO,
SX1262_RADIO,
SX1268_RADIO,
LLCC68_RADIO,
SX1280_RADIO,
LR1110_RADIO,
LR1120_RADIO,
LR1121_RADIO
};
extern LoRaRadioType radioType;
src/detect/ScanI2C.cpp
+62 -40
View File
@@ -8,60 +8,82 @@ ScanI2C::ScanI2C() = default;
void ScanI2C::scanPort(ScanI2C::I2CPort port) {}
void ScanI2C::scanPort(ScanI2C::I2CPort port, uint8_t *address, uint8_t asize) {}
void ScanI2C::setSuppressScreen() { shouldSuppressScreen = true; }
void ScanI2C::setSuppressScreen()
{
shouldSuppressScreen = true;
}
ScanI2C::FoundDevice ScanI2C::firstScreen() const {
// Allow to override the scanner results for screen
if (shouldSuppressScreen)
ScanI2C::FoundDevice ScanI2C::firstScreen() const
{
// Allow to override the scanner results for screen
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;
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);
bool ScanI2C::exists(ScanI2C::DeviceType) const
{
return false;
}
ScanI2C::FoundDevice ScanI2C::firstKeyboard() const {
ScanI2C::DeviceType types[] = {CARDKB, TDECKKB, BBQ10KB, RAK14004, MPR121KB, TCA8418KB};
return firstOfOrNONE(6, types);
ScanI2C::FoundDevice ScanI2C::firstOfOrNONE(size_t count, ScanI2C::DeviceType *types) const
{
return DEVICE_NONE;
}
ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const {
ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160, STK8BAXX, ICM20948, QMA6100P, BMM150};
return firstOfOrNONE(9, types);
size_t ScanI2C::countDevices() const
{
return 0;
}
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() : DeviceAddress(I2CPort::NO_I2C, 0) {}
bool ScanI2C::DeviceAddress::operator<(const ScanI2C::DeviceAddress &other) const {
return
// If this one has no port and other has a port
(port == NO_I2C && other.port != NO_I2C)
// if both have a port and this one's address is lower
|| (port != NO_I2C && other.port != NO_I2C && (address < other.address));
bool ScanI2C::DeviceAddress::operator<(const ScanI2C::DeviceAddress &other) const
{
return
// If this one has no port and other has a port
(port == NO_I2C && other.port != NO_I2C)
// if both have a port and this one's address is lower
|| (port != NO_I2C && other.port != NO_I2C && (address < other.address));
}
ScanI2C::FoundDevice::FoundDevice(ScanI2C::DeviceType type, ScanI2C::DeviceAddress address) : type(type), address(address) {}
src/detect/ScanI2C.h
+129 -128
View File
@@ -3,155 +3,156 @@
#include <stddef.h>
#include <stdint.h>
class ScanI2C {
public:
typedef enum DeviceType {
NONE,
SCREEN_SSD1306,
SCREEN_SH1106,
SCREEN_UNKNOWN, // has the same address as the two above but does not respond to the same commands
SCREEN_ST7567,
RTC_RV3028,
RTC_PCF8563,
RTC_PCF85063,
RTC_RX8130CE,
CARDKB,
TDECKKB,
BBQ10KB,
RAK14004,
PMU_AXP192_AXP2101, // has the same adress as the TCA8418KB
BME_680,
BME_280,
BMP_280,
BMP_085,
BMP_3XX,
INA260,
INA219,
INA3221,
MAX17048,
MCP9808,
SHT31,
SHT4X,
SHTC3,
LPS22HB,
QMC6310,
QMI8658,
QMC5883L,
HMC5883L,
PMSA0031,
QMA6100P,
MPU6050,
LIS3DH,
BMA423,
BQ24295,
LSM6DS3,
TCA9535,
TCA9555,
VEML7700,
RCWL9620,
NCP5623,
LP5562,
TSL2591,
OPT3001,
MLX90632,
MLX90614,
AHT10,
BMX160,
DFROBOT_LARK,
NAU7802,
FT6336U,
STK8BAXX,
ICM20948,
SCD4X,
MAX30102,
TPS65233,
MPR121KB,
CGRADSENS,
INA226,
NXP_SE050,
DFROBOT_RAIN,
DPS310,
LTR390UV,
RAK12035,
TCA8418KB,
PCT2075,
CST328,
BQ25896,
BQ27220,
LTR553ALS,
BHI260AP,
BMM150,
TSL2561,
DRV2605,
BH1750,
DA217,
CHSC6X,
CST226SE
} DeviceType;
class ScanI2C
{
public:
typedef enum DeviceType {
NONE,
SCREEN_SSD1306,
SCREEN_SH1106,
SCREEN_UNKNOWN, // has the same address as the two above but does not respond to the same commands
SCREEN_ST7567,
RTC_RV3028,
RTC_PCF8563,
RTC_PCF85063,
RTC_RX8130CE,
CARDKB,
TDECKKB,
BBQ10KB,
RAK14004,
PMU_AXP192_AXP2101, // has the same adress as the TCA8418KB
BME_680,
BME_280,
BMP_280,
BMP_085,
BMP_3XX,
INA260,
INA219,
INA3221,
MAX17048,
MCP9808,
SHT31,
SHT4X,
SHTC3,
LPS22HB,
QMC6310,
QMI8658,
QMC5883L,
HMC5883L,
PMSA0031,
QMA6100P,
MPU6050,
LIS3DH,
BMA423,
BQ24295,
LSM6DS3,
TCA9535,
TCA9555,
VEML7700,
RCWL9620,
NCP5623,
LP5562,
TSL2591,
OPT3001,
MLX90632,
MLX90614,
AHT10,
BMX160,
DFROBOT_LARK,
NAU7802,
FT6336U,
STK8BAXX,
ICM20948,
SCD4X,
MAX30102,
TPS65233,
MPR121KB,
CGRADSENS,
INA226,
NXP_SE050,
DFROBOT_RAIN,
DPS310,
LTR390UV,
RAK12035,
TCA8418KB,
PCT2075,
CST328,
BQ25896,
BQ27220,
LTR553ALS,
BHI260AP,
BMM150,
TSL2561,
DRV2605,
BH1750,
DA217,
CHSC6X,
CST226SE
} DeviceType;
// typedef uint8_t DeviceAddress;
typedef enum I2CPort {
NO_I2C,
WIRE,
WIRE1,
} I2CPort;
// typedef uint8_t DeviceAddress;
typedef enum I2CPort {
NO_I2C,
WIRE,
WIRE1,
} I2CPort;
typedef struct DeviceAddress {
// set default values for ADDRESS_NONE
I2CPort port = I2CPort::NO_I2C;
uint8_t address = 0;
typedef struct DeviceAddress {
// set default values for ADDRESS_NONE
I2CPort port = I2CPort::NO_I2C;
uint8_t address = 0;
explicit DeviceAddress(I2CPort port, uint8_t address);
DeviceAddress();
explicit DeviceAddress(I2CPort port, uint8_t address);
DeviceAddress();
bool operator<(const DeviceAddress &other) const;
} DeviceAddress;
bool operator<(const DeviceAddress &other) const;
} DeviceAddress;
static const DeviceAddress ADDRESS_NONE;
static const DeviceAddress ADDRESS_NONE;
typedef uint8_t RegisterAddress;
typedef uint8_t RegisterAddress;
typedef struct FoundDevice {
DeviceType type;
DeviceAddress address;
typedef struct FoundDevice {
DeviceType type;
DeviceAddress address;
explicit FoundDevice(DeviceType = DeviceType::NONE, DeviceAddress = ADDRESS_NONE);
} FoundDevice;
explicit FoundDevice(DeviceType = DeviceType::NONE, DeviceAddress = ADDRESS_NONE);
} FoundDevice;
static const FoundDevice DEVICE_NONE;
static const FoundDevice DEVICE_NONE;
public:
ScanI2C();
public:
ScanI2C();
virtual void scanPort(ScanI2C::I2CPort);
virtual void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t);
virtual void scanPort(ScanI2C::I2CPort);
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.
*/
void setSuppressScreen();
/*
* A bit of a hack, this tells the scanner not to tell later systems there is a screen to avoid enabling it.
*/
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:
virtual FoundDevice firstOfOrNONE(size_t, DeviceType[]) const;
protected:
virtual FoundDevice firstOfOrNONE(size_t, DeviceType[]) const;
private:
bool shouldSuppressScreen = false;
private:
bool shouldSuppressScreen = false;
};
src/detect/ScanI2CConsumer.cpp
+9 -5
View File
@@ -3,10 +3,14 @@
static std::forward_list<ScanI2CConsumer *> ScanI2CConsumers;
ScanI2CConsumer::ScanI2CConsumer() { ScanI2CConsumers.push_front(this); }
ScanI2CConsumer::ScanI2CConsumer()
{
ScanI2CConsumers.push_front(this);
}
void ScanI2CCompleted(ScanI2C *i2cScanner) {
for (ScanI2CConsumer *consumer : ScanI2CConsumers) {
consumer->i2cScanFinished(i2cScanner);
}
void ScanI2CCompleted(ScanI2C *i2cScanner)
{
for (ScanI2CConsumer *consumer : ScanI2CConsumers) {
consumer->i2cScanFinished(i2cScanner);
}
}
src/detect/ScanI2CConsumer.h
+5 -4
View File
@@ -3,10 +3,11 @@
#include "ScanI2C.h"
#include <stddef.h>
class ScanI2CConsumer {
public:
ScanI2CConsumer();
virtual void i2cScanFinished(ScanI2C *i2cScanner) = 0;
class ScanI2CConsumer
{
public:
ScanI2CConsumer();
virtual void i2cScanFinished(ScanI2C *i2cScanner) = 0;
};
void ScanI2CCompleted(ScanI2C *i2cScanner);
src/detect/ScanI2CTwoWire.cpp
+563 -544
View File
File diff suppressed because it is too large Load Diff
src/detect/ScanI2CTwoWire.h
+28 -24
View File
@@ -14,45 +14,49 @@
#include "../concurrency/Lock.h"
class ScanI2CTwoWire : public ScanI2C {
public:
void scanPort(ScanI2C::I2CPort) override;
class ScanI2CTwoWire : public ScanI2C
{
public:
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:
FoundDevice firstOfOrNONE(size_t, DeviceType[]) const override;
protected:
FoundDevice firstOfOrNONE(size_t, DeviceType[]) const override;
private:
typedef struct RegisterLocation {
DeviceAddress i2cAddress;
RegisterAddress registerAddress;
private:
typedef struct RegisterLocation {
DeviceAddress i2cAddress;
RegisterAddress registerAddress;
RegisterLocation(DeviceAddress deviceAddress, RegisterAddress registerAddress) : i2cAddress(deviceAddress), registerAddress(registerAddress) {}
RegisterLocation(DeviceAddress 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?)
std::map<ScanI2C::DeviceType, ScanI2C::DeviceAddress> deviceAddresses;
// note: prone to overwriting if multiple devices of a type are added at different addresses (rare?)
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
src/detect/einkScan.h
+53 -49
View File
@@ -4,60 +4,64 @@
#include "../main.h"
#include <SPI.h>
void d_writeCommand(uint8_t c) {
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0));
if (PIN_EINK_DC >= 0)
digitalWrite(PIN_EINK_DC, LOW);
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, LOW);
SPI1.transfer(c);
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, HIGH);
if (PIN_EINK_DC >= 0)
digitalWrite(PIN_EINK_DC, HIGH);
SPI1.endTransaction();
void d_writeCommand(uint8_t c)
{
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0));
if (PIN_EINK_DC >= 0)
digitalWrite(PIN_EINK_DC, LOW);
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, LOW);
SPI1.transfer(c);
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, HIGH);
if (PIN_EINK_DC >= 0)
digitalWrite(PIN_EINK_DC, HIGH);
SPI1.endTransaction();
}
void d_writeData(uint8_t d) {
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0));
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, LOW);
SPI1.transfer(d);
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, HIGH);
SPI1.endTransaction();
void d_writeData(uint8_t d)
{
SPI1.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0));
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, LOW);
SPI1.transfer(d);
if (PIN_EINK_CS >= 0)
digitalWrite(PIN_EINK_CS, HIGH);
SPI1.endTransaction();
}
unsigned long d_waitWhileBusy(uint16_t busy_time) {
if (PIN_EINK_BUSY >= 0) {
delay(1); // add some margin to become active
unsigned long start = micros();
while (1) {
if (digitalRead(PIN_EINK_BUSY) != HIGH)
break;
delay(1);
if (digitalRead(PIN_EINK_BUSY) != HIGH)
break;
if (micros() - start > 10000000)
break;
}
unsigned long elapsed = micros() - start;
(void)start;
return elapsed;
} else
return busy_time;
unsigned long d_waitWhileBusy(uint16_t busy_time)
{
if (PIN_EINK_BUSY >= 0) {
delay(1); // add some margin to become active
unsigned long start = micros();
while (1) {
if (digitalRead(PIN_EINK_BUSY) != HIGH)
break;
delay(1);
if (digitalRead(PIN_EINK_BUSY) != HIGH)
break;
if (micros() - start > 10000000)
break;
}
unsigned long elapsed = micros() - start;
(void)start;
return elapsed;
} else
return busy_time;
}
void scanEInkDevice(void) {
SPI1.begin();
d_writeCommand(0x22);
d_writeData(0x83);
d_writeCommand(0x20);
eink_found = (d_waitWhileBusy(150) > 0) ? true : false;
if (eink_found)
LOG_DEBUG("EInk display found");
else
LOG_DEBUG("EInk display not found");
SPI1.end();
void scanEInkDevice(void)
{
SPI1.begin();
d_writeCommand(0x22);
d_writeData(0x83);
d_writeCommand(0x20);
eink_found = (d_waitWhileBusy(150) > 0) ? true : false;
if (eink_found)
LOG_DEBUG("EInk display found");
else
LOG_DEBUG("EInk display not found");
SPI1.end();
}
#endif
src/freertosinc.h
+2 -2
View File
@@ -1,7 +1,7 @@
#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 options so this is my quick hack to make things work
// The FreeRTOS includes are in a different directory on ESP32 and I can't figure out how to make that work with platformio gcc
// options so this is my quick hack to make things work
#if defined(ARDUINO_ARCH_ESP32)
#define HAS_FREE_RTOS
src/gps/GPS.cpp
+1475 -1428
View File
File diff suppressed because it is too large Load Diff
src/gps/GPS.h
+159 -158
View File
@@ -20,234 +20,235 @@ static constexpr uint32_t GPS_UPDATE_ALWAYS_ON_THRESHOLD_MS = 10 * 1000UL;
static constexpr uint32_t GPS_FIX_HOLD_MAX_MS = 20000;
typedef enum {
GNSS_MODEL_ATGM336H,
GNSS_MODEL_MTK,
GNSS_MODEL_UBLOX6,
GNSS_MODEL_UBLOX7,
GNSS_MODEL_UBLOX8,
GNSS_MODEL_UBLOX9,
GNSS_MODEL_UBLOX10,
GNSS_MODEL_UC6580,
GNSS_MODEL_UNKNOWN,
GNSS_MODEL_MTK_L76B,
GNSS_MODEL_MTK_PA1010D,
GNSS_MODEL_MTK_PA1616S,
GNSS_MODEL_AG3335,
GNSS_MODEL_AG3352,
GNSS_MODEL_LS20031,
GNSS_MODEL_CM121
GNSS_MODEL_ATGM336H,
GNSS_MODEL_MTK,
GNSS_MODEL_UBLOX6,
GNSS_MODEL_UBLOX7,
GNSS_MODEL_UBLOX8,
GNSS_MODEL_UBLOX9,
GNSS_MODEL_UBLOX10,
GNSS_MODEL_UC6580,
GNSS_MODEL_UNKNOWN,
GNSS_MODEL_MTK_L76B,
GNSS_MODEL_MTK_PA1010D,
GNSS_MODEL_MTK_PA1616S,
GNSS_MODEL_AG3335,
GNSS_MODEL_AG3352,
GNSS_MODEL_LS20031,
GNSS_MODEL_CM121
} GnssModel_t;
typedef enum {
GNSS_RESPONSE_NONE,
GNSS_RESPONSE_NAK,
GNSS_RESPONSE_FRAME_ERRORS,
GNSS_RESPONSE_OK,
GNSS_RESPONSE_NONE,
GNSS_RESPONSE_NAK,
GNSS_RESPONSE_FRAME_ERRORS,
GNSS_RESPONSE_OK,
} GPS_RESPONSE;
enum GPSPowerState : uint8_t {
GPS_ACTIVE, // Awake and want a position
GPS_IDLE, // Awake, but not wanting another position yet
GPS_SOFTSLEEP, // Physically powered on, but soft-sleeping
GPS_HARDSLEEP, // Physically powered off, but scheduled to wake
GPS_OFF // Powered off indefinitely
GPS_ACTIVE, // Awake and want a position
GPS_IDLE, // Awake, but not wanting another position yet
GPS_SOFTSLEEP, // Physically powered on, but soft-sleeping
GPS_HARDSLEEP, // Physically powered off, but scheduled to wake
GPS_OFF // Powered off indefinitely
};
struct ChipInfo {
String chipName; // The name of the chip (for logging)
String detectionString; // The string to match in the response
GnssModel_t driver; // The driver to use
String chipName; // The name of the chip (for logging)
String detectionString; // The string to match in the response
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
*
* When new data is available it will notify observers.
*/
class GPS : private concurrency::OSThread {
public:
meshtastic_Position p = meshtastic_Position_init_default;
class GPS : private concurrency::OSThread
{
public:
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 implementations. Those boards will set this public variable to a custom implementation.
*
* Normally set by GPS::createGPS()
*/
GpioVirtPin *enablePin = NULL;
/** This is normally bound to config.position.gps_en_gpio but some rare boards (like heltec tracker) need more advanced
* implementations. Those boards will set this public variable to a custom implementation.
*
* Normally set by GPS::createGPS()
*/
GpioVirtPin *enablePin = NULL;
virtual ~GPS();
virtual ~GPS();
/** We will notify this observable anytime GPS state has changed meaningfully */
Observable<const meshtastic::GPSStatus *> newStatus;
/** We will notify this observable anytime GPS state has changed meaningfully */
Observable<const meshtastic::GPSStatus *> newStatus;
/**
* Returns true if we succeeded
*/
virtual bool setup();
/**
* Returns true if we succeeded
*/
virtual bool setup();
// re-enable the thread
void enable();
// re-enable the thread
void enable();
// Disable the thread
int32_t disable() override;
// Disable the thread
int32_t disable() override;
// toggle between enabled/disabled
void toggleGpsMode();
// toggle between enabled/disabled
void toggleGpsMode();
// Change the power state of the GPS - for power saving / shutdown
void setPowerState(GPSPowerState newState, uint32_t sleepMs = 0);
// Change the power state of the GPS - for power saving / shutdown
void setPowerState(GPSPowerState newState, uint32_t sleepMs = 0);
/// Returns true if we have acquired GPS lock.
virtual bool hasLock();
/// Returns true if we have acquired GPS lock.
virtual bool hasLock();
/// Returns true if there's valid data flow with the chip.
virtual bool hasFlow();
/// Returns true if there's valid data flow with the chip.
virtual bool hasFlow();
/// Return true if we are connected to a GPS
bool isConnected() const { return hasGPS; }
/// Return true if we are connected to a GPS
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
void clearBuffer();
// Empty the input buffer as quickly as possible
void clearBuffer();
// Creates an instance of the GPS class.
// Returns the new instance or null if the GPS is not present.
static GPS *createGps();
// Creates an instance of the GPS class.
// Returns the new instance or null if the GPS is not present.
static GPS *createGps();
// Wake the GPS hardware - ready for an update
void up();
// Wake the GPS hardware - ready for an update
void up();
// Let the GPS hardware save power between updates
void down();
// Let the GPS hardware save power between updates
void down();
private:
GPS() : concurrency::OSThread("GPS") {}
private:
GPS() : concurrency::OSThread("GPS") {}
/// Record that we have a GPS
void setConnected();
/// Record that we have a GPS
void setConnected();
/** 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
*/
virtual bool whileActive();
/** 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
*/
virtual bool whileActive();
/**
* 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
*
* @return true if we've acquired a time
*/
virtual bool lookForTime();
/**
* 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
*
* @return true if we've acquired a time
*/
virtual bool lookForTime();
/**
* 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
*
* @return true if we've acquired a new location
*/
virtual bool lookForLocation();
/**
* 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
*
* @return true if we've acquired a new location
*/
virtual bool lookForLocation();
GnssModel_t gnssModel = GNSS_MODEL_UNKNOWN;
GnssModel_t gnssModel = GNSS_MODEL_UNKNOWN;
TinyGPSPlus reader;
uint8_t fixQual = 0; // fix quality from GPGGA
uint32_t lastChecksumFailCount = 0;
uint8_t currentStep = 0;
int32_t currentDelay = 2000;
TinyGPSPlus reader;
uint8_t fixQual = 0; // fix quality from GPGGA
uint32_t lastChecksumFailCount = 0;
uint8_t currentStep = 0;
int32_t currentDelay = 2000;
#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
// (20210908) TinyGps++ can only read the GPGSA "FIX TYPE" field
// via optional feature "custom fields", currently disabled (bug #525)
TinyGPSCustom gsafixtype; // custom extract fix type from GPGSA
TinyGPSCustom gsapdop; // custom extract PDOP from GPGSA
uint8_t fixType = 0; // fix type from GPGSA
// (20210908) TinyGps++ can only read the GPGSA "FIX TYPE" field
// via optional feature "custom fields", currently disabled (bug #525)
TinyGPSCustom gsafixtype; // custom extract fix type from GPGSA
TinyGPSCustom gsapdop; // custom extract PDOP from GPGSA
uint8_t fixType = 0; // fix type from GPGSA
#endif
uint32_t fixHoldEnds = 0;
uint32_t rx_gpio = 0;
uint32_t tx_gpio = 0;
uint32_t fixHoldEnds = 0;
uint32_t rx_gpio = 0;
uint32_t tx_gpio = 0;
uint8_t speedSelect = 0;
uint8_t probeTries = 0;
uint8_t speedSelect = 0;
uint8_t probeTries = 0;
/**
* hasValidLocation - indicates that the position variables contain a complete
* GPS location, valid and fresh (< gps_update_interval + position_broadcast_secs)
*/
bool hasValidLocation = false; // default to false, until we complete our first read
/**
* hasValidLocation - indicates that the position variables contain a complete
* GPS location, valid and fresh (< gps_update_interval + position_broadcast_secs)
*/
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 GPSInitStarted = false; // Init thread finished?
bool GPSInitFinished = 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)
static SerialUART *_serial_gps;
static SerialUART *_serial_gps;
#elif defined(ARCH_NRF52)
static Uart *_serial_gps;
static Uart *_serial_gps;
#else
static HardwareSerial *_serial_gps;
static HardwareSerial *_serial_gps;
#endif
// 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 makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
// 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 makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
// scratch space for creating ublox packets
uint8_t UBXscratch[250] = {0};
// scratch space for creating ublox packets
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);
GPS_RESPONSE getACK(uint8_t c, uint8_t i, uint32_t waitMillis);
GPS_RESPONSE getACK(const char *message, 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(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
/// always returns 0 to indicate okay to sleep
int prepareDeepSleep(void *unused);
/// 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
int prepareDeepSleep(void *unused);
/** Set power with EN pin, if relevant
*/
void writePinEN(bool on);
/** Set power with EN pin, if relevant
*/
void writePinEN(bool on);
/** Set the value of the STANDBY pin, if relevant
*/
void writePinStandby(bool standby);
/** Set the value of the STANDBY pin, if relevant
*/
void writePinStandby(bool standby);
/** Set GPS power with PMU, if relevant
*/
void setPowerPMU(bool on);
/** Set GPS power with PMU, if relevant
*/
void setPowerPMU(bool on);
/** Set UBLOX power, if relevant
*/
void setPowerUBLOX(bool on, uint32_t sleepMs = 0);
/** Set UBLOX power, if relevant
*/
void setPowerUBLOX(bool on, uint32_t sleepMs = 0);
/**
* Tell users we have new GPS readings
*/
void publishUpdate();
/**
* Tell users we have new GPS readings
*/
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
GnssModel_t probe(int serialSpeed);
// Get GNSS model
GnssModel_t probe(int serialSpeed);
// delay counter to allow more sats before fixed position stops GPS thread
uint8_t fixeddelayCtr = 0;
// delay counter to allow more sats before fixed position stops GPS thread
uint8_t fixeddelayCtr = 0;
};
extern GPS *gps;
src/gps/GPSUpdateScheduling.cpp
+74 -58
View File
@@ -3,100 +3,116 @@
#include "Default.h"
// Mark the time when searching for GPS position begins
void GPSUpdateScheduling::informSearching() { searchStartedMs = millis(); }
void GPSUpdateScheduling::informSearching()
{
searchStartedMs = millis();
}
// Mark the time when searching for GPS is complete,
// then update the predicted lock-time
void GPSUpdateScheduling::informGotLock() {
searchEndedMs = millis();
LOG_DEBUG("Took %us to get lock", (searchEndedMs - searchStartedMs) / 1000);
updateLockTimePrediction();
void GPSUpdateScheduling::informGotLock()
{
searchEndedMs = millis();
LOG_DEBUG("Took %us to get lock", (searchEndedMs - searchStartedMs) / 1000);
updateLockTimePrediction();
}
// Clear old lock-time prediction data.
// When re-enabling GPS with user button.
void GPSUpdateScheduling::reset() {
searchStartedMs = 0;
searchEndedMs = 0;
searchCount = 0;
predictedMsToGetLock = 0;
void GPSUpdateScheduling::reset()
{
searchStartedMs = 0;
searchEndedMs = 0;
searchCount = 0;
predictedMsToGetLock = 0;
}
// How many milliseconds before we should next search for GPS position
// Used by GPS hardware directly, to enter timed hardware sleep
uint32_t GPSUpdateScheduling::msUntilNextSearch() {
uint32_t now = millis();
uint32_t GPSUpdateScheduling::msUntilNextSearch()
{
uint32_t now = millis();
// Target interval (seconds), between GPS updates
uint32_t updateInterval = Default::getConfiguredOrDefaultMs(config.position.gps_update_interval, default_gps_update_interval);
// Target interval (seconds), between GPS updates
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
uint32_t dueAtMs = searchEndedMs + updateInterval;
uint32_t compensatedStart = dueAtMs - predictedMsToGetLock;
int32_t remainingMs = compensatedStart - now;
// Check how long until we should start searching, to hopefully hit our target interval
uint32_t dueAtMs = searchEndedMs + updateInterval;
uint32_t compensatedStart = dueAtMs - predictedMsToGetLock;
int32_t remainingMs = compensatedStart - now;
// If we should have already started (negative value), start ASAP
if (remainingMs < 0)
remainingMs = 0;
// If we should have already started (negative value), start ASAP
if (remainingMs < 0)
remainingMs = 0;
return (uint32_t)remainingMs;
return (uint32_t)remainingMs;
}
// How long have we already been searching?
// Used to abort a search in progress, if it runs unacceptably long
uint32_t GPSUpdateScheduling::elapsedSearchMs() {
// If searching
if (searchStartedMs > searchEndedMs)
return millis() - searchStartedMs;
uint32_t GPSUpdateScheduling::elapsedSearchMs()
{
// If searching
if (searchStartedMs > searchEndedMs)
return millis() - searchStartedMs;
// If not searching - 0ms. We shouldn't really consume this value
else
return 0;
// If not searching - 0ms. We shouldn't really consume this value
else
return 0;
}
// Is it now time to begin searching for a GPS position?
bool GPSUpdateScheduling::isUpdateDue() { return (msUntilNextSearch() == 0); }
bool GPSUpdateScheduling::isUpdateDue()
{
return (msUntilNextSearch() == 0);
}
// Have we been searching for a GPS position for too long?
bool GPSUpdateScheduling::searchedTooLong() {
uint32_t minimumOrConfiguredSecs = Default::getConfiguredOrMinimumValue(config.position.position_broadcast_secs, default_broadcast_interval_secs);
uint32_t maxSearchMs = Default::getConfiguredOrDefaultMs(minimumOrConfiguredSecs, default_broadcast_interval_secs);
// If broadcast interval set to max, no such thing as "too long"
if (maxSearchMs == UINT32_MAX)
return false;
bool GPSUpdateScheduling::searchedTooLong()
{
uint32_t minimumOrConfiguredSecs =
Default::getConfiguredOrMinimumValue(config.position.position_broadcast_secs, default_broadcast_interval_secs);
uint32_t maxSearchMs = Default::getConfiguredOrDefaultMs(minimumOrConfiguredSecs, default_broadcast_interval_secs);
// If broadcast interval set to max, no such thing as "too long"
if (maxSearchMs == UINT32_MAX)
return false;
// If we've been searching longer than our position broadcast interval: that's too long
else if (elapsedSearchMs() > maxSearchMs)
return true;
// If we've been searching longer than our position broadcast interval: that's too long
else if (elapsedSearchMs() > maxSearchMs)
return true;
// Otherwise, not too long yet!
else
return false;
// Otherwise, not too long yet!
else
return false;
}
// 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?
// Duration between down() calls
int32_t lockTime = searchEndedMs - searchStartedMs;
if (lockTime < 0)
lockTime = 0;
// How long did it take to get GPS lock this time?
// Duration between down() calls
int32_t lockTime = searchEndedMs - searchStartedMs;
if (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
if (searchCount == 1)
predictedMsToGetLock = lockTime;
// Second locktime: likely stable. Use to initialize the smoothing filter
if (searchCount == 1)
predictedMsToGetLock = lockTime;
// Third locktime and after: predict using exponential smoothing. Respond slowly to changes
else if (searchCount > 1)
predictedMsToGetLock = (lockTime * weighting) + (predictedMsToGetLock * (1 - weighting));
// Third locktime and after: predict using exponential smoothing. Respond slowly to changes
else if (searchCount > 1)
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?
uint32_t GPSUpdateScheduling::predictedSearchDurationMs() { return GPSUpdateScheduling::predictedMsToGetLock; }
uint32_t GPSUpdateScheduling::predictedSearchDurationMs()
{
return GPSUpdateScheduling::predictedMsToGetLock;
}
src/gps/GPSUpdateScheduling.h
+19 -18
View File
@@ -3,26 +3,27 @@
#include "configuration.h"
// Encapsulates code responsible for the timing of GPS updates
class GPSUpdateScheduling {
public:
// Marks the time of these events, for calculation use
void informSearching();
void informGotLock(); // Predicted lock-time is recalculated here
class GPSUpdateScheduling
{
public:
// Marks the time of these events, for calculation use
void informSearching();
void informGotLock(); // Predicted lock-time is recalculated here
void reset(); // Reset the prediction - after GPS::disable() / GPS::enable()
bool isUpdateDue(); // Is it time to begin searching for a GPS position?
bool searchedTooLong(); // Have we been searching for too long?
void reset(); // Reset the prediction - after GPS::disable() / GPS::enable()
bool isUpdateDue(); // Is it time to begin searching for a GPS position?
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 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 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 predictedSearchDurationMs(); // How long do we expect to spend searching for a lock?
private:
void updateLockTimePrediction(); // Called from informGotLock
uint32_t searchStartedMs = 0;
uint32_t searchEndedMs = 0;
uint32_t searchCount = 0;
uint32_t predictedMsToGetLock = 0;
private:
void updateLockTimePrediction(); // Called from informGotLock
uint32_t searchStartedMs = 0;
uint32_t searchEndedMs = 0;
uint32_t searchCount = 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
+449 -408
View File
@@ -1,378 +1,402 @@
#include "GeoCoord.h"
GeoCoord::GeoCoord() { _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()
{
_dirty = true;
}
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();
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(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
void GeoCoord::setCoords() {
double lat = _latitude * 1e-7;
double lon = _longitude * 1e-7;
GeoCoord::latLongToDMS(lat, lon, _dms);
GeoCoord::latLongToUTM(lat, lon, _utm);
GeoCoord::latLongToMGRS(lat, lon, _mgrs);
GeoCoord::latLongToOSGR(lat, lon, _osgr);
GeoCoord::latLongToOLC(lat, lon, _olc);
_dirty = false;
void GeoCoord::setCoords()
{
double lat = _latitude * 1e-7;
double lon = _longitude * 1e-7;
GeoCoord::latLongToDMS(lat, lon, _dms);
GeoCoord::latLongToUTM(lat, lon, _utm);
GeoCoord::latLongToMGRS(lat, lon, _mgrs);
GeoCoord::latLongToOSGR(lat, lon, _osgr);
GeoCoord::latLongToOLC(lat, lon, _olc);
_dirty = false;
}
void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt) {
// If marked dirty or new coordinates
if (_dirty || _latitude != lat || _longitude != lon || _altitude != alt) {
_dirty = true;
_latitude = lat;
_longitude = lon;
_altitude = alt;
setCoords();
}
void GeoCoord::updateCoords(int32_t lat, int32_t lon, int32_t alt)
{
// If marked dirty or new coordinates
if (_dirty || _latitude != lat || _longitude != lon || _altitude != alt) {
_dirty = true;
_latitude = lat;
_longitude = lon;
_altitude = alt;
setCoords();
}
}
void GeoCoord::updateCoords(const double lat, const double lon, const int32_t alt) {
int32_t iLat = lat * 1e+7;
int32_t iLon = lon * 1e+7;
// If marked dirty or new coordinates
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
_dirty = true;
_latitude = iLat;
_longitude = iLon;
_altitude = alt;
setCoords();
}
void GeoCoord::updateCoords(const double lat, const double lon, const int32_t alt)
{
int32_t iLat = lat * 1e+7;
int32_t iLon = lon * 1e+7;
// If marked dirty or new coordinates
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
_dirty = true;
_latitude = iLat;
_longitude = iLon;
_altitude = alt;
setCoords();
}
}
void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt) {
int32_t iLat = lat * 1e+7;
int32_t iLon = lon * 1e+7;
// If marked dirty or new coordinates
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
_dirty = true;
_latitude = iLat;
_longitude = iLon;
_altitude = alt;
setCoords();
}
void GeoCoord::updateCoords(const float lat, const float lon, const int32_t alt)
{
int32_t iLat = lat * 1e+7;
int32_t iLon = lon * 1e+7;
// If marked dirty or new coordinates
if (_dirty || _latitude != iLat || _longitude != iLon || _altitude != alt) {
_dirty = true;
_latitude = iLat;
_longitude = iLon;
_altitude = alt;
setCoords();
}
}
/**
* Converts lat long coordinates from decimal degrees to degrees minutes seconds format.
* DD°MM'SS"C DDD°MM'SS"C
*/
void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms) {
if (lat < 0)
dms.latCP = 'S';
else
dms.latCP = 'N';
void GeoCoord::latLongToDMS(const double lat, const double lon, DMS &dms)
{
if (lat < 0)
dms.latCP = 'S';
else
dms.latCP = 'N';
double latDeg = lat;
double latDeg = lat;
if (lat < 0)
latDeg = latDeg * -1;
if (lat < 0)
latDeg = latDeg * -1;
dms.latDeg = floor(latDeg);
double latMin = (latDeg - dms.latDeg) * 60;
dms.latMin = floor(latMin);
dms.latSec = (latMin - dms.latMin) * 60;
dms.latDeg = floor(latDeg);
double latMin = (latDeg - dms.latDeg) * 60;
dms.latMin = floor(latMin);
dms.latSec = (latMin - dms.latMin) * 60;
if (lon < 0)
dms.lonCP = 'W';
else
dms.lonCP = 'E';
if (lon < 0)
dms.lonCP = 'W';
else
dms.lonCP = 'E';
double lonDeg = lon;
double lonDeg = lon;
if (lon < 0)
lonDeg = lonDeg * -1;
if (lon < 0)
lonDeg = lonDeg * -1;
dms.lonDeg = floor(lonDeg);
double lonMin = (lonDeg - dms.lonDeg) * 60;
dms.lonMin = floor(lonMin);
dms.lonSec = (lonMin - dms.lonMin) * 60;
dms.lonDeg = floor(lonDeg);
double lonMin = (lonDeg - dms.lonDeg) * 60;
dms.lonMin = floor(lonMin);
dms.lonSec = (lonMin - dms.lonMin) * 60;
}
/**
* Converts lat long coordinates to UTM.
* 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";
utm.zone = int((lon + 180) / 6 + 1);
utm.band = latBands[int(lat / 8 + 10)];
double a = 6378137; // WGS84 - equatorial radius
double k0 = 0.9996; // UTM point scale on the central meridian
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 latRad = toRadians(lat);
double lonRad = toRadians(lonTemp);
const std::string latBands = "CDEFGHJKLMNPQRSTUVWXX";
utm.zone = int((lon + 180) / 6 + 1);
utm.band = latBands[int(lat / 8 + 10)];
double a = 6378137; // WGS84 - equatorial radius
double k0 = 0.9996; // UTM point scale on the central meridian
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 latRad = toRadians(lat);
double lonRad = toRadians(lonTemp);
// Special Zones for Norway and Svalbard
if (lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0) // Norway
utm.zone = 32;
if (lat >= 72.0 && lat < 84.0) { // Svalbard
if (lonTemp >= 0.0 && lonTemp < 9.0)
utm.zone = 31;
else if (lonTemp >= 9.0 && lonTemp < 21.0)
utm.zone = 33;
else if (lonTemp >= 21.0 && lonTemp < 33.0)
utm.zone = 35;
else if (lonTemp >= 33.0 && lonTemp < 42.0)
utm.zone = 37;
}
// Special Zones for Norway and Svalbard
if (lat >= 56.0 && lat < 64.0 && lonTemp >= 3.0 && lonTemp < 12.0) // Norway
utm.zone = 32;
if (lat >= 72.0 && lat < 84.0) { // Svalbard
if (lonTemp >= 0.0 && lonTemp < 9.0)
utm.zone = 31;
else if (lonTemp >= 9.0 && lonTemp < 21.0)
utm.zone = 33;
else if (lonTemp >= 21.0 && lonTemp < 33.0)
utm.zone = 35;
else if (lonTemp >= 33.0 && lonTemp < 42.0)
utm.zone = 37;
}
double lonOrigin = (utm.zone - 1) * 6 - 180 + 3; // puts origin in middle of zone
double lonOriginRad = toRadians(lonOrigin);
double eccPrimeSquared = (eccSquared) / (1 - eccSquared);
double N = a / sqrt(1 - eccSquared * sin(latRad) * sin(latRad));
double T = tan(latRad) * tan(latRad);
double C = eccPrimeSquared * cos(latRad) * cos(latRad);
double A = cos(latRad) * (lonRad - lonOriginRad);
double M = 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) +
(15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * latRad) -
(35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * 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) +
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)));
double lonOrigin = (utm.zone - 1) * 6 - 180 + 3; // puts origin in middle of zone
double lonOriginRad = toRadians(lonOrigin);
double eccPrimeSquared = (eccSquared) / (1 - eccSquared);
double N = a / sqrt(1 - eccSquared * sin(latRad) * sin(latRad));
double T = tan(latRad) * tan(latRad);
double C = eccPrimeSquared * cos(latRad) * cos(latRad);
double A = cos(latRad) * (lonRad - lonOriginRad);
double M =
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) +
(15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * latRad) -
(35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * 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) +
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)
utm.northing += 10000000.0; // 10000000 meter offset for southern hemisphere
if (lat < 0)
utm.northing += 10000000.0; // 10000000 meter offset for southern hemisphere
}
// Converts lat long coordinates to an MGRS.
void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs) {
const std::string e100kLetters[3] = {"ABCDEFGH", "JKLMNPQR", "STUVWXYZ"};
const std::string n100kLetters[2] = {"ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE"};
UTM utm;
latLongToUTM(lat, lon, utm);
mgrs.zone = utm.zone;
mgrs.band = utm.band;
double col = floor(utm.easting / 100000);
mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3][col - 1];
double row = (int32_t)floor(utm.northing / 100000.0) % 20;
mgrs.north100k = n100kLetters[(mgrs.zone - 1) % 2][row];
mgrs.easting = (int32_t)utm.easting % 100000;
mgrs.northing = (int32_t)utm.northing % 100000;
void GeoCoord::latLongToMGRS(const double lat, const double lon, MGRS &mgrs)
{
const std::string e100kLetters[3] = {"ABCDEFGH", "JKLMNPQR", "STUVWXYZ"};
const std::string n100kLetters[2] = {"ABCDEFGHJKLMNPQRSTUV", "FGHJKLMNPQRSTUVABCDE"};
UTM utm;
latLongToUTM(lat, lon, utm);
mgrs.zone = utm.zone;
mgrs.band = utm.band;
double col = floor(utm.easting / 100000);
mgrs.east100k = e100kLetters[(mgrs.zone - 1) % 3][col - 1];
double row = (int32_t)floor(utm.northing / 100000.0) % 20;
mgrs.north100k = n100kLetters[(mgrs.zone - 1) % 2][row];
mgrs.easting = (int32_t)utm.easting % 100000;
mgrs.northing = (int32_t)utm.northing % 100000;
}
/**
* 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
*/
void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr) {
const char letter[] = "ABCDEFGHJKLMNOPQRSTUVWXYZ"; // No 'I' in OSGR
double a = 6377563.396; // Airy 1830 semi-major axis
double b = 6356256.909; // Airy 1830 semi-minor axis
double f0 = 0.9996012717; // National Grid point scale factor on the central meridian
double phi0 = toRadians(49);
double lambda0 = toRadians(-2);
double n0 = -100000;
double e0 = 400000;
double e2 = 1 - (b * b) / (a * a); // eccentricity squared
double n = (a - b) / (a + b);
void GeoCoord::latLongToOSGR(const double lat, const double lon, OSGR &osgr)
{
const char letter[] = "ABCDEFGHJKLMNOPQRSTUVWXYZ"; // No 'I' in OSGR
double a = 6377563.396; // Airy 1830 semi-major axis
double b = 6356256.909; // Airy 1830 semi-minor axis
double f0 = 0.9996012717; // National Grid point scale factor on the central meridian
double phi0 = toRadians(49);
double lambda0 = toRadians(-2);
double n0 = -100000;
double e0 = 400000;
double e2 = 1 - (b * b) / (a * a); // eccentricity squared
double n = (a - b) / (a + b);
double osgb_Latitude;
double osgb_Longitude;
convertWGS84ToOSGB36(lat, lon, osgb_Latitude, osgb_Longitude);
double phi = osgb_Latitude; // already in radians
double lambda = osgb_Longitude; // already in radians
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 eta2 = v / rho - 1;
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);
// 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 mD = (35 / 24) * n * n * n * sin(3 * (phi - phi0)) * cos(3 * (phi + phi0));
double m = b * f0 * (mA - mB + mC - mD);
double osgb_Latitude;
double osgb_Longitude;
convertWGS84ToOSGB36(lat, lon, osgb_Latitude, osgb_Longitude);
double phi = osgb_Latitude; // already in radians
double lambda = osgb_Longitude; // already in radians
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 eta2 = v / rho - 1;
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);
// 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 mD = (35 / 24) * n * n * n * sin(3 * (phi - phi0)) * cos(3 * (phi + phi0));
double m = b * f0 * (mA - mB + mC - mD);
double cos3Phi = cos(phi) * cos(phi) * cos(phi);
double cos5Phi = cos3Phi * cos(phi) * cos(phi);
double tan2Phi = tan(phi) * tan(phi);
double tan4Phi = tan2Phi * tan2Phi;
double I = m + n0;
double II = (v / 2) * sin(phi) * cos(phi);
double III = (v / 24) * sin(phi) * cos3Phi * (5 - tan2Phi + 9 * eta2);
double IIIA = (v / 720) * sin(phi) * cos5Phi * (61 - 58 * tan2Phi + tan4Phi);
double IV = v * cos(phi);
double V = (v / 6) * cos3Phi * (v / rho - tan2Phi);
double VI = (v / 120) * cos5Phi * (5 - 18 * tan2Phi + tan4Phi + 14 * eta2 - 58 * tan2Phi * eta2);
double cos3Phi = cos(phi) * cos(phi) * cos(phi);
double cos5Phi = cos3Phi * cos(phi) * cos(phi);
double tan2Phi = tan(phi) * tan(phi);
double tan4Phi = tan2Phi * tan2Phi;
double I = m + n0;
double II = (v / 2) * sin(phi) * cos(phi);
double III = (v / 24) * sin(phi) * cos3Phi * (5 - tan2Phi + 9 * eta2);
double IIIA = (v / 720) * sin(phi) * cos5Phi * (61 - 58 * tan2Phi + tan4Phi);
double IV = v * cos(phi);
double V = (v / 6) * cos3Phi * (v / rho - tan2Phi);
double VI = (v / 120) * cos5Phi * (5 - 18 * tan2Phi + tan4Phi + 14 * eta2 - 58 * tan2Phi * eta2);
double deltaLambda = lambda - lambda0;
double deltaLambda2 = deltaLambda * deltaLambda;
double northing = I + II * deltaLambda2 + III * deltaLambda2 * deltaLambda2 + IIIA * deltaLambda2 * deltaLambda2 * deltaLambda2;
double easting = e0 + IV * deltaLambda + V * deltaLambda2 * deltaLambda + VI * deltaLambda2 * deltaLambda2 * deltaLambda;
double deltaLambda = lambda - lambda0;
double deltaLambda2 = deltaLambda * deltaLambda;
double northing =
I + II * deltaLambda2 + III * deltaLambda2 * deltaLambda2 + IIIA * deltaLambda2 * deltaLambda2 * deltaLambda2;
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
osgr = {'I', 'I', 0, 0};
else {
uint32_t e100k = floor(easting / 100000);
uint32_t n100k = floor(northing / 100000);
int8_t l1 = (19 - n100k) - (19 - n100k) % 5 + floor((e100k + 10) / 5);
int8_t l2 = (19 - n100k) * 5 % 25 + e100k % 5;
osgr.e100k = letter[l1];
osgr.n100k = letter[l2];
osgr.easting = floor((int)easting % 100000);
osgr.northing = floor((int)northing % 100000);
}
if (easting < 0 || easting > 700000 || northing < 0 || northing > 1300000) // Check if out of boundaries
osgr = {'I', 'I', 0, 0};
else {
uint32_t e100k = floor(easting / 100000);
uint32_t n100k = floor(northing / 100000);
int8_t l1 = (19 - n100k) - (19 - n100k) % 5 + floor((e100k + 10) / 5);
int8_t l2 = (19 - n100k) * 5 % 25 + e100k % 5;
osgr.e100k = letter[l1];
osgr.n100k = letter[l2];
osgr.easting = floor((int)easting % 100000);
osgr.northing = floor((int)northing % 100000);
}
}
/**
* Converts lat long coordinates to Open Location Code.
* Based on: https://github.com/google/open-location-code/blob/main/c/src/olc.c
*/
void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc) {
char tempCode[] = "1234567890abc";
const char kAlphabet[] = "23456789CFGHJMPQRVWX";
double latitude;
double longitude = lon;
double latitude_degrees = std::min(90.0, std::max(-90.0, lat));
void GeoCoord::latLongToOLC(double lat, double lon, OLC &olc)
{
char tempCode[] = "1234567890abc";
const char kAlphabet[] = "23456789CFGHJMPQRVWX";
double latitude;
double longitude = lon;
double latitude_degrees = std::min(90.0, std::max(-90.0, lat));
if (latitude_degrees < 90) // Check latitude less than lat max
latitude = latitude_degrees;
else {
double precision;
if (OLC_CODE_LEN <= 10)
precision = pow_neg(20, floor((OLC_CODE_LEN / -2) + 2));
else
precision = pow_neg(20, -3) / pow(5, OLC_CODE_LEN - 10);
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;
if (latitude_degrees < 90) // Check latitude less than lat max
latitude = latitude_degrees;
else {
double precision;
if (OLC_CODE_LEN <= 10)
precision = pow_neg(20, floor((OLC_CODE_LEN / -2) + 2));
else
precision = pow_neg(20, -3) / pow(5, OLC_CODE_LEN - 10);
latitude = latitude_degrees - precision / 2;
}
} else {
lat_val /= pow(5, 5);
lng_val /= pow(4, 5);
}
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;
pos = 10;
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;
}
} else {
lat_val /= pow(5, 5);
lng_val /= pow(4, 5);
}
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;
pos = 10;
if (i == 0)
tempCode[pos--] = '+';
}
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;
if (OLC_CODE_LEN < 9) { // Add padding if needed
for (size_t i = OLC_CODE_LEN; i < 9; i++)
tempCode[i] = '0';
tempCode[9] = '+';
}
if (i == 0)
tempCode[pos--] = '+';
}
size_t char_count = OLC_CODE_LEN;
if (10 > char_count) {
char_count = 10;
}
for (size_t i = 0; i < char_count; i++) {
olc.code[i] = tempCode[i];
}
olc.code[char_count] = '\0';
if (OLC_CODE_LEN < 9) { // Add padding if needed
for (size_t i = OLC_CODE_LEN; i < 9; i++)
tempCode[i] = '0';
tempCode[9] = '+';
}
size_t char_count = OLC_CODE_LEN;
if (10 > char_count) {
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.
void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude) {
// Convert lat long to cartesian
double phi = toRadians(lat);
double lambda = toRadians(lon);
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 wgsF = 1 / 298.257223563; // WGS84 datum flattening
double ecc = 2 * wgsF - wgsF * wgsF;
double vee = wgsA / sqrt(1 - ecc * pow(sin(phi), 2));
double wgsX = (vee + h) * cos(phi) * cos(lambda);
double wgsY = (vee + h) * cos(phi) * sin(lambda);
double wgsZ = ((1 - ecc) * vee + h) * sin(phi);
void GeoCoord::convertWGS84ToOSGB36(const double lat, const double lon, double &osgb_Latitude, double &osgb_Longitude)
{
// Convert lat long to cartesian
double phi = toRadians(lat);
double lambda = toRadians(lon);
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 wgsF = 1 / 298.257223563; // WGS84 datum flattening
double ecc = 2 * wgsF - wgsF * wgsF;
double vee = wgsA / sqrt(1 - ecc * pow(sin(phi), 2));
double wgsX = (vee + h) * cos(phi) * cos(lambda);
double wgsY = (vee + h) * cos(phi) * sin(lambda);
double wgsZ = ((1 - ecc) * vee + h) * sin(phi);
// 7-parameter Helmert transform
double tx = -446.448; // x shift in meters
double ty = 125.157; // y 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 rx = toRadians(-0.1502 / 3600); // x 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 osgbX = tx + wgsX * s - wgsY * rz + wgsZ * ry;
double osgbY = ty + wgsX * rz + wgsY * s - wgsZ * rx;
double osgbZ = tz - wgsX * ry + wgsY * rx + wgsZ * s;
// 7-parameter Helmert transform
double tx = -446.448; // x shift in meters
double ty = 125.157; // y 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 rx = toRadians(-0.1502 / 3600); // x 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 osgbX = tx + wgsX * s - wgsY * rz + wgsZ * ry;
double osgbY = ty + wgsX * rz + wgsY * s - wgsZ * rx;
double osgbZ = tz - wgsX * ry + wgsY * rx + wgsZ * s;
// Convert cartesian to lat long
double airyA = 6377563.396; // Airy1830 datum semi major axis
double airyB = 6356256.909; // Airy1830 datum semi minor axis
double airyF = 1 / 299.3249646; // Airy1830 datum flattening
double airyEcc = 2 * airyF - airyF * airyF;
double airyEcc2 = airyEcc / (1 - airyEcc);
double p = sqrt(osgbX * osgbX + osgbY * osgbY);
double R = sqrt(p * p + osgbZ * osgbZ);
double tanBeta = (airyB * osgbZ) / (airyA * p) * (1 + airyEcc2 * airyB / R);
double sinBeta = tanBeta / sqrt(1 + tanBeta * tanBeta);
double cosBeta = sinBeta / tanBeta;
osgb_Latitude = atan2(osgbZ + airyEcc2 * airyB * sinBeta * sinBeta * sinBeta,
p - airyEcc * airyA * cosBeta * cosBeta * cosBeta); // leave in radians
osgb_Longitude = atan2(osgbY, osgbX); // leave in radians
// 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
// Convert cartesian to lat long
double airyA = 6377563.396; // Airy1830 datum semi major axis
double airyB = 6356256.909; // Airy1830 datum semi minor axis
double airyF = 1 / 299.3249646; // Airy1830 datum flattening
double airyEcc = 2 * airyF - airyF * airyF;
double airyEcc2 = airyEcc / (1 - airyEcc);
double p = sqrt(osgbX * osgbX + osgbY * osgbY);
double R = sqrt(p * p + osgbZ * osgbZ);
double tanBeta = (airyB * osgbZ) / (airyA * p) * (1 + airyEcc2 * airyB / R);
double sinBeta = tanBeta / sqrt(1 + tanBeta * tanBeta);
double cosBeta = sinBeta / tanBeta;
osgb_Latitude = atan2(osgbZ + airyEcc2 * airyB * sinBeta * sinBeta * sinBeta,
p - airyEcc * airyA * cosBeta * cosBeta * cosBeta); // leave in radians
osgb_Longitude = atan2(osgbY, osgbX); // leave in radians
// 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
}
/// Ported from my old java code, returns distance in meters along the globe
/// surface (by Haversine formula)
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
if (lat_a == lat_b && lng_a == lng_b)
return 0.0;
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
if (lat_a == lat_b && lng_a == lng_b)
return 0.0;
double a1 = lat_a / DEG_CONVERT;
double a2 = lng_a / DEG_CONVERT;
double b1 = lat_b / DEG_CONVERT;
double b2 = lng_b / DEG_CONVERT;
double cos_b1 = cos(b1);
double cos_a1 = cos(a1);
double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);
double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2);
double t3 = sin(a1) * sin(b1);
double tt = acos(t1 + t2 + t3);
if (std::isnan(tt))
tt = 0.0; // Must have been the same point?
double a1 = lat_a / DEG_CONVERT;
double a2 = lng_a / DEG_CONVERT;
double b1 = lat_b / DEG_CONVERT;
double b2 = lng_b / DEG_CONVERT;
double cos_b1 = cos(b1);
double cos_a1 = cos(a1);
double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2);
double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2);
double t3 = sin(a1) * sin(b1);
double tt = acos(t1 + t2 + t3);
if (std::isnan(tt))
tt = 0.0; // Must have been the same point?
return (float)(6366000 * tt);
return (float)(6366000 * tt);
}
/**
@@ -390,13 +414,14 @@ 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
* north.
*/
float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2) {
double lat1Rad = toRadians(lat1);
double lat2Rad = toRadians(lat2);
double deltaLonRad = toRadians(lon2 - lon1);
double y = sin(deltaLonRad) * cos(lat2Rad);
double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
return atan2(y, x);
float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2)
{
double lat1Rad = toRadians(lat1);
double lat2Rad = toRadians(lat2);
double deltaLonRad = toRadians(lon2 - lon1);
double y = sin(deltaLonRad) * cos(lat2Rad);
double x = cos(lat1Rad) * sin(lat2Rad) - (sin(lat1Rad) * cos(lat2Rad) * cos(deltaLonRad));
return atan2(y, x);
}
/**
@@ -406,10 +431,11 @@ float GeoCoord::bearing(double lat1, double lon1, double lat2, double lon2) {
* The range in meters
* @return range in radians on a great circle
*/
float GeoCoord::rangeMetersToRadians(double range_meters) {
// 1 nm is 1852 meters
double distance_nm = range_meters * 1852;
return (PI / (180 * 60)) * distance_nm;
float GeoCoord::rangeMetersToRadians(double range_meters)
{
// 1 nm is 1852 meters
double distance_nm = range_meters * 1852;
return (PI / (180 * 60)) * distance_nm;
}
/**
@@ -419,20 +445,25 @@ float GeoCoord::rangeMetersToRadians(double range_meters) {
* The range in radians
* @return Range in meters on a great circle
*/
float GeoCoord::rangeRadiansToMeters(double range_radians) {
double distance_nm = ((180 * 60) / PI) * range_radians;
// 1 meter is 0.000539957 nm
return distance_nm * 0.000539957;
float GeoCoord::rangeRadiansToMeters(double range_radians)
{
double distance_nm = ((180 * 60) / PI) * range_radians;
// 1 meter is 0.000539957 nm
return distance_nm * 0.000539957;
}
// Find distance from point to passed in point
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);
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);
}
// Find bearing from point to passed in point
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);
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);
}
/**
@@ -444,15 +475,16 @@ int32_t GeoCoord::bearingTo(const GeoCoord &pointB) {
* range in meters
* @return GeoCoord object of point at bearing and range from initial point
*/
std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range_meters) {
double range_radians = rangeMetersToRadians(range_meters);
double lat1 = this->getLatitude() * 1e-7;
double lon1 = this->getLongitude() * 1e-7;
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 lon = fmod(lon1 - dlon + PI, 2 * PI) - PI;
std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range_meters)
{
double range_radians = rangeMetersToRadians(range_meters);
double lat1 = this->getLatitude() * 1e-7;
double lon1 = this->getLongitude() * 1e-7;
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 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());
}
/**
@@ -461,41 +493,42 @@ std::shared_ptr<GeoCoord> GeoCoord::pointAtDistance(double bearing, double range
* The bearing in string format
* @return Bearing in degrees
*/
unsigned int GeoCoord::bearingToDegrees(const char *bearing) {
if (strcmp(bearing, "N") == 0)
return 0;
else if (strcmp(bearing, "NNE") == 0)
return 22;
else if (strcmp(bearing, "NE") == 0)
return 45;
else if (strcmp(bearing, "ENE") == 0)
return 67;
else if (strcmp(bearing, "E") == 0)
return 90;
else if (strcmp(bearing, "ESE") == 0)
return 112;
else if (strcmp(bearing, "SE") == 0)
return 135;
else if (strcmp(bearing, "SSE") == 0)
return 157;
else if (strcmp(bearing, "S") == 0)
return 180;
else if (strcmp(bearing, "SSW") == 0)
return 202;
else if (strcmp(bearing, "SW") == 0)
return 225;
else if (strcmp(bearing, "WSW") == 0)
return 247;
else if (strcmp(bearing, "W") == 0)
return 270;
else if (strcmp(bearing, "WNW") == 0)
return 292;
else if (strcmp(bearing, "NW") == 0)
return 315;
else if (strcmp(bearing, "NNW") == 0)
return 337;
else
return 0;
unsigned int GeoCoord::bearingToDegrees(const char *bearing)
{
if (strcmp(bearing, "N") == 0)
return 0;
else if (strcmp(bearing, "NNE") == 0)
return 22;
else if (strcmp(bearing, "NE") == 0)
return 45;
else if (strcmp(bearing, "ENE") == 0)
return 67;
else if (strcmp(bearing, "E") == 0)
return 90;
else if (strcmp(bearing, "ESE") == 0)
return 112;
else if (strcmp(bearing, "SE") == 0)
return 135;
else if (strcmp(bearing, "SSE") == 0)
return 157;
else if (strcmp(bearing, "S") == 0)
return 180;
else if (strcmp(bearing, "SSW") == 0)
return 202;
else if (strcmp(bearing, "SW") == 0)
return 225;
else if (strcmp(bearing, "WSW") == 0)
return 247;
else if (strcmp(bearing, "W") == 0)
return 270;
else if (strcmp(bearing, "WNW") == 0)
return 292;
else if (strcmp(bearing, "NW") == 0)
return 315;
else if (strcmp(bearing, "NNW") == 0)
return 337;
else
return 0;
}
/**
@@ -504,52 +537,60 @@ unsigned int GeoCoord::bearingToDegrees(const char *bearing) {
* The bearing in degrees
* @return Bearing in string format
*/
const char *GeoCoord::degreesToBearing(unsigned int degrees) {
if (degrees >= 348 || degrees < 11)
return "N";
else if (degrees >= 11 && degrees < 34)
return "NNE";
else if (degrees >= 34 && degrees < 56)
return "NE";
else if (degrees >= 56 && degrees < 79)
return "ENE";
else if (degrees >= 79 && degrees < 101)
return "E";
else if (degrees >= 101 && degrees < 124)
return "ESE";
else if (degrees >= 124 && degrees < 146)
return "SE";
else if (degrees >= 146 && degrees < 169)
return "SSE";
else if (degrees >= 169 && degrees < 191)
return "S";
else if (degrees >= 191 && degrees < 214)
return "SSW";
else if (degrees >= 214 && degrees < 236)
return "SW";
else if (degrees >= 236 && degrees < 259)
return "WSW";
else if (degrees >= 259 && degrees < 281)
return "W";
else if (degrees >= 281 && degrees < 304)
return "WNW";
else if (degrees >= 304 && degrees < 326)
return "NW";
else if (degrees >= 326 && degrees < 348)
return "NNW";
else
return "N";
const char *GeoCoord::degreesToBearing(unsigned int degrees)
{
if (degrees >= 348 || degrees < 11)
return "N";
else if (degrees >= 11 && degrees < 34)
return "NNE";
else if (degrees >= 34 && degrees < 56)
return "NE";
else if (degrees >= 56 && degrees < 79)
return "ENE";
else if (degrees >= 79 && degrees < 101)
return "E";
else if (degrees >= 101 && degrees < 124)
return "ESE";
else if (degrees >= 124 && degrees < 146)
return "SE";
else if (degrees >= 146 && degrees < 169)
return "SSE";
else if (degrees >= 169 && degrees < 191)
return "S";
else if (degrees >= 191 && degrees < 214)
return "SSW";
else if (degrees >= 214 && degrees < 236)
return "SW";
else if (degrees >= 236 && degrees < 259)
return "WSW";
else if (degrees >= 259 && degrees < 281)
return "W";
else if (degrees >= 281 && degrees < 304)
return "WNW";
else if (degrees >= 304 && degrees < 326)
return "NW";
else if (degrees >= 326 && degrees < 348)
return "NNW";
else
return "N";
}
double GeoCoord::pow_neg(double base, double exponent) {
if (exponent == 0) {
return 1;
} else if (exponent > 0) {
return pow(base, exponent);
}
return 1 / pow(base, -exponent);
double GeoCoord::pow_neg(double base, double exponent)
{
if (exponent == 0) {
return 1;
} else if (exponent > 0) {
return pow(base, exponent);
}
return 1 / pow(base, -exponent);
}
double GeoCoord::toRadians(double deg) { return deg * PI / 180; }
double GeoCoord::toRadians(double deg)
{
return deg * PI / 180;
}
double GeoCoord::toDegrees(double r) { return r * 180 / PI; }
double GeoCoord::toDegrees(double r)
{
return r * 180 / PI;
}
src/gps/GeoCoord.h
+97 -96
View File
@@ -16,132 +16,133 @@
// GeoCoord structs/classes
// A struct to hold the data for a DMS coordinate.
struct DMS {
uint8_t latDeg;
uint8_t latMin;
uint32_t latSec;
char latCP;
uint8_t lonDeg;
uint8_t lonMin;
uint32_t lonSec;
char lonCP;
uint8_t latDeg;
uint8_t latMin;
uint32_t latSec;
char latCP;
uint8_t lonDeg;
uint8_t lonMin;
uint32_t lonSec;
char lonCP;
};
// A struct to hold the data for a UTM coordinate, this is also used when creating an MGRS coordinate.
struct UTM {
uint8_t zone;
char band;
uint32_t easting;
uint32_t northing;
uint8_t zone;
char band;
uint32_t easting;
uint32_t northing;
};
// A struct to hold the data for a MGRS coordinate.
struct MGRS {
uint8_t zone;
char band;
char east100k;
char north100k;
uint32_t easting;
uint32_t northing;
uint8_t zone;
char band;
char east100k;
char north100k;
uint32_t easting;
uint32_t northing;
};
// A struct to hold the data for a OSGR coordinate
struct OSGR {
char e100k;
char n100k;
uint32_t easting;
uint32_t northing;
char e100k;
char n100k;
uint32_t easting;
uint32_t northing;
};
// A struct to hold the data for a OLC coordinate
struct OLC {
char code[OLC_CODE_LEN + 1]; // +1 for null termination
char code[OLC_CODE_LEN + 1]; // +1 for null termination
};
class GeoCoord {
private:
int32_t _latitude = 0;
int32_t _longitude = 0;
int32_t _altitude = 0;
class GeoCoord
{
private:
int32_t _latitude = 0;
int32_t _longitude = 0;
int32_t _altitude = 0;
DMS _dms = {};
UTM _utm = {};
MGRS _mgrs = {};
OSGR _osgr = {};
OLC _olc = {};
DMS _dms = {};
UTM _utm = {};
MGRS _mgrs = {};
OSGR _osgr = {};
OLC _olc = {};
bool _dirty = true;
bool _dirty = true;
void setCoords();
void setCoords();
public:
GeoCoord();
GeoCoord(int32_t lat, int32_t lon, int32_t alt);
GeoCoord(double lat, double lon, int32_t alt);
GeoCoord(float lat, float lon, int32_t alt);
public:
GeoCoord();
GeoCoord(int32_t lat, int32_t lon, int32_t alt);
GeoCoord(double lat, double 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 double lat, const double lon, const int32_t alt);
void updateCoords(const float lat, const float 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 float lat, const float lon, const int32_t alt);
// Conversions
static void latLongToDMS(const double lat, const double lon, DMS &dms);
static void latLongToUTM(const double lat, const double lon, UTM &utm);
static void latLongToMGRS(const double lat, const double lon, MGRS &mgrs);
static void latLongToOSGR(const double lat, const double lon, OSGR &osgr);
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 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 rangeRadiansToMeters(double range_radians);
static float rangeMetersToRadians(double range_meters);
static unsigned int bearingToDegrees(const char *bearing);
static const char *degreesToBearing(unsigned int degrees);
// Conversions
static void latLongToDMS(const double lat, const double lon, DMS &dms);
static void latLongToUTM(const double lat, const double lon, UTM &utm);
static void latLongToMGRS(const double lat, const double lon, MGRS &mgrs);
static void latLongToOSGR(const double lat, const double lon, OSGR &osgr);
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 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 rangeRadiansToMeters(double range_radians);
static float rangeMetersToRadians(double range_meters);
static unsigned int bearingToDegrees(const char *bearing);
static const char *degreesToBearing(unsigned int degrees);
// Raises a number to an exponent, handling negative exponents.
static double pow_neg(double base, double exponent);
static double toRadians(double deg);
static double toDegrees(double r);
// Raises a number to an exponent, handling negative exponents.
static double pow_neg(double base, double exponent);
static double toRadians(double deg);
static double toDegrees(double r);
// Point to point conversions
int32_t distanceTo(const GeoCoord &pointB);
int32_t bearingTo(const GeoCoord &pointB);
std::shared_ptr<GeoCoord> pointAtDistance(double bearing, double range);
// Point to point conversions
int32_t distanceTo(const GeoCoord &pointB);
int32_t bearingTo(const GeoCoord &pointB);
std::shared_ptr<GeoCoord> pointAtDistance(double bearing, double range);
// Lat lon alt getters
int32_t getLatitude() const { return _latitude; }
int32_t getLongitude() const { return _longitude; }
int32_t getAltitude() const { return _altitude; }
// Lat lon alt getters
int32_t getLatitude() const { return _latitude; }
int32_t getLongitude() const { return _longitude; }
int32_t getAltitude() const { return _altitude; }
// DMS getters
uint8_t getDMSLatDeg() const { return _dms.latDeg; }
uint8_t getDMSLatMin() const { return _dms.latMin; }
uint32_t getDMSLatSec() const { return _dms.latSec; }
char getDMSLatCP() const { return _dms.latCP; }
uint8_t getDMSLonDeg() const { return _dms.lonDeg; }
uint8_t getDMSLonMin() const { return _dms.lonMin; }
uint32_t getDMSLonSec() const { return _dms.lonSec; }
char getDMSLonCP() const { return _dms.lonCP; }
// DMS getters
uint8_t getDMSLatDeg() const { return _dms.latDeg; }
uint8_t getDMSLatMin() const { return _dms.latMin; }
uint32_t getDMSLatSec() const { return _dms.latSec; }
char getDMSLatCP() const { return _dms.latCP; }
uint8_t getDMSLonDeg() const { return _dms.lonDeg; }
uint8_t getDMSLonMin() const { return _dms.lonMin; }
uint32_t getDMSLonSec() const { return _dms.lonSec; }
char getDMSLonCP() const { return _dms.lonCP; }
// UTM getters
uint8_t getUTMZone() const { return _utm.zone; }
char getUTMBand() const { return _utm.band; }
uint32_t getUTMEasting() const { return _utm.easting; }
uint32_t getUTMNorthing() const { return _utm.northing; }
// UTM getters
uint8_t getUTMZone() const { return _utm.zone; }
char getUTMBand() const { return _utm.band; }
uint32_t getUTMEasting() const { return _utm.easting; }
uint32_t getUTMNorthing() const { return _utm.northing; }
// MGRS getters
uint8_t getMGRSZone() const { return _mgrs.zone; }
char getMGRSBand() const { return _mgrs.band; }
char getMGRSEast100k() const { return _mgrs.east100k; }
char getMGRSNorth100k() const { return _mgrs.north100k; }
uint32_t getMGRSEasting() const { return _mgrs.easting; }
uint32_t getMGRSNorthing() const { return _mgrs.northing; }
// MGRS getters
uint8_t getMGRSZone() const { return _mgrs.zone; }
char getMGRSBand() const { return _mgrs.band; }
char getMGRSEast100k() const { return _mgrs.east100k; }
char getMGRSNorth100k() const { return _mgrs.north100k; }
uint32_t getMGRSEasting() const { return _mgrs.easting; }
uint32_t getMGRSNorthing() const { return _mgrs.northing; }
// OSGR getters
char getOSGRE100k() const { return _osgr.e100k; }
char getOSGRN100k() const { return _osgr.n100k; }
uint32_t getOSGREasting() const { return _osgr.easting; }
uint32_t getOSGRNorthing() const { return _osgr.northing; }
// OSGR getters
char getOSGRE100k() const { return _osgr.e100k; }
char getOSGRN100k() const { return _osgr.n100k; }
uint32_t getOSGREasting() const { return _osgr.easting; }
uint32_t getOSGRNorthing() const { return _osgr.northing; }
// OLC getter
void getOLCCode(char *code) { strncpy(code, _olc.code, OLC_CODE_LEN + 1); } // +1 for null termination
// OLC getter
void getOLCCode(char *code) { strncpy(code, _olc.code, OLC_CODE_LEN + 1); } // +1 for null termination
};
src/gps/NMEAWPL.cpp
+53 -48
View File
@@ -19,32 +19,36 @@
* -------------------------------------------
*/
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);
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(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(),
(abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), name);
uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i];
}
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
return len;
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);
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(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(),
geoCoord.getDMSLonDeg(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6,
geoCoord.getDMSLonCP(), name);
uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i];
}
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) {
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
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(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(), geoCoord.getDMSLonDeg(),
(abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6, geoCoord.getDMSLonCP(), name);
uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i];
}
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)
{
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
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(),
(abs(geoCoord.getLatitude()) - geoCoord.getDMSLatDeg() * 1e+7) * 6e-6, geoCoord.getDMSLatCP(),
geoCoord.getDMSLonDeg(), (abs(geoCoord.getLongitude()) - geoCoord.getDMSLonDeg() * 1e+7) * 6e-6,
geoCoord.getDMSLonCP(), name);
uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i];
}
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
@@ -62,36 +66,37 @@ uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_Position &pos, const
* 8 Horizontal Dilution of precision (meters)
* 9 Antenna Altitude above/below mean-sea-level (geoid) (in 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 below ellipsoid 12 Units of geoidal separation, meters 13 Age of differential GPS data, time in
* 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
* 11 Geoidal separation, the difference between the WGS-84 earth ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level
* below ellipsoid 12 Units of geoidal separation, meters 13 Age of differential GPS data, time in 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) {
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
time_t timestamp = pos.timestamp;
uint32_t printGGA(char *buf, size_t bufsz, const meshtastic_Position &pos)
{
GeoCoord geoCoord(pos.latitude_i, pos.longitude_i, pos.altitude);
time_t timestamp = pos.timestamp;
tm *t = gmtime(&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);
timestamp = rtc_sec;
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.
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice);
timestamp = rtc_sec;
t = gmtime(&timestamp);
}
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,
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.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);
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,
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.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);
uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i];
}
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
return len;
uint32_t chk = 0;
for (uint32_t i = 1; i < len; i++) {
chk ^= buf[i];
}
len += snprintf(buf + len, bufsz - len, "*%02X\r\n", chk);
return len;
}
#endif
src/gps/RTC.cpp
+305 -291
View File
@@ -12,145 +12,149 @@ uint32_t lastSetFromPhoneNtpOrGps = 0;
static uint32_t lastTimeValidationWarning = 0;
static const uint32_t TIME_VALIDATION_WARNING_INTERVAL_MS = 15000; // 15 seconds
RTCQuality getRTCQuality() { return currentQuality; }
RTCQuality getRTCQuality()
{
return currentQuality;
}
// stuff that really should be in in the instance instead...
static uint32_t timeStartMsec; // Once we have a GPS lock, this is where we hold the initial msec clock that corresponds
// to that time
static uint32_t
timeStartMsec; // Once we have a GPS lock, this is where we hold the initial msec clock that corresponds to that time
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.
* @return True if the RTC was successfully read and the system time was updated, false otherwise.
*/
RTCSetResult readFromRTC() {
struct timeval tv; /* btw settimeofday() is helpful here too*/
RTCSetResult readFromRTC()
{
struct timeval tv; /* btw settimeofday() is helpful here too*/
#ifdef RV3028_RTC
if (rtc_found.address == RV3028_RTC) {
uint32_t now = millis();
Melopero_RV3028 rtc;
if (rtc_found.address == RV3028_RTC) {
uint32_t now = millis();
Melopero_RV3028 rtc;
#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
rtc.initI2C();
rtc.initI2C();
#endif
tm t;
t.tm_year = rtc.getYear() - 1900;
t.tm_mon = rtc.getMonth() - 1;
t.tm_mday = rtc.getDate();
t.tm_hour = rtc.getHour();
t.tm_min = rtc.getMinute();
t.tm_sec = rtc.getSecond();
tv.tv_sec = gm_mktime(&t);
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
tm t;
t.tm_year = rtc.getYear() - 1900;
t.tm_mon = rtc.getMonth() - 1;
t.tm_mday = rtc.getDate();
t.tm_hour = rtc.getHour();
t.tm_min = rtc.getMinute();
t.tm_sec = rtc.getSecond();
tv.tv_sec = gm_mktime(&t);
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
#ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
}
return RTCSetResultInvalidTime;
}
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
}
return RTCSetResultInvalidTime;
}
#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, t.tm_mday, t.tm_hour,
t.tm_min, t.tm_sec, printableEpoch);
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice;
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_min, t.tm_sec, printableEpoch);
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
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)
#if defined(PCF8563_RTC)
if (rtc_found.address == PCF8563_RTC) {
if (rtc_found.address == PCF8563_RTC) {
#elif defined(PCF85063_RTC)
if (rtc_found.address == PCF85063_RTC) {
if (rtc_found.address == PCF85063_RTC) {
#endif
uint32_t now = millis();
SensorRtcHelper rtc;
uint32_t now = millis();
SensorRtcHelper rtc;
#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
rtc.begin(Wire);
rtc.begin(Wire);
#endif
RTC_DateTime datetime = rtc.getDateTime();
tm t = datetime.toUnixTime();
tv.tv_sec = gm_mktime(&t);
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
RTC_DateTime datetime = rtc.getDateTime();
tm t = datetime.toUnixTime();
tv.tv_sec = gm_mktime(&t);
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
#ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
}
#endif
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_mday, t.tm_hour, t.tm_min, t.tm_sec, printableEpoch);
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
#elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) {
uint32_t now = millis();
#ifdef MUZI_BASE
ArtronShop_RX8130CE rtc(&Wire1);
#else
ArtronShop_RX8130CE rtc(&Wire);
#endif
tm t;
if (rtc.getTime(&t)) {
tv.tv_sec = gm_mktime(&t);
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
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_hour, t.tm_min, t.tm_sec, printableEpoch);
#ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis();
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
}
return RTCSetResultInvalidTime;
}
#endif
if (currentQuality == RTCQualityNone) {
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_mday, t.tm_hour, t.tm_min, t.tm_sec, printableEpoch);
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
#elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) {
uint32_t now = millis();
#ifdef MUZI_BASE
ArtronShop_RX8130CE rtc(&Wire1);
#else
ArtronShop_RX8130CE rtc(&Wire);
#endif
tm t;
if (rtc.getTime(&t)) {
tv.tv_sec = gm_mktime(&t);
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
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_hour, t.tm_min, t.tm_sec, printableEpoch);
#ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis();
}
return RTCSetResultInvalidTime;
}
#endif
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
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;
currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
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
return RTCSetResultNotSet;
return RTCSetResultNotSet;
}
/**
@@ -162,140 +166,143 @@ RTCSetResult readFromRTC() {
*
* 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) {
static uint32_t lastSetMsec = 0;
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
RTCSetResult perhapsSetRTC(RTCQuality q, const struct timeval *tv, bool forceUpdate)
{
static uint32_t lastSetMsec = 0;
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
#ifdef BUILD_EPOCH
if (tv->tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis();
if (tv->tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%ld) before build epoch (%ld)!", printableEpoch, BUILD_EPOCH);
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
bool shouldSet;
if (forceUpdate) {
shouldSet = true;
LOG_DEBUG("Override current RTC quality (%s) with incoming time of RTC quality of %s", RtcName(currentQuality), RtcName(q));
} else if (q > currentQuality) {
shouldSet = true;
LOG_DEBUG("Upgrade time to quality %s", RtcName(q));
} else if (q == RTCQualityGPS) {
shouldSet = true;
LOG_DEBUG("Reapply GPS time: %ld secs", printableEpoch);
} 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
shouldSet = true;
LOG_DEBUG("Reapply external time to correct clock drift %ld secs", printableEpoch);
} else {
shouldSet = false;
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;
}
// 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
if (rtc_found.address == RV3028_RTC) {
Melopero_RV3028 rtc;
#if WIRE_INTERFACES_COUNT == 2
rtc.initI2C(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else
rtc.initI2C();
#endif
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);
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_sec, printableEpoch);
bool shouldSet;
if (forceUpdate) {
shouldSet = true;
LOG_DEBUG("Override current RTC quality (%s) with incoming time of RTC quality of %s", RtcName(currentQuality),
RtcName(q));
} else if (q > currentQuality) {
shouldSet = true;
LOG_DEBUG("Upgrade time to quality %s", RtcName(q));
} else if (q == RTCQualityGPS) {
shouldSet = true;
LOG_DEBUG("Reapply GPS time: %ld secs", printableEpoch);
} 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
shouldSet = true;
LOG_DEBUG("Reapply external time to correct clock drift %ld secs", printableEpoch);
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
shouldSet = false;
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;
}
// 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
if (rtc_found.address == RV3028_RTC) {
Melopero_RV3028 rtc;
#if WIRE_INTERFACES_COUNT == 2
rtc.initI2C(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else
rtc.initI2C();
#endif
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);
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_sec, printableEpoch);
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
#elif defined(PCF8563_RTC) || defined(PCF85063_RTC)
#if defined(PCF8563_RTC)
if (rtc_found.address == PCF8563_RTC) {
if (rtc_found.address == PCF8563_RTC) {
#elif defined(PCF85063_RTC)
if (rtc_found.address == PCF85063_RTC) {
if (rtc_found.address == PCF85063_RTC) {
#endif
SensorRtcHelper rtc;
SensorRtcHelper rtc;
#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
rtc.begin(Wire);
rtc.begin(Wire);
#endif
tm *t = gmtime(&tv->tv_sec);
rtc.setDateTime(*t);
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_min, t->tm_sec, printableEpoch);
} else {
LOG_WARN("RTC not found (found address 0x%02X)", rtc_found.address);
}
tm *t = gmtime(&tv->tv_sec);
rtc.setDateTime(*t);
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_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) {
if (rtc_found.address == RX8130CE_RTC) {
#ifdef MUZI_BASE
ArtronShop_RX8130CE rtc(&Wire1);
ArtronShop_RX8130CE rtc(&Wire1);
#else
ArtronShop_RX8130CE rtc(&Wire);
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");
}
}
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);
settimeofday(tv, NULL);
#endif
// nrf52 doesn't have a readable RTC (yet - software not written)
// nrf52 doesn't have a readable RTC (yet - software not written)
#if HAS_RTC
readFromRTC();
readFromRTC();
#endif
return RTCSetResultSuccess;
} else {
return RTCSetResultNotSet; // RTC was already set with a higher quality time
}
return RTCSetResultSuccess;
} else {
return RTCSetResultNotSet; // RTC was already set with a higher quality time
}
}
const char *RtcName(RTCQuality quality) {
switch (quality) {
case RTCQualityNone:
return "None";
case RTCQualityDevice:
return "Device";
case RTCQualityFromNet:
return "Net";
case RTCQualityNTP:
return "NTP";
case RTCQualityGPS:
return "GPS";
default:
return "Unknown";
}
const char *RtcName(RTCQuality quality)
{
switch (quality) {
case RTCQualityNone:
return "None";
case RTCQualityDevice:
return "Device";
case RTCQualityFromNet:
return "Net";
case RTCQualityNTP:
return "NTP";
case RTCQualityGPS:
return "GPS";
default:
return "Unknown";
}
}
/**
@@ -305,45 +312,46 @@ const char *RtcName(RTCQuality quality) {
* @param t The time to potentially set the RTC to.
* @return True if the RTC was set to the provided time, false otherwise.
*/
RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t) {
/* 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
1, 1970 (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
// https://www.gnu.org/software/libc/manual/html_node/Broken_002ddown-Time.html
time_t res = gm_mktime(&t);
struct timeval tv;
tv.tv_sec = res;
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
RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t)
{
/* 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 1, 1970
(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
// https://www.gnu.org/software/libc/manual/html_node/Broken_002ddown-Time.html
time_t res = gm_mktime(&t);
struct timeval tv;
tv.tv_sec = res;
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
#ifdef BUILD_EPOCH
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%lu) before build epoch (%lu)!", printableEpoch, BUILD_EPOCH);
lastTimeValidationWarning = millis();
if (tv.tv_sec < BUILD_EPOCH) {
if (Throttle::isWithinTimespanMs(lastTimeValidationWarning, TIME_VALIDATION_WARNING_INTERVAL_MS) == false) {
LOG_WARN("Ignore time (%lu) before build epoch (%lu)!", printableEpoch, BUILD_EPOCH);
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
// 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) {
// LOG_DEBUG("Ignore invalid GPS month=%d, year=%d, unixtime=%ld", t.tm_mon, t.tm_year, tv.tv_sec);
return RTCSetResultInvalidTime;
} else {
return perhapsSetRTC(q, &tv);
}
// 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) {
// LOG_DEBUG("Ignore invalid GPS month=%d, year=%d, unixtime=%ld", t.tm_mon, t.tm_year, tv.tv_sec);
return RTCSetResultInvalidTime;
} else {
return perhapsSetRTC(q, &tv);
}
}
/**
@@ -351,15 +359,16 @@ RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t) {
*
* @return The timezone offset in seconds.
*/
int32_t getTZOffset() {
int32_t getTZOffset()
{
#if MESHTASTIC_EXCLUDE_TZ
return 0;
return 0;
#else
time_t now = getTime(false);
struct tm *gmt;
gmt = gmtime(&now);
gmt->tm_isdst = -1;
return (int32_t)difftime(now, mktime(gmt));
time_t now = getTime(false);
struct tm *gmt;
gmt = gmtime(&now);
gmt->tm_isdst = -1;
return (int32_t)difftime(now, mktime(gmt));
#endif
}
@@ -368,12 +377,13 @@ int32_t getTZOffset() {
*
* @return The current time in seconds since the Unix epoch.
*/
uint32_t getTime(bool local) {
if (local) {
return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs + getTZOffset();
} else {
return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs;
}
uint32_t getTime(bool local)
{
if (local) {
return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs + getTZOffset();
} else {
return (((uint32_t)millis() - timeStartMsec) / 1000) + zeroOffsetSecs;
}
}
/**
@@ -382,45 +392,49 @@ uint32_t getTime(bool local) {
* @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.
*/
uint32_t getValidTime(RTCQuality minQuality, bool local) { return (currentQuality >= minQuality) ? getTime(local) : 0; }
uint32_t getValidTime(RTCQuality minQuality, bool local)
{
return (currentQuality >= minQuality) ? getTime(local) : 0;
}
time_t gm_mktime(struct tm *tm) {
time_t gm_mktime(struct tm *tm)
{
#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.
int year = 1900 + tm->tm_year; // tm_year is years since 1900
int year_minus_one = year - 1;
int days_before_this_year = 0;
days_before_this_year += year_minus_one * 365;
// 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;
// subtract from 1970-01-01 to get days since epoch
days_before_this_year -= 719162; // (1969 * 365 + 1969 / 4 - 1969 / 100 + 1969 / 400);
// 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_minus_one = year - 1;
int days_before_this_year = 0;
days_before_this_year += year_minus_one * 365;
// 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;
// subtract from 1970-01-01 to get days since epoch
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.
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
// 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_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 (tm->tm_mon >= 2 && (year % 4) == 0 && ((year % 100) != 0 || (year % 400) == 0)) {
days_this_year_before_this_month += 1;
}
// 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)) {
days_this_year_before_this_month += 1;
}
// And within this month:
int days_this_month_before_today = tm->tm_mday - 1; // tm->tm_mday is 1..31
// And within this month:
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:
result += (days_before_this_year + days_this_year_before_this_month + days_this_month_before_today);
result *= 86400L;
// 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 *= 86400L;
// Finally, add in the hours, minutes, and seconds of today:
result += tm->tm_hour * 3600;
result += tm->tm_min * 60;
result += tm->tm_sec;
// Finally, add in the hours, minutes, and seconds of today:
result += tm->tm_hour * 3600;
result += tm->tm_min * 60;
result += tm->tm_sec;
return result;
return result;
#else
return mktime(tm);
return mktime(tm);
#endif
}
src/gps/RTC.h
+14 -14
View File
@@ -10,29 +10,29 @@
enum RTCQuality {
/// We haven't had our RTC set yet
RTCQualityNone = 0,
/// We haven't had our RTC set yet
RTCQualityNone = 0,
/// We got time from an onboard peripheral after boot.
RTCQualityDevice = 1,
/// We got time from an onboard peripheral after boot.
RTCQualityDevice = 1,
/// Some other node gave us a time we can use
RTCQualityFromNet = 2,
/// Some other node gave us a time we can use
RTCQualityFromNet = 2,
/// Our time is based on NTP
RTCQualityNTP = 3,
/// Our time is based on NTP
RTCQualityNTP = 3,
/// Our time is based on our own GPS
RTCQualityGPS = 4
/// Our time is based on our own GPS
RTCQualityGPS = 4
};
/// The RTC set result codes
/// Used to indicate the result of an attempt to set the RTC.
enum RTCSetResult {
RTCSetResultNotSet = 0, ///< RTC was set successfully
RTCSetResultSuccess = 1, ///< RTC was set successfully
RTCSetResultInvalidTime = 3, ///< The provided time was invalid (e.g., before the build epoch)
RTCSetResultError = 4 ///< An error occurred while setting the RTC
RTCSetResultNotSet = 0, ///< RTC was set successfully
RTCSetResultSuccess = 1, ///< RTC was set successfully
RTCSetResultInvalidTime = 3, ///< The provided time was invalid (e.g., before the build epoch)
RTCSetResultError = 4 ///< An error occurred while setting the RTC
};
RTCQuality getRTCQuality();
src/gps/ubx.h
+26 -24
View File
@@ -1,13 +1,13 @@
static const char *failMessage = "Unable to %s";
#define SEND_UBX_PACKET(TYPE, ID, DATA, ERRMSG, TIMEOUT) \
do { \
msglen = makeUBXPacket(TYPE, ID, sizeof(DATA), DATA); \
_serial_gps->write(UBXscratch, msglen); \
if (getACK(TYPE, ID, TIMEOUT) != GNSS_RESPONSE_OK) { \
LOG_WARN(failMessage, #ERRMSG); \
} \
} while (0)
#define SEND_UBX_PACKET(TYPE, ID, DATA, ERRMSG, TIMEOUT) \
do { \
msglen = makeUBXPacket(TYPE, ID, sizeof(DATA), DATA); \
_serial_gps->write(UBXscratch, msglen); \
if (getACK(TYPE, ID, TIMEOUT) != GNSS_RESPONSE_OK) { \
LOG_WARN(failMessage, #ERRMSG); \
} \
} while (0)
// 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.
// 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.
// for all configurations using sleep / low power modes, V_BCKP needs to be hooked to permanent power for fast
// aquisition after sleep
// for all configurations using sleep / low power modes, V_BCKP needs to be hooked to permanent power for fast aquisition after
// sleep
// VALSET Commands for M10
// Please refer to the M10 Protocol Specification:
@@ -370,13 +370,11 @@ EXTINTACTIVITY U4 0 no ext ints
LIMITPEAKCURRENT L 1
// 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
// 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
*/
static const uint8_t _message_VALSET_PM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40,
@@ -398,21 +396,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
*/
static const uint8_t _message_VALSET_ITFM_RAM[] = {0x00, 0x01, 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};
static const uint8_t _message_VALSET_ITFM_RAM[] = {0x00, 0x01, 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:
// 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
// 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
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,
0x20, 0x00, 0xac, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x00};
@@ -439,10 +437,14 @@ 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_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, 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_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};
static const uint8_t _message_VALSET_ENABLE_NMEA_RAM[] = {0x00, 0x01, 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,
0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01};
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:
src/graphics/EInkDisplay2.cpp
+180 -171
View File
@@ -33,243 +33,252 @@
*/
// Constructor
EInkDisplay::EInkDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus) {
// Set dimensions in OLEDDisplay base class
this->geometry = GEOMETRY_RAWMODE;
this->displayWidth = EINK_WIDTH;
this->displayHeight = EINK_HEIGHT;
EInkDisplay::EInkDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus)
{
// Set dimensions in OLEDDisplay base class
this->geometry = GEOMETRY_RAWMODE;
this->displayWidth = EINK_WIDTH;
this->displayHeight = EINK_HEIGHT;
// Round shortest side up to nearest byte, to prevent truncation causing an undersized buffer
uint16_t shortSide = min(EINK_WIDTH, EINK_HEIGHT);
uint16_t longSide = max(EINK_WIDTH, EINK_HEIGHT);
if (shortSide % 8 != 0)
shortSide = (shortSide | 7) + 1;
// Round shortest side up to nearest byte, to prevent truncation causing an undersized buffer
uint16_t shortSide = min(EINK_WIDTH, EINK_HEIGHT);
uint16_t longSide = max(EINK_WIDTH, EINK_HEIGHT);
if (shortSide % 8 != 0)
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
*/
bool EInkDisplay::forceDisplay(uint32_t msecLimit) {
// No need to grab this lock because we are on our own SPI bus
// concurrency::LockGuard g(spiLock);
bool EInkDisplay::forceDisplay(uint32_t msecLimit)
{
// No need to grab this lock because we are on our own SPI bus
// concurrency::LockGuard g(spiLock);
uint32_t now = millis();
uint32_t sinceLast = now - lastDrawMsec;
uint32_t now = millis();
uint32_t sinceLast = now - lastDrawMsec;
if (adafruitDisplay && (sinceLast > msecLimit || lastDrawMsec == 0))
lastDrawMsec = now;
else
return false;
if (adafruitDisplay && (sinceLast > msecLimit || lastDrawMsec == 0))
lastDrawMsec = now;
else
return false;
// FIXME - only draw bits have changed (use backbuf similar to the other displays)
const bool flipped = config.display.flip_screen;
// HACK for L1 EInk
// FIXME - only draw bits have changed (use backbuf similar to the other displays)
const bool flipped = config.display.flip_screen;
// HACK for 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 (uint32_t y = 0; y < displayHeight; y++) {
for (uint32_t x = 0; x < displayWidth; x++) {
auto b = buffer[x + (y / 8) * displayWidth];
auto isset = b & (1 << (y & 7));
adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE);
// 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 x = 0; x < displayWidth; x++) {
auto b = buffer[x + (y / 8) * displayWidth];
auto isset = b & (1 << (y & 7));
adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE);
}
}
}
#else
for (uint32_t y = 0; y < displayHeight; y++) {
for (uint32_t x = 0; x < displayWidth; x++) {
auto b = buffer[x + (y / 8) * displayWidth];
auto isset = b & (1 << (y & 7));
if (flipped)
adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE);
else
adafruitDisplay->drawPixel(x, y, isset ? GxEPD_BLACK : GxEPD_WHITE);
for (uint32_t y = 0; y < displayHeight; y++) {
for (uint32_t x = 0; x < displayWidth; x++) {
auto b = buffer[x + (y / 8) * displayWidth];
auto isset = b & (1 << (y & 7));
if (flipped)
adafruitDisplay->drawPixel((displayWidth - 1) - x, (displayHeight - 1) - y, isset ? GxEPD_BLACK : GxEPD_WHITE);
else
adafruitDisplay->drawPixel(x, y, isset ? GxEPD_BLACK : GxEPD_WHITE);
}
}
}
#endif
// Trigger the refresh in GxEPD2
LOG_DEBUG("Update E-Paper");
adafruitDisplay->nextPage();
// Trigger the refresh in GxEPD2
LOG_DEBUG("Update E-Paper");
adafruitDisplay->nextPage();
// End the update process
endUpdate();
// End the update process
endUpdate();
LOG_DEBUG("done");
return true;
LOG_DEBUG("done");
return true;
}
// End the update process - virtual method, overriden in derived class
void EInkDisplay::endUpdate() {
// Power off display hardware, then deep-sleep (Except Wireless Paper V1.1, no deep-sleep)
adafruitDisplay->hibernate();
void EInkDisplay::endUpdate()
{
// Power off display hardware, then deep-sleep (Except Wireless Paper V1.1, no deep-sleep)
adafruitDisplay->hibernate();
}
// Write the buffer to the display memory
void EInkDisplay::display(void) {
// 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
// bootscreen (that we want to look nice)
void EInkDisplay::display(void)
{
// 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
// bootscreen (that we want to look nice)
if (lastDrawMsec) {
forceDisplay(slowUpdateMsec); // Show the first screen a few seconds after boot, then slower
}
if (lastDrawMsec) {
forceDisplay(slowUpdateMsec); // Show the first screen a few seconds after boot, then slower
}
}
// Send a command to the display (low level function)
void EInkDisplay::sendCommand(uint8_t com) {
(void)com;
// Drop all commands to device (we just update the buffer)
void EInkDisplay::sendCommand(uint8_t com)
{
(void)com;
// Drop all commands to device (we just update the buffer)
}
void EInkDisplay::setDetected(uint8_t detected) { (void)detected; }
void EInkDisplay::setDetected(uint8_t detected)
{
(void)detected;
}
// Connect to the display - variant specific
bool EInkDisplay::connect() {
LOG_INFO("Do EInk init");
bool EInkDisplay::connect()
{
LOG_INFO("Do EInk init");
#ifdef PIN_EINK_EN
// backlight power, HIGH is backlight on, LOW is off
pinMode(PIN_EINK_EN, OUTPUT);
// backlight power, HIGH is backlight on, LOW is off
pinMode(PIN_EINK_EN, OUTPUT);
#ifdef ELECROW_ThinkNode_M1
// ThinkNode M1 has a hardware dimmable backlight. Start enabled
digitalWrite(PIN_EINK_EN, HIGH);
// ThinkNode M1 has a hardware dimmable backlight. Start enabled
digitalWrite(PIN_EINK_EN, HIGH);
#else
digitalWrite(PIN_EINK_EN, LOW);
digitalWrite(PIN_EINK_EN, LOW);
#endif
#endif
#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();
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);
adafruitDisplay->setRotation(4);
#else
adafruitDisplay->setRotation(3);
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;
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
#elif defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E290) || defined(TLORA_T3S3_EPAPER) || \
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()
{
// 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);
// 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);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
#if defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER)
adafruitDisplay->setRotation(0);
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);
}
{
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->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()
{
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);
// 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);
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
return true;
return true;
}
#endif
src/graphics/EInkDisplay2.h
+50 -49
View File
@@ -22,74 +22,75 @@
* 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()
*/
class EInkDisplay : public OLEDDisplay {
/// How often should we update the display
/// thereafter we do once per 5 minutes
uint32_t slowUpdateMsec = 5 * 60 * 1000;
class EInkDisplay : public OLEDDisplay
{
/// How often should we update the display
/// thereafter we do once per 5 minutes
uint32_t slowUpdateMsec = 5 * 60 * 1000;
public:
/* constructor
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);
public:
/* constructor
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);
// Write the buffer to the display memory (for eink we only do this occasionally)
virtual void display(void) override;
// Write the buffer to the display memory (for eink we only do this occasionally)
virtual void display(void) override;
/**
* Force a display update if we haven't drawn within the specified msecLimit
*
* @return true if we did draw the screen
*/
virtual bool forceDisplay(uint32_t msecLimit = 1000);
/**
* Force a display update if we haven't drawn within the specified msecLimit
*
* @return true if we did draw the screen
*/
virtual bool forceDisplay(uint32_t msecLimit = 1000);
/**
* 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.
*
*/
virtual void endUpdate();
/**
* 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.
*
*/
virtual void endUpdate();
/**
* shim to make the abstraction happy
*
*/
void setDetected(uint8_t detected);
/**
* shim to make the abstraction happy
*
*/
void setDetected(uint8_t detected);
protected:
// the header size of the buffer used, e.g. for the SPI command header
virtual int getBufferOffset(void) override { return 0; }
protected:
// the header size of the buffer used, e.g. for the SPI command header
virtual int getBufferOffset(void) override { return 0; }
// Send a command to the display (low level function)
virtual void sendCommand(uint8_t com) override;
// Send a command to the display (low level function)
virtual void sendCommand(uint8_t com) override;
// Connect to the display
virtual bool connect() override;
// Connect to the display
virtual bool connect() override;
#ifdef GXEPD2_DRIVER_0
// AdafruitGFX display object - wrapper for multiple drivers
// Allows runtime detection of multiple displays
// Avoid this situation if possible!
GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1> *adafruitDisplay = NULL;
// AdafruitGFX display object - wrapper for multiple drivers
// Allows runtime detection of multiple displays
// Avoid this situation if possible!
GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1> *adafruitDisplay = NULL;
#else
// AdafruitGFX display object (for single display model) - instantiated in connect(), variant specific
GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT> *adafruitDisplay = NULL;
// AdafruitGFX display object (for single display model) - instantiated in connect(), variant specific
GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT> *adafruitDisplay = NULL;
#endif
// If display uses HSPI
#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) || \
// If display uses HSPI
#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(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER) || defined(ELECROW_ThinkNode_M5)
SPIClass *hspi = NULL;
SPIClass *hspi = NULL;
#endif
#if defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK)
SPIClass *spi1 = NULL;
SPIClass *spi1 = NULL;
#endif
private:
// FIXME quick hack to limit drawing to a very slow rate
uint32_t lastDrawMsec = 0;
private:
// FIXME quick hack to limit drawing to a very slow rate
uint32_t lastDrawMsec = 0;
};
#endif
src/graphics/EInkDynamicDisplay.cpp
+380 -346
View File
@@ -6,524 +6,558 @@
// Constructor
EInkDynamicDisplay::EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus)
: EInkDisplay(address, sda, scl, geometry, i2cBus), NotifiedWorkerThread("EInkDynamicDisplay") {
// If tracking ghost pixels, grab memory
: EInkDisplay(address, sda, scl, geometry, i2cBus), NotifiedWorkerThread("EInkDynamicDisplay")
{
// If tracking ghost pixels, grab memory
#ifdef EINK_LIMIT_GHOSTING_PX
dirtyPixels = new uint8_t[EInkDisplay::displayBufferSize](); // Init with zeros
dirtyPixels = new uint8_t[EInkDisplay::displayBufferSize](); // Init with zeros
#endif
}
// Destructor
EInkDynamicDisplay::~EInkDynamicDisplay() {
// If we were tracking ghost pixels, free the memory
EInkDynamicDisplay::~EInkDynamicDisplay()
{
// If we were tracking ghost pixels, free the memory
#ifdef EINK_LIMIT_GHOSTING_PX
delete[] dirtyPixels;
delete[] dirtyPixels;
#endif
}
// Screen requests a BACKGROUND frame
void EInkDynamicDisplay::display() {
addFrameFlag(BACKGROUND);
update();
void EInkDynamicDisplay::display()
{
addFrameFlag(BACKGROUND);
update();
}
// Screen requests a RESPONSIVE frame
bool EInkDynamicDisplay::forceDisplay(uint32_t msecLimit) {
addFrameFlag(RESPONSIVE);
return update(); // (Unutilized) Base class promises to return true if update ran
bool EInkDynamicDisplay::forceDisplay(uint32_t msecLimit)
{
addFrameFlag(RESPONSIVE);
return update(); // (Unutilized) Base class promises to return true if update ran
}
// Add flag for the next frame
void EInkDynamicDisplay::addFrameFlag(frameFlagTypes flag) {
// OR the new flag into the existing flags
this->frameFlags = (frameFlagTypes)(this->frameFlags | flag);
void EInkDynamicDisplay::addFrameFlag(frameFlagTypes flag)
{
// OR the new flag into the existing flags
this->frameFlags = (frameFlagTypes)(this->frameFlags | flag);
}
// GxEPD2 code to set fast refresh
void EInkDynamicDisplay::configForFastRefresh() {
// Variant-specific code can go here
void EInkDynamicDisplay::configForFastRefresh()
{
// Variant-specific code can go here
#if defined(PRIVATE_HW)
#else
// Otherwise:
adafruitDisplay->setPartialWindow(0, 0, adafruitDisplay->width(), adafruitDisplay->height());
// Otherwise:
adafruitDisplay->setPartialWindow(0, 0, adafruitDisplay->width(), adafruitDisplay->height());
#endif
}
// GxEPD2 code to set full refresh
void EInkDynamicDisplay::configForFullRefresh() {
// Variant-specific code can go here
void EInkDynamicDisplay::configForFullRefresh()
{
// Variant-specific code can go here
#if defined(PRIVATE_HW)
#else
// Otherwise:
adafruitDisplay->setFullWindow();
// Otherwise:
adafruitDisplay->setFullWindow();
#endif
}
// Run any relevant GxEPD2 code, so next update will use correct refresh type
void EInkDynamicDisplay::applyRefreshMode() {
// Change from FULL to FAST
if (currentConfig == FULL && refresh == FAST) {
configForFastRefresh();
currentConfig = FAST;
}
void EInkDynamicDisplay::applyRefreshMode()
{
// Change from FULL to FAST
if (currentConfig == FULL && refresh == FAST) {
configForFastRefresh();
currentConfig = FAST;
}
// Change from FAST back to FULL
else if (currentConfig == FAST && refresh == FULL) {
configForFullRefresh();
currentConfig = FULL;
}
// Change from FAST back to FULL
else if (currentConfig == FAST && refresh == FULL) {
configForFullRefresh();
currentConfig = FULL;
}
}
// Update fastRefreshCount
void EInkDynamicDisplay::adjustRefreshCounters() {
if (refresh == FAST)
fastRefreshCount++;
void EInkDynamicDisplay::adjustRefreshCounters()
{
if (refresh == FAST)
fastRefreshCount++;
else if (refresh == FULL)
fastRefreshCount = 0;
else if (refresh == FULL)
fastRefreshCount = 0;
}
// Trigger the display update by calling base class
bool EInkDynamicDisplay::update() {
// Detemine the refresh mode to use, and start the update
bool refreshApproved = determineMode();
if (refreshApproved) {
EInkDisplay::forceDisplay(0); // Bypass base class' own rate-limiting system
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)
} else
storeAndReset(); // No update, no post-update code, just store the results
bool EInkDynamicDisplay::update()
{
// Detemine the refresh mode to use, and start the update
bool refreshApproved = determineMode();
if (refreshApproved) {
EInkDisplay::forceDisplay(0); // Bypass base class' own rate-limiting system
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)
} else
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
void EInkDynamicDisplay::endOrDetach() {
// If the GxEPD2 version reports that it has the async modifications
void EInkDynamicDisplay::endOrDetach()
{
// If the GxEPD2 version reports that it has the async modifications
#ifdef HAS_EINK_ASYNCFULL
if (previousRefresh == FULL) {
asyncRefreshRunning = true; // Set the flag - checked in determineMode(); cleared by onNotify()
if (previousRefresh == FULL) {
asyncRefreshRunning = true; // Set the flag - checked in determineMode(); cleared by onNotify()
if (previousFrameFlags & BLOCKING)
awaitRefresh();
else {
// Async begins
LOG_DEBUG("Async full-refresh begins (drop frames)");
notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true); // Hand-off to NotifiedWorkerThread
if (previousFrameFlags & BLOCKING)
awaitRefresh();
else {
// Async begins
LOG_DEBUG("Async full-refresh begins (drop frames)");
notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true); // Hand-off to NotifiedWorkerThread
}
}
}
// Fast Refresh
else if (previousRefresh == FAST)
EInkDisplay::endUpdate(); // Still block while updating, but EInkDisplay needs us to call endUpdate() ourselves.
// Fast Refresh
else if (previousRefresh == FAST)
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
if (previousRefresh == FULL || previousRefresh == FAST) { // If refresh wasn't skipped (on unspecified..)
LOG_WARN("GxEPD2 version has not been modified to support async refresh; using fallback behavior. Please update "
"lib_deps in "
"variant's platformio.ini file");
EInkDisplay::endUpdate();
}
if (previousRefresh == FULL || previousRefresh == FAST) { // If refresh wasn't skipped (on unspecified..)
LOG_WARN(
"GxEPD2 version has not been modified to support async refresh; using fallback behavior. Please update lib_deps in "
"variant's platformio.ini file");
EInkDisplay::endUpdate();
}
#endif
}
// Assess situation, pick a refresh type
bool EInkDynamicDisplay::determineMode() {
checkInitialized();
checkForPromotion();
bool EInkDynamicDisplay::determineMode()
{
checkInitialized();
checkForPromotion();
#if defined(HAS_EINK_ASYNCFULL)
checkBusyAsyncRefresh();
checkBusyAsyncRefresh();
#endif
checkRateLimiting();
checkRateLimiting();
// If too soon for a new frame, or display busy, abort early
if (refresh == SKIPPED)
return false; // No refresh
// If too soon for a new frame, or display busy, abort early
if (refresh == SKIPPED)
return false; // No refresh
// -- New frame is due --
// -- New frame is due --
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
LOG_DEBUG("determineMode(): "); // Begin log entry
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
LOG_DEBUG("determineMode(): "); // Begin log entry
// Once mode determined, any remaining checks will bypass
checkCosmetic();
checkDemandingFast();
checkFrameMatchesPrevious();
checkConsecutiveFastRefreshes();
// Once mode determined, any remaining checks will bypass
checkCosmetic();
checkDemandingFast();
checkFrameMatchesPrevious();
checkConsecutiveFastRefreshes();
#ifdef EINK_LIMIT_GHOSTING_PX
checkExcessiveGhosting();
checkExcessiveGhosting();
#endif
checkFastRequested();
checkFastRequested();
if (refresh == UNSPECIFIED)
LOG_WARN("There was a flaw in the determineMode() logic");
if (refresh == UNSPECIFIED)
LOG_WARN("There was a flaw in the determineMode() logic");
// -- Decision has been reached --
applyRefreshMode();
adjustRefreshCounters();
// -- Decision has been reached --
applyRefreshMode();
adjustRefreshCounters();
#ifdef EINK_LIMIT_GHOSTING_PX
// Full refresh clears any ghosting
if (refresh == FULL)
resetGhostPixelTracking();
// Full refresh clears any ghosting
if (refresh == FULL)
resetGhostPixelTracking();
#endif
// Return - call a refresh or not?
if (refresh == SKIPPED)
return false; // Don't trigger a refresh
else
return true; // Do trigger a refresh
// Return - call a refresh or not?
if (refresh == SKIPPED)
return false; // Don't trigger a refresh
else
return true; // Do trigger a refresh
}
// Is this the very first frame?
void EInkDynamicDisplay::checkInitialized() {
if (!initialized) {
// Undo GxEPD2_BW::partialWindow(), if set by developer in EInkDisplay::connect()
configForFullRefresh();
void EInkDynamicDisplay::checkInitialized()
{
if (!initialized) {
// Undo GxEPD2_BW::partialWindow(), if set by developer in EInkDisplay::connect()
configForFullRefresh();
// Clear any existing image, so we can draw logo with fast-refresh, but also to set GxEPD2_EPD::_initial_write
adafruitDisplay->clearScreen();
// Clear any existing image, so we can draw logo with fast-refresh, but also to set GxEPD2_EPD::_initial_write
adafruitDisplay->clearScreen();
LOG_DEBUG("initialized, ");
initialized = true;
LOG_DEBUG("initialized, ");
initialized = true;
// Use a fast-refresh for the next frame; no skipping or else blank screen when waking from deep sleep
addFrameFlag(DEMAND_FAST);
}
// Use a fast-refresh for the next frame; no skipping or else blank screen when waking from deep sleep
addFrameFlag(DEMAND_FAST);
}
}
// Was a frame skipped (rate, display busy) that should have been a FAST refresh?
void EInkDynamicDisplay::checkForPromotion() {
// 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
void EInkDynamicDisplay::checkForPromotion()
{
// 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
switch (previousReason) {
case ASYNC_REFRESH_BLOCKED_DEMANDFAST:
addFrameFlag(DEMAND_FAST);
break;
case ASYNC_REFRESH_BLOCKED_COSMETIC:
addFrameFlag(COSMETIC);
break;
case ASYNC_REFRESH_BLOCKED_RESPONSIVE:
case EXCEEDED_RATELIMIT_FAST:
addFrameFlag(RESPONSIVE);
break;
default:
break;
}
switch (previousReason) {
case ASYNC_REFRESH_BLOCKED_DEMANDFAST:
addFrameFlag(DEMAND_FAST);
break;
case ASYNC_REFRESH_BLOCKED_COSMETIC:
addFrameFlag(COSMETIC);
break;
case ASYNC_REFRESH_BLOCKED_RESPONSIVE:
case EXCEEDED_RATELIMIT_FAST:
addFrameFlag(RESPONSIVE);
break;
default:
break;
}
}
// Is it too soon for another frame of this type?
void EInkDynamicDisplay::checkRateLimiting() {
// Sanity check: millis() overflow - just let the update run..
if (previousRunMs > millis())
return;
void EInkDynamicDisplay::checkRateLimiting()
{
// Sanity check: millis() overflow - just let the update run..
if (previousRunMs > millis())
return;
// Skip update: too soon for BACKGROUND
if (frameFlags == BACKGROUND) {
if (Throttle::isWithinTimespanMs(previousRunMs, 30000)) {
refresh = SKIPPED;
reason = EXCEEDED_RATELIMIT_FULL;
return;
// Skip update: too soon for BACKGROUND
if (frameFlags == BACKGROUND) {
if (Throttle::isWithinTimespanMs(previousRunMs, 30000)) {
refresh = SKIPPED;
reason = EXCEEDED_RATELIMIT_FULL;
return;
}
}
}
// No rate-limit for these special cases
if (frameFlags & COSMETIC || frameFlags & DEMAND_FAST)
return;
// No rate-limit for these special cases
if (frameFlags & COSMETIC || frameFlags & DEMAND_FAST)
return;
// Skip update: too soon for RESPONSIVE
if (frameFlags & RESPONSIVE) {
if (Throttle::isWithinTimespanMs(previousRunMs, 1000)) {
refresh = SKIPPED;
reason = EXCEEDED_RATELIMIT_FAST;
LOG_DEBUG("refresh=SKIPPED, reason=EXCEEDED_RATELIMIT_FAST, frameFlags=0x%x", frameFlags);
return;
// Skip update: too soon for RESPONSIVE
if (frameFlags & RESPONSIVE) {
if (Throttle::isWithinTimespanMs(previousRunMs, 1000)) {
refresh = SKIPPED;
reason = EXCEEDED_RATELIMIT_FAST;
LOG_DEBUG("refresh=SKIPPED, reason=EXCEEDED_RATELIMIT_FAST, frameFlags=0x%x", frameFlags);
return;
}
}
}
}
// Is this frame COSMETIC (splash screens?)
void EInkDynamicDisplay::checkCosmetic() {
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::checkCosmetic()
{
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
// A full refresh is requested for cosmetic purposes: we have a decision
if (frameFlags & COSMETIC) {
refresh = FULL;
reason = FLAGGED_COSMETIC;
LOG_DEBUG("refresh=FULL, reason=FLAGGED_COSMETIC, frameFlags=0x%x", frameFlags);
}
// A full refresh is requested for cosmetic purposes: we have a decision
if (frameFlags & COSMETIC) {
refresh = FULL;
reason = FLAGGED_COSMETIC;
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?
void EInkDynamicDisplay::checkDemandingFast() {
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::checkDemandingFast()
{
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
// A fast refresh is demanded: we have a decision
if (frameFlags & DEMAND_FAST) {
refresh = FAST;
reason = FLAGGED_DEMAND_FAST;
LOG_DEBUG("refresh=FAST, reason=FLAGGED_DEMAND_FAST, frameFlags=0x%x", frameFlags);
}
// A fast refresh is demanded: we have a decision
if (frameFlags & DEMAND_FAST) {
refresh = FAST;
reason = FLAGGED_DEMAND_FAST;
LOG_DEBUG("refresh=FAST, reason=FLAGGED_DEMAND_FAST, frameFlags=0x%x", frameFlags);
}
}
// Does the new frame match the currently displayed image?
void EInkDynamicDisplay::checkFrameMatchesPrevious() {
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::checkFrameMatchesPrevious()
{
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
// If frame is *not* a duplicate, abort the check
if (imageHash != previousImageHash)
return;
// If frame is *not* a duplicate, abort the check
if (imageHash != previousImageHash)
return;
#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 (frameFlags == BACKGROUND && fastRefreshCount > 0) {
refresh = FULL;
reason = REDRAW_WITH_FULL;
LOG_DEBUG("refresh=FULL, reason=REDRAW_WITH_FULL, frameFlags=0x%x", frameFlags);
return;
}
// If BACKGROUND, and last update was FAST: redraw the same image in FULL (for display health + image quality)
if (frameFlags == BACKGROUND && fastRefreshCount > 0) {
refresh = FULL;
reason = REDRAW_WITH_FULL;
LOG_DEBUG("refresh=FULL, reason=REDRAW_WITH_FULL, frameFlags=0x%x", frameFlags);
return;
}
#endif
// Not redrawn, not COSMETIC, not DEMAND_FAST
refresh = SKIPPED;
reason = FRAME_MATCHED_PREVIOUS;
LOG_DEBUG("refresh=SKIPPED, reason=FRAME_MATCHED_PREVIOUS, frameFlags=0x%x", frameFlags);
// Not redrawn, not COSMETIC, not DEMAND_FAST
refresh = SKIPPED;
reason = FRAME_MATCHED_PREVIOUS;
LOG_DEBUG("refresh=SKIPPED, reason=FRAME_MATCHED_PREVIOUS, frameFlags=0x%x", frameFlags);
}
// Have too many fast-refreshes occured consecutively, since last full refresh?
void EInkDynamicDisplay::checkConsecutiveFastRefreshes() {
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::checkConsecutiveFastRefreshes()
{
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
// Bypass limit if UNLIMITED_FAST mode is active
if (frameFlags & UNLIMITED_FAST) {
refresh = FAST;
reason = NO_OBJECTIONS;
LOG_DEBUG("refresh=FAST, reason=UNLIMITED_FAST_MODE_ACTIVE, frameFlags=0x%x", frameFlags);
return;
}
// Bypass limit if UNLIMITED_FAST mode is active
if (frameFlags & UNLIMITED_FAST) {
refresh = FAST;
reason = NO_OBJECTIONS;
LOG_DEBUG("refresh=FAST, reason=UNLIMITED_FAST_MODE_ACTIVE, frameFlags=0x%x", frameFlags);
return;
}
// If too many FAST refreshes consecutively - force a FULL refresh
if (fastRefreshCount >= EINK_LIMIT_FASTREFRESH) {
refresh = FULL;
reason = EXCEEDED_LIMIT_FASTREFRESH;
LOG_DEBUG("refresh=FULL, reason=EXCEEDED_LIMIT_FASTREFRESH, frameFlags=0x%x", frameFlags);
}
// If too many FAST refreshes consecutively - force a FULL refresh
if (fastRefreshCount >= EINK_LIMIT_FASTREFRESH) {
refresh = FULL;
reason = EXCEEDED_LIMIT_FASTREFRESH;
LOG_DEBUG("refresh=FULL, reason=EXCEEDED_LIMIT_FASTREFRESH, frameFlags=0x%x", frameFlags);
}
}
// No objections, we can perform fast-refresh, if desired
void EInkDynamicDisplay::checkFastRequested() {
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::checkFastRequested()
{
if (refresh != UNSPECIFIED)
return;
if (frameFlags == BACKGROUND) {
if (frameFlags == BACKGROUND) {
#ifdef EINK_BACKGROUND_USES_FAST
// If we want BACKGROUND to use fast. (FULL only when a limit is hit)
refresh = FAST;
reason = BACKGROUND_USES_FAST;
LOG_DEBUG("refresh=FAST, reason=BACKGROUND_USES_FAST, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, frameFlags);
// If we want BACKGROUND to use fast. (FULL only when a limit is hit)
refresh = FAST;
reason = BACKGROUND_USES_FAST;
LOG_DEBUG("refresh=FAST, reason=BACKGROUND_USES_FAST, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount,
frameFlags);
#else
// If we do want to use FULL for BACKGROUND updates
refresh = FULL;
reason = FLAGGED_BACKGROUND;
LOG_DEBUG("refresh=FULL, reason=FLAGGED_BACKGROUND");
// If we do want to use FULL for BACKGROUND updates
refresh = FULL;
reason = FLAGGED_BACKGROUND;
LOG_DEBUG("refresh=FULL, reason=FLAGGED_BACKGROUND");
#endif
}
}
// Sanity: confirm that we did ask for a RESPONSIVE frame.
if (frameFlags & RESPONSIVE) {
refresh = FAST;
reason = NO_OBJECTIONS;
LOG_DEBUG("refresh=FAST, reason=NO_OBJECTIONS, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, frameFlags);
}
// Sanity: confirm that we did ask for a RESPONSIVE frame.
if (frameFlags & RESPONSIVE) {
refresh = FAST;
reason = NO_OBJECTIONS;
LOG_DEBUG("refresh=FAST, reason=NO_OBJECTIONS, fastRefreshCount=%lu, frameFlags=0x%x", fastRefreshCount, frameFlags);
}
}
// Reset the timer used for rate-limiting
void EInkDynamicDisplay::resetRateLimiting() { previousRunMs = millis(); }
void EInkDynamicDisplay::resetRateLimiting()
{
previousRunMs = millis();
}
// Generate a hash of this frame, to compare against previous update
void EInkDynamicDisplay::hashImage() {
imageHash = 0;
void EInkDynamicDisplay::hashImage()
{
imageHash = 0;
// Sum all bytes of the image buffer together
for (uint16_t b = 0; b < (displayWidth / 8) * displayHeight; b++) {
imageHash ^= buffer[b] << b;
}
// Sum all bytes of the image buffer together
for (uint16_t b = 0; b < (displayWidth / 8) * displayHeight; b++) {
imageHash ^= buffer[b] << b;
}
}
// Store the results of determineMode() for future use, and reset for next call
void EInkDynamicDisplay::storeAndReset() {
previousFrameFlags = frameFlags;
previousRefresh = refresh;
previousReason = reason;
void EInkDynamicDisplay::storeAndReset()
{
previousFrameFlags = frameFlags;
previousRefresh = refresh;
previousReason = reason;
// Only store image hash if the display will update
if (refresh != SKIPPED) {
previousImageHash = imageHash;
}
// Only store image hash if the display will update
if (refresh != SKIPPED) {
previousImageHash = imageHash;
}
frameFlags = BACKGROUND;
refresh = UNSPECIFIED;
frameFlags = BACKGROUND;
refresh = UNSPECIFIED;
}
#ifdef EINK_LIMIT_GHOSTING_PX
// Count how many ghost pixels the new image will display
void EInkDynamicDisplay::countGhostPixels() {
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::countGhostPixels()
{
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
// Start a new count
ghostPixelCount = 0;
// Start a new count
ghostPixelCount = 0;
// Check new image, bit by bit, for any white pixels at locations marked "dirty"
for (uint16_t i = 0; i < displayBufferSize; i++) {
for (uint8_t bit = 0; bit < 7; bit++) {
// Check new image, bit by bit, for any white pixels at locations marked "dirty"
for (uint16_t i = 0; i < displayBufferSize; i++) {
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 shouldBeBlank = !((buffer[i] >> bit) & 1); // Is pixel location white in the new image?
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?
// If pixel is (or has been) black since last full-refresh, and now is white: ghosting
if (dirty && shouldBeBlank)
ghostPixelCount++;
// If pixel is (or has been) black since last full-refresh, and now is white: ghosting
if (dirty && shouldBeBlank)
ghostPixelCount++;
// Update the dirty status for this pixel - will this location become a ghost if set white in future?
if (!dirty && !shouldBeBlank)
dirtyPixels[i] |= (1 << bit);
// Update the dirty status for this pixel - will this location become a ghost if set white in future?
if (!dirty && !shouldBeBlank)
dirtyPixels[i] |= (1 << bit);
}
}
}
LOG_DEBUG("ghostPixels=%hu, ", ghostPixelCount);
LOG_DEBUG("ghostPixels=%hu, ", ghostPixelCount);
}
// Check if ghost pixel count exceeds the defined limit
void EInkDynamicDisplay::checkExcessiveGhosting() {
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
void EInkDynamicDisplay::checkExcessiveGhosting()
{
// If a decision was already reached, don't run the check
if (refresh != UNSPECIFIED)
return;
countGhostPixels();
countGhostPixels();
// If too many ghost pixels, select full refresh
if (ghostPixelCount > EINK_LIMIT_GHOSTING_PX) {
refresh = FULL;
reason = EXCEEDED_GHOSTINGLIMIT;
LOG_DEBUG("refresh=FULL, reason=EXCEEDED_GHOSTINGLIMIT, frameFlags=0x%x", frameFlags);
}
// If too many ghost pixels, select full refresh
if (ghostPixelCount > EINK_LIMIT_GHOSTING_PX) {
refresh = FULL;
reason = EXCEEDED_GHOSTINGLIMIT;
LOG_DEBUG("refresh=FULL, reason=EXCEEDED_GHOSTINGLIMIT, frameFlags=0x%x", frameFlags);
}
}
// Clear the dirty pixels array. Call when full-refresh cleans the display.
void EInkDynamicDisplay::resetGhostPixelTracking() {
// Copy the current frame into dirtyPixels[] from the display buffer
memcpy(dirtyPixels, EInkDisplay::buffer, EInkDisplay::displayBufferSize);
void EInkDynamicDisplay::resetGhostPixelTracking()
{
// Copy the current frame into dirtyPixels[] from the display buffer
memcpy(dirtyPixels, EInkDisplay::buffer, EInkDisplay::displayBufferSize);
}
#endif // EINK_LIMIT_GHOSTING_PX
// Handle any asyc tasks
void EInkDynamicDisplay::onNotify(uint32_t notification) {
// Which task
switch (notification) {
case DUE_POLL_ASYNCREFRESH:
pollAsyncRefresh();
break;
}
void EInkDynamicDisplay::onNotify(uint32_t notification)
{
// Which task
switch (notification) {
case DUE_POLL_ASYNCREFRESH:
pollAsyncRefresh();
break;
}
}
#ifdef HAS_EINK_ASYNCFULL
// Public: wait for an refresh already in progress, then run the post-update code. See Screen::setScreensaverFrames()
void EInkDynamicDisplay::joinAsyncRefresh() {
// If no async refresh running, nothing to do
if (!asyncRefreshRunning)
return;
void EInkDynamicDisplay::joinAsyncRefresh()
{
// If no async refresh running, nothing to do
if (!asyncRefreshRunning)
return;
LOG_DEBUG("Join an async refresh in progress");
LOG_DEBUG("Join an async refresh in progress");
// Continually poll the BUSY pin
while (adafruitDisplay->epd2.isBusy())
yield();
// Continually poll the BUSY pin
while (adafruitDisplay->epd2.isBusy())
yield();
// If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag
LOG_DEBUG("Refresh complete");
// If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag
LOG_DEBUG("Refresh complete");
// Note: this code only works because of a modification to meshtastic/GxEPD2.
// It is only equipped to intercept calls to nextPage()
// Note: this code only works because of a modification to meshtastic/GxEPD2.
// It is only equipped to intercept calls to nextPage()
}
// Called from NotifiedWorkerThread. Run the post-update code if the hardware is ready
void EInkDynamicDisplay::pollAsyncRefresh() {
// In theory, this condition should never be met
if (!asyncRefreshRunning)
return;
void EInkDynamicDisplay::pollAsyncRefresh()
{
// In theory, this condition should never be met
if (!asyncRefreshRunning)
return;
// Still running, check back later
if (adafruitDisplay->epd2.isBusy()) {
// Schedule next call of pollAsyncRefresh()
NotifiedWorkerThread::notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true);
return;
}
// Still running, check back later
if (adafruitDisplay->epd2.isBusy()) {
// Schedule next call of pollAsyncRefresh()
NotifiedWorkerThread::notifyLater(intervalPollAsyncRefresh, DUE_POLL_ASYNCREFRESH, true);
return;
}
// If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag
LOG_DEBUG("Async full-refresh complete");
// If asyncRefreshRunning flag is still set, but display's BUSY pin reports the refresh is done
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag
LOG_DEBUG("Async full-refresh complete");
// Note: this code only works because of a modification to meshtastic/GxEPD2.
// It is only equipped to intercept calls to nextPage()
// Note: this code only works because of a modification to meshtastic/GxEPD2.
// It is only equipped to intercept calls to nextPage()
}
// Check the status of "async full-refresh"; skip if running
void EInkDynamicDisplay::checkBusyAsyncRefresh() {
// No refresh taking place, continue with determineMode()
if (!asyncRefreshRunning)
return;
void EInkDynamicDisplay::checkBusyAsyncRefresh()
{
// No refresh taking place, continue with determineMode()
if (!asyncRefreshRunning)
return;
// Full refresh still running
if (adafruitDisplay->epd2.isBusy()) {
// No refresh
refresh = SKIPPED;
// Full refresh still running
if (adafruitDisplay->epd2.isBusy()) {
// No refresh
refresh = SKIPPED;
// Set the reason, marking what type of frame we're skipping
if (frameFlags & DEMAND_FAST)
reason = ASYNC_REFRESH_BLOCKED_DEMANDFAST;
else if (frameFlags & COSMETIC)
reason = ASYNC_REFRESH_BLOCKED_COSMETIC;
else if (frameFlags & RESPONSIVE)
reason = ASYNC_REFRESH_BLOCKED_RESPONSIVE;
// Set the reason, marking what type of frame we're skipping
if (frameFlags & DEMAND_FAST)
reason = ASYNC_REFRESH_BLOCKED_DEMANDFAST;
else if (frameFlags & COSMETIC)
reason = ASYNC_REFRESH_BLOCKED_COSMETIC;
else if (frameFlags & 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
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
pollAsyncRefresh(); // Check (and terminate) the async refresh manually
}
// Hold control while an async refresh runs
void EInkDynamicDisplay::awaitRefresh() {
// Continually poll the BUSY pin
while (adafruitDisplay->epd2.isBusy())
yield();
void EInkDynamicDisplay::awaitRefresh()
{
// Continually poll the BUSY pin
while (adafruitDisplay->epd2.isBusy())
yield();
// End the full-refresh process
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag
// End the full-refresh process
adafruitDisplay->endAsyncFull(); // Run the end of nextPage() code
EInkDisplay::endUpdate(); // Run base-class code to finish off update (NOT our derived class override)
asyncRefreshRunning = false; // Unset the flag
}
#endif // HAS_EINK_ASYNCFULL
src/graphics/EInkDynamicDisplay.h
+106 -105
View File
@@ -15,130 +15,131 @@
(Full, Fast, Skip)
*/
class EInkDynamicDisplay : public EInkDisplay, protected concurrency::NotifiedWorkerThread {
public:
// Constructor
// ( Parameters unused, passed to EInkDisplay. Maintains compatibility OLEDDisplay class )
EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus);
~EInkDynamicDisplay();
class EInkDynamicDisplay : public EInkDisplay, protected concurrency::NotifiedWorkerThread
{
public:
// Constructor
// ( Parameters unused, passed to EInkDisplay. Maintains compatibility OLEDDisplay class )
EInkDynamicDisplay(uint8_t address, int sda, int scl, OLEDDISPLAY_GEOMETRY geometry, HW_I2C i2cBus);
~EInkDynamicDisplay();
// Methods to enable or disable unlimited fast refresh mode
void enableUnlimitedFastMode() { addFrameFlag(UNLIMITED_FAST); }
void disableUnlimitedFastMode() { frameFlags = (frameFlagTypes)(frameFlags & ~UNLIMITED_FAST); }
// Methods to enable or disable unlimited fast refresh mode
void enableUnlimitedFastMode() { addFrameFlag(UNLIMITED_FAST); }
void disableUnlimitedFastMode() { frameFlags = (frameFlagTypes)(frameFlags & ~UNLIMITED_FAST); }
// What kind of frame is this
enum frameFlagTypes : uint8_t {
BACKGROUND = (1 << 0), // For frames via display()
RESPONSIVE = (1 << 1), // For frames via forceDisplay()
COSMETIC = (1 << 2), // For splashes
DEMAND_FAST = (1 << 3), // Special case only
BLOCKING = (1 << 4), // Modifier - block while refresh runs
UNLIMITED_FAST = (1 << 5)
};
void addFrameFlag(frameFlagTypes flag);
// What kind of frame is this
enum frameFlagTypes : uint8_t {
BACKGROUND = (1 << 0), // For frames via display()
RESPONSIVE = (1 << 1), // For frames via forceDisplay()
COSMETIC = (1 << 2), // For splashes
DEMAND_FAST = (1 << 3), // Special case only
BLOCKING = (1 << 4), // Modifier - block while refresh runs
UNLIMITED_FAST = (1 << 5)
};
void addFrameFlag(frameFlagTypes flag);
// Set the correct frame flag, then call universal "update()" method
void display() override;
bool forceDisplay(uint32_t msecLimit) override; // Shadows base class. Parameter and return val unused.
// Set the correct frame flag, then call universal "update()" method
void display() override;
bool forceDisplay(uint32_t msecLimit) override; // Shadows base class. Parameter and return val unused.
protected:
enum refreshTypes : uint8_t { // Which refresh operation will be used
UNSPECIFIED,
FULL,
FAST,
SKIPPED,
};
enum reasonTypes : uint8_t { // How was the decision reached
NO_OBJECTIONS,
ASYNC_REFRESH_BLOCKED_DEMANDFAST,
ASYNC_REFRESH_BLOCKED_COSMETIC,
ASYNC_REFRESH_BLOCKED_RESPONSIVE,
ASYNC_REFRESH_BLOCKED_BACKGROUND,
EXCEEDED_RATELIMIT_FAST,
EXCEEDED_RATELIMIT_FULL,
FLAGGED_COSMETIC,
FLAGGED_DEMAND_FAST,
EXCEEDED_LIMIT_FASTREFRESH,
EXCEEDED_GHOSTINGLIMIT,
FRAME_MATCHED_PREVIOUS,
BACKGROUND_USES_FAST,
FLAGGED_BACKGROUND,
REDRAW_WITH_FULL,
};
protected:
enum refreshTypes : uint8_t { // Which refresh operation will be used
UNSPECIFIED,
FULL,
FAST,
SKIPPED,
};
enum reasonTypes : uint8_t { // How was the decision reached
NO_OBJECTIONS,
ASYNC_REFRESH_BLOCKED_DEMANDFAST,
ASYNC_REFRESH_BLOCKED_COSMETIC,
ASYNC_REFRESH_BLOCKED_RESPONSIVE,
ASYNC_REFRESH_BLOCKED_BACKGROUND,
EXCEEDED_RATELIMIT_FAST,
EXCEEDED_RATELIMIT_FULL,
FLAGGED_COSMETIC,
FLAGGED_DEMAND_FAST,
EXCEEDED_LIMIT_FASTREFRESH,
EXCEEDED_GHOSTINGLIMIT,
FRAME_MATCHED_PREVIOUS,
BACKGROUND_USES_FAST,
FLAGGED_BACKGROUND,
REDRAW_WITH_FULL,
};
enum notificationTypes : uint8_t { // What was onNotify() called for
NONE = 0, // This behavior (NONE=0) is fixed by NotifiedWorkerThread class
DUE_POLL_ASYNCREFRESH = 1,
};
const uint32_t intervalPollAsyncRefresh = 100;
enum notificationTypes : uint8_t { // What was onNotify() called for
NONE = 0, // This behavior (NONE=0) is fixed by NotifiedWorkerThread class
DUE_POLL_ASYNCREFRESH = 1,
};
const uint32_t intervalPollAsyncRefresh = 100;
void onNotify(uint32_t notification) override; // Handle any async tasks - overrides NotifiedWorkerThread
void configForFastRefresh(); // GxEPD2 code to set fast-refresh
void configForFullRefresh(); // GxEPD2 code to set full-refresh
bool determineMode(); // Assess situation, pick a refresh type
void applyRefreshMode(); // Run any relevant GxEPD2 code, so next update will use correct refresh type
void adjustRefreshCounters(); // Update fastRefreshCount
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 onNotify(uint32_t notification) override; // Handle any async tasks - overrides NotifiedWorkerThread
void configForFastRefresh(); // GxEPD2 code to set fast-refresh
void configForFullRefresh(); // GxEPD2 code to set full-refresh
bool determineMode(); // Assess situation, pick a refresh type
void applyRefreshMode(); // Run any relevant GxEPD2 code, so next update will use correct refresh type
void adjustRefreshCounters(); // Update fastRefreshCount
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()
// Checks as part of determineMode()
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 checkRateLimiting(); // Is this frame too soon?
void checkCosmetic(); // Was the COSMETIC flag set?
void checkDemandingFast(); // Was the DEMAND_FAST flag set?
void checkFrameMatchesPrevious(); // Does the new frame match the existing display image?
void checkConsecutiveFastRefreshes(); // Too many fast-refreshes consecutively?
void checkFastRequested(); // Was the flag set for RESPONSIVE, or only BACKGROUND?
// Checks as part of determineMode()
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 checkRateLimiting(); // Is this frame too soon?
void checkCosmetic(); // Was the COSMETIC flag set?
void checkDemandingFast(); // Was the DEMAND_FAST flag set?
void checkFrameMatchesPrevious(); // Does the new frame match the existing display image?
void checkConsecutiveFastRefreshes(); // Too many fast-refreshes consecutively?
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 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 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 storeAndReset(); // Keep results of determineMode() for later, tidy-up for next call
// What we are determining for this frame
frameFlagTypes frameFlags = BACKGROUND; // Frame characteristics - determineMode() input
refreshTypes refresh = UNSPECIFIED; // Refresh type - determineMode() output
reasonTypes reason = NO_OBJECTIONS; // Reason - why was refresh type used
// What we are determining for this frame
frameFlagTypes frameFlags = BACKGROUND; // Frame characteristics - determineMode() input
refreshTypes refresh = UNSPECIFIED; // Refresh type - determineMode() output
reasonTypes reason = NO_OBJECTIONS; // Reason - why was refresh type used
// What happened last time determineMode() ran
frameFlagTypes previousFrameFlags = BACKGROUND; // (Previous) Frame flags
refreshTypes previousRefresh = UNSPECIFIED; // (Previous) Outcome
reasonTypes previousReason = NO_OBJECTIONS; // (Previous) Reason
// What happened last time determineMode() ran
frameFlagTypes previousFrameFlags = BACKGROUND; // (Previous) Frame flags
refreshTypes previousRefresh = UNSPECIFIED; // (Previous) Outcome
reasonTypes previousReason = NO_OBJECTIONS; // (Previous) Reason
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 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 fastRefreshCount = 0; // How many fast-refreshes consecutively since last full refresh?
refreshTypes currentConfig = FULL; // Which refresh type is GxEPD2 currently configured for
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 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 fastRefreshCount = 0; // How many fast-refreshes consecutively since last full refresh?
refreshTypes currentConfig = FULL; // Which refresh type is GxEPD2 currently configured for
// Optional - track ghosting, pixel by pixel
// May 2024: no longer used by any display. Kept for possible future use.
// Optional - track ghosting, pixel by pixel
// May 2024: no longer used by any display. Kept for possible future use.
#ifdef EINK_LIMIT_GHOSTING_PX
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 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)
uint32_t ghostPixelCount = 0; // Number of pixels with problematic ghosting. Retained here for LOG_DEBUG use
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 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)
uint32_t ghostPixelCount = 0; // Number of pixels with problematic ghosting. Retained here for LOG_DEBUG use
#endif
// Conditional - async full refresh - only with modified meshtastic/GxEPD2
// Conditional - async full refresh - only with modified meshtastic/GxEPD2
#if defined(HAS_EINK_ASYNCFULL)
public:
void joinAsyncRefresh(); // Main thread joins an async refresh already in progress. Blocks, then runs post-update code
public:
void joinAsyncRefresh(); // Main thread joins an async refresh already in progress. Blocks, then runs post-update code
protected:
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 awaitRefresh(); // Hold control while an async refresh runs
void endUpdate() override {} // Disable base-class behavior of running post-update immediately after forceDisplay()
bool asyncRefreshRunning = false; // Flag, checked by checkBusyAsyncRefresh()
protected:
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 awaitRefresh(); // Hold control while an async refresh runs
void endUpdate() override {} // Disable base-class behavior of running post-update immediately after forceDisplay()
bool asyncRefreshRunning = false; // Flag, checked by checkBusyAsyncRefresh()
#else
public:
void joinAsyncRefresh() {} // Dummy method
public:
void joinAsyncRefresh() {} // Dummy method
protected:
void pollAsyncRefresh() {} // Dummy method. In theory, not reachable
protected:
void pollAsyncRefresh() {} // Dummy method. In theory, not reachable
#endif
};
src/graphics/GxEPD2Multi.h
+125 -110
View File
@@ -4,117 +4,132 @@
// Workaround for issue of GxEPD2_BW objects not having a shared base class
// Only exposes methods which we are actually using
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 {
template <typename Driver0, typename Driver1> class GxEPD2_Multi
{
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);
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);
}
}
private:
uint8_t which;
GxEPD2_BW<Driver0, Driver0::HEIGHT> *driver0;
GxEPD2_BW<Driver1, Driver1::HEIGHT> *driver1;
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:
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
+581 -535
View File
File diff suppressed because it is too large Load Diff
src/graphics/Panel_sdl.hpp
+95 -92
View File
@@ -36,126 +36,129 @@ Porting for SDL:
#include "lgfx/v1/panel/Panel_FrameBufferBase.hpp"
#include <cstdint>
namespace lgfx {
inline namespace v1 {
namespace lgfx
{
inline namespace v1
{
struct Panel_sdl;
struct monitor_t {
SDL_Window *window = nullptr;
SDL_Renderer *renderer = nullptr;
SDL_Texture *texture = nullptr;
SDL_Texture *texture_frameimage = nullptr;
Panel_sdl *panel = nullptr;
SDL_Window *window = nullptr;
SDL_Renderer *renderer = nullptr;
SDL_Texture *texture = nullptr;
SDL_Texture *texture_frameimage = nullptr;
Panel_sdl *panel = nullptr;
// 外枠
const void *frame_image = 0;
uint_fast16_t frame_width = 0;
uint_fast16_t frame_height = 0;
uint_fast16_t frame_inner_x = 0;
uint_fast16_t frame_inner_y = 0;
int_fast16_t frame_rotation = 0;
int_fast16_t frame_angle = 0;
// 外枠
const void *frame_image = 0;
uint_fast16_t frame_width = 0;
uint_fast16_t frame_height = 0;
uint_fast16_t frame_inner_x = 0;
uint_fast16_t frame_inner_y = 0;
int_fast16_t frame_rotation = 0;
int_fast16_t frame_angle = 0;
float scaling_x = 1;
float scaling_y = 1;
int_fast16_t touch_x, touch_y;
bool touched = false;
bool closing = false;
float scaling_x = 1;
float scaling_y = 1;
int_fast16_t touch_x, touch_y;
bool touched = false;
bool closing = false;
};
//----------------------------------------------------------------------------
struct Touch_sdl : public ITouch {
bool init(void) override { return true; }
void wakeup(void) override {}
void sleep(void) override {}
bool isEnable(void) override { return true; };
uint_fast8_t getTouchRaw(touch_point_t *tp, uint_fast8_t count) override { return 0; }
bool init(void) override { return true; }
void wakeup(void) override {}
void sleep(void) override {}
bool isEnable(void) override { return true; };
uint_fast8_t getTouchRaw(touch_point_t *tp, uint_fast8_t count) override { return 0; }
};
//----------------------------------------------------------------------------
struct Panel_sdl : public Panel_FrameBufferBase {
static constexpr size_t EMULATED_GPIO_MAX = 128;
static volatile uint8_t _gpio_dummy_values[EMULATED_GPIO_MAX];
static constexpr size_t EMULATED_GPIO_MAX = 128;
static volatile uint8_t _gpio_dummy_values[EMULATED_GPIO_MAX];
public:
Panel_sdl(void);
virtual ~Panel_sdl(void);
public:
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 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 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, 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 writePixels(pixelcopy_t *param, uint32_t len, bool use_dma) override;
// void setInvert(bool invert) 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 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,
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 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 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 setFrameRotation(uint_fast16_t frame_rotaion);
void setBrightness(uint8_t brightness) override{};
void setWindowTitle(const char *title);
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 setFrameRotation(uint_fast16_t frame_rotaion);
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_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 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 bool gpio_in(uint32_t pin) { return _gpio_dummy_values[pin & (EMULATED_GPIO_MAX - 1)]; }
static int setup(void);
static int loop(void);
static int close(void);
static int setup(void);
static int loop(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 {
SDL_KeyCode keycode = SDLK_UNKNOWN;
uint8_t gpio = 0;
};
static void addKeyCodeMapping(SDL_KeyCode keyCode, uint8_t gpio);
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();
struct KeyCodeMapping_t {
SDL_KeyCode keycode = SDLK_UNKNOWN;
uint8_t gpio = 0;
};
static void addKeyCodeMapping(SDL_KeyCode keyCode, uint8_t gpio);
static int getKeyCodeMapping(SDL_KeyCode keyCode);
protected:
Panel_sdl *_parent;
};
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:
Panel_sdl *_parent;
};
};
//----------------------------------------------------------------------------
} // namespace v1
src/graphics/PointStruct.h
+2 -2
View File
@@ -1,4 +1,4 @@
struct PointStruct {
int x;
int y;
int x;
int y;
};
src/graphics/Screen.cpp
+1281 -1220
View File
File diff suppressed because it is too large Load Diff
src/graphics/Screen.h
+526 -504
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
#if (defined(USE_EINK) || defined(ILI9341_DRIVER) || defined(ILI9342_DRIVER) || defined(ST7701_CS) || defined(ST7735_CS) || defined(ST7789_CS) || \
defined(USE_ST7789) || defined(HX8357_CS) || defined(ILI9488_CS) || defined(ST7796_CS) || defined(HACKADAY_COMMUNICATOR) || \
defined(USE_ST7796)) && \
#if (defined(USE_EINK) || defined(ILI9341_DRIVER) || defined(ILI9342_DRIVER) || defined(ST7701_CS) || defined(ST7735_CS) || \
defined(ST7789_CS) || defined(USE_ST7789) || defined(HX8357_CS) || defined(ILI9488_CS) || defined(ST7796_CS) || \
defined(HACKADAY_COMMUNICATOR) || defined(USE_ST7796)) && \
!defined(DISPLAY_FORCE_SMALL_FONTS)
// The screen is bigger so use bigger fonts
#define FONT_SMALL FONT_MEDIUM_LOCAL // Height: 19
src/graphics/SharedUIDisplay.cpp
+419 -407
View File
@@ -13,43 +13,46 @@
#include <OLEDDisplay.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
return ScreenResolution::Low;
#else
// Unit C6L and other ultra low res screens
if (screenwidth <= 64 || screenheight <= 48) {
return ScreenResolution::UltraLow;
}
// Standard OLED screens
if (screenwidth > 128 && screenheight <= 64) {
return ScreenResolution::Low;
}
#else
// Unit C6L and other ultra low res screens
if (screenwidth <= 64 || screenheight <= 48) {
return ScreenResolution::UltraLow;
}
// High Resolutions screens like T114, TDeck, TLora Pager, etc
if (screenwidth > 128) {
return ScreenResolution::High;
}
// Standard OLED screens
if (screenwidth > 128 && screenheight <= 64) {
return ScreenResolution::Low;
}
// Default to low resolution
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
}
void decomposeTime(uint32_t rtc_sec, int &hour, int &minute, int &second) {
hour = 0;
minute = 0;
second = 0;
if (rtc_sec == 0)
return;
uint32_t hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY;
hour = hms / SEC_PER_HOUR;
minute = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
second = hms % SEC_PER_MIN;
void decomposeTime(uint32_t rtc_sec, int &hour, int &minute, int &second)
{
hour = 0;
minute = 0;
second = 0;
if (rtc_sec == 0)
return;
uint32_t hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY;
hour = hms / SEC_PER_HOUR;
minute = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
second = hms % SEC_PER_MIN;
}
// === Shared External State ===
@@ -65,448 +68,457 @@ uint32_t lastMailBlink = 0;
// *********************************
// * Rounded Header when inverted *
// *********************************
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
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 + w - r, y + r, r, h - 2 * r); // right edge
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
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 + w - r, y + r, r, h - 2 * r); // right edge
// Draw the rounded corners using filled circles
display->fillCircle(x + r + 1, y + r, r); // top-left
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 + w - r - 1, y + h - r - 1, r); // bottom-right
// Draw the rounded corners using filled circles
display->fillCircle(x + r + 1, y + r, r); // top-left
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 + w - r - 1, y + h - r - 1, r); // bottom-right
}
// *************************
// * Common Header Drawing *
// *************************
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;
y += HEADER_OFFSET_Y;
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;
y += HEADER_OFFSET_Y;
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_LEFT);
display->setFont(FONT_SMALL);
display->setTextAlignment(TEXT_ALIGN_LEFT);
const int xOffset = 4;
const int highlightHeight = FONT_HEIGHT_SMALL - 1;
const bool isInverted = (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_INVERTED);
const bool isBold = config.display.heading_bold;
const int xOffset = 4;
const int highlightHeight = FONT_HEIGHT_SMALL - 1;
const bool isInverted = (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_INVERTED);
const bool isBold = config.display.heading_bold;
const int screenW = display->getWidth();
const int screenH = display->getHeight();
const int screenW = display->getWidth();
const int screenH = display->getHeight();
if (!force_no_invert) {
// === Inverted Header Background ===
if (isInverted) {
display->setColor(BLACK);
display->fillRect(0, 0, screenW, highlightHeight + 2);
display->setColor(WHITE);
drawRoundedHighlight(display, x, y, screenW, highlightHeight, 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(0, 0, screenW, highlightHeight + 2);
display->setColor(WHITE);
if (currentResolution == ScreenResolution::High) {
display->drawLine(0, 20, screenW, 20);
} else {
display->drawLine(0, 14, screenW, 14);
}
if (!force_no_invert) {
// === Inverted Header Background ===
if (isInverted) {
display->setColor(BLACK);
display->fillRect(0, 0, screenW, highlightHeight + 2);
display->setColor(WHITE);
drawRoundedHighlight(display, x, y, screenW, highlightHeight, 2);
display->setColor(BLACK);
} else {
display->setColor(BLACK);
display->fillRect(0, 0, screenW, highlightHeight + 2);
display->setColor(WHITE);
if (currentResolution == ScreenResolution::High) {
display->drawLine(0, 20, screenW, 20);
} else {
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
}
// === 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();
uint32_t now = millis();
#ifndef USE_EINK
if (isCharging && now - lastBlinkShared > 500) {
isBoltVisibleShared = !isBoltVisibleShared;
lastBlinkShared = now;
}
if (isCharging && now - lastBlinkShared > 500) {
isBoltVisibleShared = !isBoltVisibleShared;
lastBlinkShared = now;
}
#endif
bool useHorizontalBattery = (currentResolution == ScreenResolution::High && screenW >= screenH);
const int textY = y + (highlightHeight - FONT_HEIGHT_SMALL) / 2;
bool useHorizontalBattery = (currentResolution == ScreenResolution::High && screenW >= screenH);
const int textY = y + (highlightHeight - FONT_HEIGHT_SMALL) / 2;
int batteryX = 1;
int batteryY = HEADER_OFFSET_Y + 1;
int batteryX = 1;
int batteryY = HEADER_OFFSET_Y + 1;
#if !defined(M5STACK_UNITC6L)
// === Battery Icons ===
if (usbPowered && !isCharging) { // This is a basic check to determine USB Powered is flagged but not charging
batteryX += 1;
batteryY += 2;
if (currentResolution == ScreenResolution::High) {
display->drawXbm(batteryX, batteryY, 19, 12, imgUSB_HighResolution);
batteryX += 20; // Icon + 1 pixel
} else {
display->drawXbm(batteryX, batteryY, 10, 8, imgUSB);
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
// === Battery Icons ===
if (usbPowered && !isCharging) { // This is a basic check to determine USB Powered is flagged but not charging
batteryX += 1;
batteryY += 2;
if (currentResolution == ScreenResolution::High) {
display->drawXbm(batteryX, batteryY, 19, 12, imgUSB_HighResolution);
batteryX += 20; // Icon + 1 pixel
} else {
display->drawXbm(batteryX, batteryY, 10, 8, imgUSB);
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 {
#ifdef USE_EINK
batteryY += 2;
batteryY += 2;
#endif
display->drawXbm(batteryX, batteryY, 7, 11, batteryBitmap_v);
if (isCharging && isBoltVisibleShared)
display->drawXbm(batteryX + 1, batteryY + 3, 5, 5, lightning_bolt_v);
else {
display->drawXbm(batteryX - 1, batteryY + 4, 8, 3, batteryBitmap_sidegaps_v);
int fillHeight = 8 * chargePercent / 100;
int fillY = batteryY - fillHeight;
display->fillRect(batteryX + 1, fillY + 10, 5, fillHeight);
}
batteryX += 9; // Icon + 2 pixels
}
}
if (chargePercent != 101) {
// === Battery % Display ===
char chargeStr[4];
snprintf(chargeStr, sizeof(chargeStr), "%d", chargePercent);
int chargeNumWidth = display->getStringWidth(chargeStr);
display->drawString(batteryX, textY, chargeStr);
display->drawString(batteryX + chargeNumWidth - 1, textY, "%");
if (isBold) {
display->drawString(batteryX + 1, textY, chargeStr);
display->drawString(batteryX + chargeNumWidth, textY, "%");
}
}
// === Time and Right-aligned Icons ===
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true);
char timeStr[10] = "--:--"; // Fallback display
int timeStrWidth = display->getStringWidth("12:34"); // Default alignment
int timeX = screenW - xOffset - timeStrWidth + 4;
if (rtc_sec > 0) {
// === Build Time String ===
long hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY;
int hour, minute, second;
graphics::decomposeTime(rtc_sec, hour, minute, second);
snprintf(timeStr, sizeof(timeStr), "%d:%02d", hour, minute);
// === Build Date String ===
char datetimeStr[25];
UIRenderer::formatDateTime(datetimeStr, sizeof(datetimeStr), rtc_sec, display, false);
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]);
}
display->drawXbm(batteryX, batteryY, 7, 11, batteryBitmap_v);
if (isCharging && isBoltVisibleShared)
display->drawXbm(batteryX + 1, batteryY + 3, 5, 5, lightning_bolt_v);
else {
display->drawXbm(batteryX - 1, batteryY + 4, 8, 3, batteryBitmap_sidegaps_v);
int fillHeight = 8 * chargePercent / 100;
int fillY = batteryY - fillHeight;
display->fillRect(batteryX + 1, fillY + 10, 5, fillHeight);
}
batteryX += 9; // Icon + 2 pixels
}
}
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 (chargePercent != 101) {
// === Battery % Display ===
char chargeStr[4];
snprintf(chargeStr, sizeof(chargeStr), "%d", chargePercent);
int chargeNumWidth = display->getStringWidth(chargeStr);
display->drawString(batteryX, textY, chargeStr);
display->drawString(batteryX + chargeNumWidth - 1, textY, "%");
if (isBold) {
display->drawString(batteryX + 1, textY, chargeStr);
display->drawString(batteryX + chargeNumWidth, textY, "%");
}
}
if (show_date) {
timeStrWidth = display->getStringWidth(dateLine);
} else {
timeStrWidth = display->getStringWidth(timeStr);
}
timeX = screenW - xOffset - timeStrWidth + 3;
// === Time and Right-aligned Icons ===
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true);
char timeStr[10] = "--:--"; // Fallback display
int timeStrWidth = display->getStringWidth("12:34"); // Default alignment
int timeX = screenW - xOffset - timeStrWidth + 4;
// === Show Mail or Mute Icon to the Left of Time ===
int iconRightEdge = timeX - 2;
if (rtc_sec > 0) {
// === Build Time String ===
long hms = (rtc_sec % SEC_PER_DAY + SEC_PER_DAY) % SEC_PER_DAY;
int hour, minute, second;
graphics::decomposeTime(rtc_sec, hour, minute, second);
snprintf(timeStr, sizeof(timeStr), "%d:%02d", hour, minute);
bool showMail = false;
// === Build Date String ===
char datetimeStr[25];
UIRenderer::formatDateTime(datetimeStr, sizeof(datetimeStr), rtc_sec, display, false);
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;
}
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
#else
if (hasUnreadMessage) {
showMail = true;
}
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 (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_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);
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);
}
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);
}
// === No Time Available: Mail/Mute Icon Moves to Far Right ===
int iconRightEdge = screenW - xOffset;
} else {
// === No Time Available: Mail/Mute Icon Moves to Far Right ===
int iconRightEdge = screenW - xOffset;
bool showMail = false;
bool showMail = false;
#ifndef USE_EINK
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
if (hasUnreadMessage) {
if (now - lastMailBlink > 500) {
isMailIconVisible = !isMailIconVisible;
lastMailBlink = now;
}
showMail = isMailIconVisible;
}
#else
if (hasUnreadMessage) {
showMail = true;
}
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);
}
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
display->setColor(WHITE); // Reset for other UI
}
const int *getTextPositions(OLEDDisplay *display) {
static int textPositions[7]; // Static array that persists beyond function scope
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;
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 *
// *************************
void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y) {
bool drawConnectionState = false;
if (service->api_state == service->STATE_BLE || service->api_state == service->STATE_WIFI || service->api_state == service->STATE_SERIAL ||
service->api_state == service->STATE_PACKET || service->api_state == service->STATE_HTTP || service->api_state == service->STATE_ETH) {
drawConnectionState = true;
}
void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y)
{
bool drawConnectionState = false;
if (service->api_state == service->STATE_BLE || service->api_state == service->STATE_WIFI ||
service->api_state == service->STATE_SERIAL || service->api_state == service->STATE_PACKET ||
service->api_state == service->STATE_HTTP || service->api_state == service->STATE_ETH) {
drawConnectionState = true;
}
if (drawConnectionState) {
const int scale = (currentResolution == ScreenResolution::High) ? 2 : 1;
display->setColor(BLACK);
display->fillRect(0, SCREEN_HEIGHT - (1 * scale) - (connection_icon_height * scale), (connection_icon_width * scale),
(connection_icon_height * scale) + (2 * scale));
display->setColor(WHITE);
if (currentResolution == ScreenResolution::High) {
const int bytesPerRow = (connection_icon_width + 7) / 8;
int iconX = 0;
int iconY = SCREEN_HEIGHT - (connection_icon_height * 2);
if (drawConnectionState) {
const int scale = (currentResolution == ScreenResolution::High) ? 2 : 1;
display->setColor(BLACK);
display->fillRect(0, SCREEN_HEIGHT - (1 * scale) - (connection_icon_height * scale), (connection_icon_width * scale),
(connection_icon_height * scale) + (2 * scale));
display->setColor(WHITE);
if (currentResolution == ScreenResolution::High) {
const int bytesPerRow = (connection_icon_width + 7) / 8;
int iconX = 0;
int iconY = SCREEN_HEIGHT - (connection_icon_height * 2);
for (int yy = 0; yy < connection_icon_height; ++yy) {
const uint8_t *rowPtr = connection_icon + yy * bytesPerRow;
for (int xx = 0; xx < connection_icon_width; ++xx) {
const uint8_t byteVal = pgm_read_byte(rowPtr + (xx >> 3));
const uint8_t bitMask = 1U << (xx & 7); // XBM is LSB-first
if (byteVal & bitMask) {
display->fillRect(iconX + xx * scale, iconY + yy * scale, scale, scale);
}
for (int yy = 0; yy < connection_icon_height; ++yy) {
const uint8_t *rowPtr = connection_icon + yy * bytesPerRow;
for (int xx = 0; xx < connection_icon_width; ++xx) {
const uint8_t byteVal = pgm_read_byte(rowPtr + (xx >> 3));
const uint8_t bitMask = 1U << (xx & 7); // XBM is LSB-first
if (byteVal & bitMask) {
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) {
const std::string allowed = ".,!?;:-_()[]{}'\"@#$/\\&+=%~^ ";
return allowed.find(c) != std::string::npos;
bool isAllowedPunctuation(char c)
{
const std::string allowed = ".,!?;:-_()[]{}'\"@#$/\\&+=%~^ ";
return allowed.find(c) != std::string::npos;
}
static void replaceAll(std::string &s, const std::string &from, const std::string &to) {
if (from.empty())
return;
size_t pos = 0;
while ((pos = s.find(from, pos)) != std::string::npos) {
s.replace(pos, from.size(), to);
pos += to.size();
}
}
std::string sanitizeString(const std::string &input) {
std::string output;
bool inReplacement = false;
// Make a mutable copy so we can normalize UTF-8 “smart punctuation” into ASCII first.
std::string s = input;
// Curly single quotes: ‘ ’
replaceAll(s, "\xE2\x80\x98", "'"); // U+2018
replaceAll(s, "\xE2\x80\x99", "'"); // U+2019
// Curly double quotes: “ ”
replaceAll(s, "\xE2\x80\x9C", "\""); // U+201C
replaceAll(s, "\xE2\x80\x9D", "\""); // U+201D
// En dash / Em dash: – —
replaceAll(s, "\xE2\x80\x93", "-"); // U+2013
replaceAll(s, "\xE2\x80\x94", "-"); // U+2014
// Non-breaking space
replaceAll(s, "\xC2\xA0", " "); // U+00A0
// Now do your original sanitize pass over the normalized string.
for (unsigned char uc : s) {
char c = static_cast<char>(uc);
if (std::isalnum(uc) || isAllowedPunctuation(c)) {
output += c;
inReplacement = false;
} else {
if (!inReplacement) {
output += static_cast<char>(0xBF); // ISO-8859-1 for inverted question mark
inReplacement = true;
}
static void replaceAll(std::string &s, const std::string &from, const std::string &to)
{
if (from.empty())
return;
size_t pos = 0;
while ((pos = s.find(from, pos)) != std::string::npos) {
s.replace(pos, from.size(), to);
pos += to.size();
}
}
}
return output;
std::string sanitizeString(const std::string &input)
{
std::string output;
bool inReplacement = false;
// Make a mutable copy so we can normalize UTF-8 “smart punctuation” into ASCII first.
std::string s = input;
// Curly single quotes: ‘ ’
replaceAll(s, "\xE2\x80\x98", "'"); // U+2018
replaceAll(s, "\xE2\x80\x99", "'"); // U+2019
// Curly double quotes: “ ”
replaceAll(s, "\xE2\x80\x9C", "\""); // U+201C
replaceAll(s, "\xE2\x80\x9D", "\""); // U+201D
// En dash / Em dash: – —
replaceAll(s, "\xE2\x80\x93", "-"); // U+2013
replaceAll(s, "\xE2\x80\x94", "-"); // U+2014
// Non-breaking space
replaceAll(s, "\xC2\xA0", " "); // U+00A0
// Now do your original sanitize pass over the normalized string.
for (unsigned char uc : s) {
char c = static_cast<char>(uc);
if (std::isalnum(uc) || isAllowedPunctuation(c)) {
output += c;
inReplacement = false;
} else {
if (!inReplacement) {
output += static_cast<char>(0xBF); // ISO-8859-1 for inverted question mark
inReplacement = true;
}
}
}
return output;
}
} // namespace graphics
src/graphics/SharedUIDisplay.h
+4 -2
View File
@@ -3,7 +3,8 @@
#include <OLEDDisplay.h>
#include <string>
namespace graphics {
namespace graphics
{
// =======================
// Shared UI Helpers
@@ -50,7 +51,8 @@ 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);
// Shared battery/time/mail header
void drawCommonHeader(OLEDDisplay *display, int16_t x, int16_t y, const char *titleStr = "", bool force_no_invert = false, bool show_date = false);
void drawCommonHeader(OLEDDisplay *display, int16_t x, int16_t y, const char *titleStr = "", bool force_no_invert = false,
bool show_date = false);
// Shared battery/time/mail header
void drawCommonFooter(OLEDDisplay *display, int16_t x, int16_t y);
src/graphics/TFTDisplay.cpp
+1122 -1071
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