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

稳定版代码

Browse Source 
Signed-off-by: 吴文峰 <kevin@lmve.net>
This commit is contained in:
kevin
2026-03-06 19:52:05 +08:00
parent 79d721d417
commit 26d5971aba
1533 changed files with 256309 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code/firmware-2.7.15.567b8ea/src/AmbientLightingThread.h
+212
View File
@@ -0,0 +1,212 @@
#include "Observer.h"
#include "configuration.h"
#ifdef HAS_NCP5623
#include <graphics/RAKled.h>
NCP5623 rgb;
#endif
#ifdef HAS_LP5562
#include <graphics/NomadStarLED.h>
LP5562 rgbw;
#endif
#ifdef HAS_NEOPIXEL
#include <graphics/NeoPixel.h>
Adafruit_NeoPixel pixels(NEOPIXEL_COUNT, NEOPIXEL_DATA, NEOPIXEL_TYPE);
#endif
#ifdef UNPHONE
#include "unPhone.h"
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.
// Enables Ambient Lighting by default if conditions are meet.
#ifdef HAS_RGB_LED
#ifdef ENABLE_AMBIENTLIGHTING
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;
#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;
}
#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");
#ifdef HAS_NCP5623
if (_type == ScanI2C::NCP5623) {
rgb.begin();
#endif
#ifdef HAS_LP5562
if (_type == ScanI2C::LP5562) {
rgbw.begin();
#endif
#ifdef RGBLED_RED
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);
#endif
setLighting();
#endif
#if defined(HAS_NCP5623) || defined(HAS_LP5562)
}
#endif
}
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) {
#endif
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();
}
// 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;
// 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");
#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");
#endif
#ifdef HAS_NEOPIXEL
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");
#elif defined(RGBLED_RED)
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");
#endif
return 0;
}
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);
#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);
#endif
#ifdef HAS_NEOPIXEL
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);
#endif
#if defined(BUTTON2_COLOR) && defined(BUTTON2_COLOR_INDEX)
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);
#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);
#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);
#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);
#endif
}
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/AudioThread.h
+107
View File
@@ -0,0 +1,107 @@
#pragma once
#include "PowerFSM.h"
#include "concurrency/OSThread.h"
#include "configuration.h"
#include "main.h"
#include "sleep.h"
#ifdef HAS_I2S
#include <AudioFileSourcePROGMEM.h>
#include <AudioGeneratorRTTTL.h>
#include <AudioOutputI2S.h>
#include <ESP8266SAM.h>
#ifdef USE_XL9555
#include "ExtensionIOXL9555.hpp"
extern ExtensionIOXL9555 io;
#endif
#define AUDIO_THREAD_INTERVAL_MS 100
class AudioThread : public concurrency::OSThread
{
public:
AudioThread() : OSThread("Audio") { initOutput(); }
void beginRttl(const void *data, uint32_t len)
{
#ifdef T_LORA_PAGER
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;
}
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);
#endif
ESP8266SAM *sam = new ESP8266SAM;
sam->Say(audioOut, text);
delete sam;
setCPUFast(false);
#ifdef T_LORA_PAGER
io.digitalWrite(EXPANDS_AMP_EN, LOW);
#endif
}
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;
}
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;
AudioFileSourcePROGMEM *rtttlFile;
};
#endif
code/firmware-2.7.15.567b8ea/src/BluetoothCommon.cpp
+17
View File
@@ -0,0 +1,17 @@
#include "BluetoothCommon.h"
#include "configuration.h"
// 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};
code/firmware-2.7.15.567b8ea/src/BluetoothCommon.h
+32
View File
@@ -0,0 +1,32 @@
#pragma once
#include <Arduino.h>
/**
* Common lib functions for all platforms that have bluetooth
*/
#define MESH_SERVICE_UUID "6ba1b218-15a8-461f-9fa8-5dcae273eafd"
#define TORADIO_UUID "f75c76d2-129e-4dad-a1dd-7866124401e7"
#define FROMRADIO_UUID "2c55e69e-4993-11ed-b878-0242ac120002"
#define FROMNUM_UUID "ed9da18c-a800-4f66-a670-aa7547e34453"
#define LEGACY_LOGRADIO_UUID "6c6fd238-78fa-436b-aacf-15c5be1ef2e2"
#define LOGRADIO_UUID "5a3d6e49-06e6-4423-9944-e9de8cdf9547"
// NRF52 wants these constants as byte arrays
// Generated here https://yupana-engineering.com/online-uuid-to-c-array-converter - but in REVERSE BYTE ORDER
extern const uint8_t MESH_SERVICE_UUID_16[], TORADIO_UUID_16[16u], FROMRADIO_UUID_16[], FROMNUM_UUID_16[], LOGRADIO_UUID_16[];
/// 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;
};
code/firmware-2.7.15.567b8ea/src/BluetoothStatus.h
+119
View File
@@ -0,0 +1,119 @@
#pragma once
#include "Status.h"
#include "assert.h"
#include "configuration.h"
#include "meshUtils.h"
#include <Arduino.h>
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,
};
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
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;
}
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
extern meshtastic::BluetoothStatus *bluetoothStatus;
code/firmware-2.7.15.567b8ea/src/DebugConfiguration.cpp
+198
View File
@@ -0,0 +1,198 @@
/* based on https://github.com/arcao/Syslog
MIT License
Copyright (c) 2016 Martin Sloup
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.*/
#include "configuration.h"
#include "DebugConfiguration.h"
#ifdef ARCH_PORTDUINO
#include "platform/portduino/PortduinoGlue.h"
#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);
}
#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)) {
this->_server = NULL;
} else {
this->_server = server;
}
this->_port = port;
return *this;
}
Syslog &Syslog::server(IPAddress ip, uint16_t port)
{
this->_ip = ip;
this->_server = NULL;
this->_port = port;
return *this;
}
Syslog &Syslog::deviceHostname(const char *deviceHostname)
{
this->_deviceHostname = (deviceHostname == NULL) ? SYSLOG_NILVALUE : deviceHostname;
return *this;
}
Syslog &Syslog::appName(const char *appName)
{
this->_appName = (appName == NULL) ? SYSLOG_NILVALUE : appName;
return *this;
}
Syslog &Syslog::defaultPriority(uint16_t pri)
{
this->_priDefault = pri;
return *this;
}
Syslog &Syslog::logMask(uint8_t priMask)
{
this->_priMask = priMask;
return *this;
}
void Syslog::enable()
{
this->_client->begin(this->_port);
this->_enabled = true;
}
void Syslog::disable()
{
this->_enabled = false;
this->_client->stop();
}
bool Syslog::isEnabled()
{
return this->_enabled;
}
bool Syslog::vlogf(uint16_t pri, const char *fmt, va_list args)
{
return this->vlogf(pri, this->_appName, fmt, args);
}
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;
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);
delete[] message;
return 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;
#else
bool utf = true;
#endif
if (!this->_enabled)
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();
return true;
}
#endif
code/firmware-2.7.15.567b8ea/src/DebugConfiguration.h
+198
View File
@@ -0,0 +1,198 @@
#pragma once
#include "configuration.h"
// Forward declarations
#if defined(DEBUG_HEAP)
class MemGet;
extern MemGet memGet;
#endif
// DEBUG LED
#ifndef LED_STATE_ON
#define LED_STATE_ON 1
#endif
// -----------------------------------------------------------------------------
// DEBUG
// -----------------------------------------------------------------------------
#ifdef CONSOLE_MAX_BAUD
#define SERIAL_BAUD CONSOLE_MAX_BAUD
#else
#define SERIAL_BAUD 115200 // Serial debug baud rate
#endif
#define MESHTASTIC_LOG_LEVEL_DEBUG "DEBUG"
#define MESHTASTIC_LOG_LEVEL_INFO "INFO "
#define MESHTASTIC_LOG_LEVEL_WARN "WARN "
#define MESHTASTIC_LOG_LEVEL_ERROR "ERROR"
#define MESHTASTIC_LOG_LEVEL_CRIT "CRIT "
#define MESHTASTIC_LOG_LEVEL_TRACE "TRACE"
#define MESHTASTIC_LOG_LEVEL_HEAP "HEAP"
#include "SerialConsole.h"
// If defined we will include support for ARM ICE "semihosting" for a virtual
// console over the JTAG port (to replace the normal serial port)
// Note: Normally this flag is passed into the gcc commandline by platformio.ini.
// for an example see env:rak4631_dap.
// #ifndef USE_SEMIHOSTING
// #define USE_SEMIHOSTING
// #endif
#define DEBUG_PORT (*console) // Serial debug port
#ifdef USE_SEGGER
// #undef DEBUG_PORT
#define LOG_DEBUG(...) SEGGER_RTT_printf(0, __VA_ARGS__)
#define LOG_INFO(...) SEGGER_RTT_printf(0, __VA_ARGS__)
#define LOG_WARN(...) SEGGER_RTT_printf(0, __VA_ARGS__)
#define LOG_ERROR(...) SEGGER_RTT_printf(0, __VA_ARGS__)
#define LOG_CRIT(...) SEGGER_RTT_printf(0, __VA_ARGS__)
#define LOG_TRACE(...) SEGGER_RTT_printf(0, __VA_ARGS__)
#else
#if defined(DEBUG_PORT) && !defined(DEBUG_MUTE)
#define LOG_DEBUG(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_DEBUG, __VA_ARGS__)
#define LOG_INFO(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_INFO, __VA_ARGS__)
#define LOG_WARN(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_WARN, __VA_ARGS__)
#define LOG_ERROR(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_ERROR, __VA_ARGS__)
#define LOG_CRIT(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_CRIT, __VA_ARGS__)
#define LOG_TRACE(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_TRACE, __VA_ARGS__)
#else
#define LOG_DEBUG(...)
#define LOG_INFO(...)
#define LOG_WARN(...)
#define LOG_ERROR(...)
#define LOG_CRIT(...)
#define LOG_TRACE(...)
#endif
#endif
#if defined(DEBUG_HEAP)
#define LOG_HEAP(...) DEBUG_PORT.log(MESHTASTIC_LOG_LEVEL_HEAP, __VA_ARGS__)
// 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)
#else
#define LOG_HEAP(...)
#define DEBUG_HEAP_BEFORE
#define DEBUG_HEAP_AFTER(context, ptr)
#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, ...);
#define SYSLOG_NILVALUE "-"
#define SYSLOG_CRIT 2 /* critical conditions */
#define SYSLOG_ERR 3 /* error conditions */
#define SYSLOG_WARN 4 /* warning conditions */
#define SYSLOG_INFO 6 /* informational */
#define SYSLOG_DEBUG 7 /* debug-level messages */
// trace does not go out to syslog (yet?)
#define LOG_PRIMASK 0x07 /* mask to extract priority part (internal) */
/* extract priority */
#define LOG_PRI(p) ((p)&LOG_PRIMASK)
#define LOG_MAKEPRI(fac, pri) (((fac) << 3) | (pri))
/* facility codes */
#define LOGLEVEL_KERN (0 << 3) /* kernel messages */
#define LOGLEVEL_USER (1 << 3) /* random user-level messages */
#define LOGLEVEL_MAIL (2 << 3) /* mail system */
#define LOGLEVEL_DAEMON (3 << 3) /* system daemons */
#define LOGLEVEL_AUTH (4 << 3) /* security/authorization messages */
#define LOGLEVEL_SYSLOG (5 << 3) /* messages generated internally by syslogd */
#define LOGLEVEL_LPR (6 << 3) /* line printer subsystem */
#define LOGLEVEL_NEWS (7 << 3) /* network news subsystem */
#define LOGLEVEL_UUCP (8 << 3) /* UUCP subsystem */
#define LOGLEVEL_CRON (9 << 3) /* clock daemon */
#define LOGLEVEL_AUTHPRIV (10 << 3) /* security/authorization messages (private) */
#define LOGLEVEL_FTP (11 << 3) /* ftp daemon */
/* other codes through 15 reserved for system use */
#define LOGLEVEL_LOCAL0 (16 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL1 (17 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL2 (18 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL3 (19 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL4 (20 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL5 (21 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL6 (22 << 3) /* reserved for local use */
#define LOGLEVEL_LOCAL7 (23 << 3) /* reserved for local use */
#define LOG_NFACILITIES 24 /* current number of facilities */
#define LOG_FACMASK 0x03f8 /* mask to extract facility part */
/* facility of pri */
#define LOG_FAC(p) (((p)&LOG_FACMASK) >> 3)
#define LOG_MASK(pri) (1 << (pri)) /* mask for one priority */
#define LOG_UPTO(pri) ((1 << ((pri) + 1)) - 1) /* all priorities through pri */
// -----------------------------------------------------------------------------
// AXP192 (Rev1-specific options)
// -----------------------------------------------------------------------------
#define GPS_POWER_CTRL_CH 3
#define LORA_POWER_CTRL_CH 2
// Default Bluetooth PIN
#define defaultBLEPin 123456
#if HAS_ETHERNET && !defined(USE_WS5500)
#include <RAK13800_W5100S.h>
#endif // HAS_ETHERNET
#if HAS_ETHERNET && defined(USE_WS5500)
#include <ETHClass2.h>
#define ETH ETH2
#endif // HAS_ETHERNET
#if HAS_WIFI
#include <WiFi.h>
#endif // HAS_WIFI
#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;
bool _sendLog(uint16_t pri, const char *appName, const char *message);
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);
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)));
};
#endif // HAS_NETWORKING
code/firmware-2.7.15.567b8ea/src/DisplayFormatters.cpp
+83
View File
@@ -0,0 +1,83 @@
#include "DisplayFormatters.h"
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";
}
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_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;
}
}
code/firmware-2.7.15.567b8ea/src/DisplayFormatters.h
+10
View File
@@ -0,0 +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);
};
code/firmware-2.7.15.567b8ea/src/FSCommon.cpp
+338
View File
@@ -0,0 +1,338 @@
/**
* @file FSCommon.cpp
* @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.
*
*/
#include "FSCommon.h"
#include "SPILock.h"
#include "configuration.h"
// Software SPI is used by MUI so disable SD card here until it's also implemented
#if defined(HAS_SDCARD) && !defined(SDCARD_USE_SOFT_SPI)
#include <SD.h>
#include <SPI.h>
#ifdef SDCARD_USE_SPI1
SPIClass SPI_HSPI(HSPI);
#define SDHandler SPI_HSPI
#else
#define SDHandler SPI
#endif
#ifndef SD_SPI_FREQUENCY
#define SD_SPI_FREQUENCY 4000000U
#endif
#endif // HAS_SDCARD
/**
* @brief Copies a file from one location to another.
*
* @param from The path of the source file.
* @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)
{
#ifdef FSCom
// 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 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);
}
f2.flush();
f2.close();
f1.close();
return true;
#endif
}
/**
* Renames a file from pathFrom to pathTo.
*
* @param pathFrom The original path of the file.
* @param pathTo The new path of the file.
*
* @return True if the file was successfully renamed, false otherwise.
*/
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;
#else
// copyFile does its own locking.
if (copyFile(pathFrom, pathTo) && FSCom.remove(pathFrom)) {
return true;
} else {
return false;
}
#endif
#endif
}
#include <vector>
/**
* @brief Get the list of files in a directory.
*
* This function returns a list of files in a directory. The list includes the full path of each file.
* We can't use SPILOCK here because of recursion. Callers of this function should use SPILOCK.
*
* @param dirname The name of the directory.
* @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 = {};
#ifdef FSCom
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) {
#ifdef ARCH_ESP32
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.path(), levels - 1);
#else
std::vector<meshtastic_FileInfo> subDirFilenames = getFiles(file.name(), levels - 1);
#endif
filenames.insert(filenames.end(), subDirFilenames.begin(), subDirFilenames.end());
file.close();
}
} else {
meshtastic_FileInfo fileInfo = {"", static_cast<uint32_t>(file.size())};
#ifdef ARCH_ESP32
strcpy(fileInfo.file_name, file.path());
#else
strcpy(fileInfo.file_name, file.name());
#endif
if (!String(fileInfo.file_name).endsWith(".")) {
filenames.push_back(fileInfo);
}
file.close();
}
file = root.openNextFile();
}
root.close();
#endif
return filenames;
}
/**
* Lists the contents of a directory.
* We can't use SPILOCK here because of recursion. Callers of this function should use SPILOCK.
*
* @param dirname The name of the directory to list.
* @param levels The number of levels of subdirectories to list.
* @param del Whether or not to delete the contents of the directory after listing.
*/
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();
#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
}
/**
* @brief Removes a directory and all its contents.
*
* This function recursively removes a directory and all its contents, including subdirectories and files.
*
* @param dirname The name of the directory to remove.
*/
void rmDir(const char *dirname)
{
#ifdef FSCom
#if (defined(ARCH_ESP32) || defined(ARCH_RP2040) || defined(ARCH_PORTDUINO))
listDir(dirname, 10, true);
#elif defined(ARCH_NRF52)
// nRF52 implementation of LittleFS has a recursive delete function
FSCom.rmdir_r(dirname);
#endif
#endif
}
/**
* Some platforms (nrf52) might need to do an extra step before FSBegin().
*/
__attribute__((weak, noinline)) void preFSBegin() {}
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.
}
#if defined(ARCH_ESP32)
LOG_DEBUG("Filesystem files (%d/%d Bytes):", FSCom.usedBytes(), FSCom.totalBytes());
#else
LOG_DEBUG("Filesystem files:");
#endif
listDir("/", 10);
#endif
}
/**
* Initializes the SD card and mounts the file system.
*/
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");
}
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
}
code/firmware-2.7.15.567b8ea/src/FSCommon.h
+58
View File
@@ -0,0 +1,58 @@
#pragma once
#include "configuration.h"
#include <vector>
// Cross platform filesystem API
#if defined(ARCH_PORTDUINO)
// Portduino version
#include "PortduinoFS.h"
#define FSCom PortduinoFS
#define FSBegin() true
#define FILE_O_WRITE "w"
#define FILE_O_READ "r"
#endif
#if defined(ARCH_STM32WL)
// STM32WL
#include "LittleFS.h"
#define FSCom InternalFS
#define FSBegin() FSCom.begin()
using namespace STM32_LittleFS_Namespace;
#endif
#if defined(ARCH_RP2040)
// RP2040
#include "LittleFS.h"
#define FSCom LittleFS
#define FSBegin() FSCom.begin() // set autoformat
#define FILE_O_WRITE "w"
#define FILE_O_READ "r"
#endif
#if defined(ARCH_ESP32)
// ESP32 version
#include "LittleFS.h"
#define FSCom LittleFS
#define FSBegin() FSCom.begin(true) // format on failure
#define FILE_O_WRITE "w"
#define FILE_O_READ "r"
#endif
#if defined(ARCH_NRF52)
// NRF52 version
#include "InternalFileSystem.h"
#define FSCom InternalFS
#define FSBegin() FSCom.begin() // InternalFS formats on failure
using namespace Adafruit_LittleFS_Namespace;
#endif
void fsInit();
void fsListFiles();
bool copyFile(const char *from, const char *to);
bool renameFile(const char *pathFrom, const char *pathTo);
std::vector<meshtastic_FileInfo> getFiles(const char *dirname, uint8_t levels);
void listDir(const char *dirname, uint8_t levels, bool del = false);
void rmDir(const char *dirname);
void setupSDCard();
code/firmware-2.7.15.567b8ea/src/Fusion/Fusion.h
+32
View File
@@ -0,0 +1,32 @@
/**
* @file Fusion.h
* @author Seb Madgwick
* @brief Main header file for the Fusion library. This is the only file that
* needs to be included when using the library.
*/
#ifndef FUSION_H
#define FUSION_H
//------------------------------------------------------------------------------
// Includes
#ifdef __cplusplus
extern "C" {
#endif
#include "FusionAhrs.h"
#include "FusionAxes.h"
#include "FusionCalibration.h"
#include "FusionCompass.h"
#include "FusionConvention.h"
#include "FusionMath.h"
#include "FusionOffset.h"
#ifdef __cplusplus
}
#endif
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionAhrs.c
+542
View File
@@ -0,0 +1,542 @@
/**
* @file FusionAhrs.c
* @author Seb Madgwick
* @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
* measurements into a single measurement of orientation relative to the Earth.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionAhrs.h"
#include <float.h> // FLT_MAX
#include <math.h> // atan2f, cosf, fabsf, powf, sinf
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Initial gain used during the initialisation.
*/
#define INITIAL_GAIN (10.0f)
/**
* @brief Initialisation period in seconds.
*/
#define INITIALISATION_PERIOD (3.0f)
//------------------------------------------------------------------------------
// Function declarations
static inline FusionVector HalfGravity(const FusionAhrs *const ahrs);
static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs);
static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference);
static inline int Clamp(const int value, const int min, const int max);
//------------------------------------------------------------------------------
// Functions
/**
* @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);
}
/**
* @brief Resets the AHRS algorithm. This is equivalent to reinitialising the
* 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;
}
/**
* @brief Sets the AHRS algorithm settings.
* @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;
}
/**
* @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
* magnetometer measurements.
* @param ahrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in g.
* @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)
{
#define Q ahrs->quaternion.element
// 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;
}
// 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;
}
// 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));
// 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)));
// Normalise quaternion
ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion);
#undef Q
}
/**
* @brief Returns the direction of gravity scaled by 0.5.
* @param ahrs AHRS algorithm structure.
* @return Direction of gravity scaled by 0.5.
*/
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
#undef Q
}
/**
* @brief Returns the direction of the magnetic field scaled by 0.5.
* @param ahrs AHRS algorithm structure.
* @return Direction of the magnetic field scaled by 0.5.
*/
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
#undef Q
}
/**
* @brief Returns the feedback.
* @param sensor Sensor.
* @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);
}
/**
* @brief Returns a value limited to maximum and minimum.
* @param value Value.
* @param min Minimum value.
* @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;
}
/**
* @brief Updates the AHRS algorithm using the gyroscope and accelerometer
* measurements only.
* @param ahrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @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)
{
// Update AHRS algorithm
FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime);
// Zero heading during initialisation
if (ahrs->initialising) {
FusionAhrsSetHeading(ahrs, 0.0f);
}
}
/**
* @brief Updates the AHRS algorithm using the gyroscope, accelerometer, and
* heading measurements.
* @param ahrs AHRS algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @param accelerometer Accelerometer measurement in 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)
{
#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 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);
#undef Q
}
/**
* @brief Returns the quaternion describing the sensor relative to the Earth.
* @param ahrs AHRS algorithm structure.
* @return Quaternion describing the sensor relative to the Earth.
*/
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;
}
/**
* @brief Returns the linear acceleration measurement equal to the accelerometer
* measurement with the 1 g of gravity removed.
* @param ahrs AHRS algorithm structure.
* @return Linear acceleration measurement in g.
*/
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
// 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
}
/**
* @brief Returns the Earth acceleration measurement equal to accelerometer
* measurement in the Earth coordinate frame with the 1 g of gravity removed.
* @param ahrs AHRS algorithm structure.
* @return Earth acceleration measurement in g.
*/
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
// 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
}
/**
* @brief Returns the AHRS algorithm internal states.
* @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;
}
/**
* @brief Returns the AHRS algorithm flags.
* @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;
}
/**
* @brief Sets the heading of the orientation measurement provided by the AHRS
* algorithm. This function can be used to reset drift in heading when the AHRS
* algorithm is being used without a magnetometer.
* @param ahrs AHRS algorithm structure.
* @param heading Heading angle in degrees.
*/
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);
#undef Q
}
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionAhrs.h
+112
View File
@@ -0,0 +1,112 @@
/**
* @file FusionAhrs.h
* @author Seb Madgwick
* @brief AHRS algorithm to combine gyroscope, accelerometer, and magnetometer
* measurements into a single measurement of orientation relative to the Earth.
*/
#ifndef FUSION_AHRS_H
#define FUSION_AHRS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionConvention.h"
#include "FusionMath.h"
#include <stdbool.h>
//------------------------------------------------------------------------------
// Definitions
/**
* @brief AHRS algorithm settings.
*/
typedef struct {
FusionConvention convention;
float gain;
float gyroscopeRange;
float accelerationRejection;
float magneticRejection;
unsigned int recoveryTriggerPeriod;
} FusionAhrsSettings;
/**
* @brief AHRS algorithm structure. Structure members are used internally and
* 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;
} FusionAhrs;
/**
* @brief AHRS algorithm internal states.
*/
typedef struct {
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;
} FusionAhrsFlags;
//------------------------------------------------------------------------------
// Function declarations
void FusionAhrsInitialise(FusionAhrs *const ahrs);
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 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);
FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs);
void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion);
FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs);
FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs);
FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs);
FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs);
void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading);
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionAxes.h
+188
View File
@@ -0,0 +1,188 @@
/**
* @file FusionAxes.h
* @author Seb Madgwick
* @brief Swaps sensor axes for alignment with the body axes.
*/
#ifndef FUSION_AXES_H
#define FUSION_AXES_H
//------------------------------------------------------------------------------
// Includes
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Axes alignment describing the sensor axes relative to the body axes.
* For example, if the body X axis is aligned with the sensor Y axis and the
* body Y axis is aligned with sensor X axis but pointing the opposite direction
* 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 */
} FusionAxesAlignment;
//------------------------------------------------------------------------------
// Inline functions
/**
* @brief Swaps sensor axes for alignment with the body axes.
* @param sensor Sensor axes.
* @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
}
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionCalibration.h
+49
View File
@@ -0,0 +1,49 @@
/**
* @file FusionCalibration.h
* @author Seb Madgwick
* @brief Gyroscope, accelerometer, and magnetometer calibration models.
*/
#ifndef FUSION_CALIBRATION_H
#define FUSION_CALIBRATION_H
//------------------------------------------------------------------------------
// Includes
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Inline functions
/**
* @brief Gyroscope and accelerometer calibration model.
* @param uncalibrated Uncalibrated measurement.
* @param misalignment Misalignment matrix.
* @param sensitivity Sensitivity.
* @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));
}
/**
* @brief Magnetometer calibration model.
* @param uncalibrated Uncalibrated measurement.
* @param softIronMatrix Soft-iron matrix.
* @param hardIronOffset Hard-iron offset.
* @return Calibrated measurement.
*/
static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix,
const FusionVector hardIronOffset)
{
return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset));
}
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionCompass.c
+51
View File
@@ -0,0 +1,51 @@
/**
* @file FusionCompass.c
* @author Seb Madgwick
* @brief Tilt-compensated compass to calculate the magnetic heading using
* accelerometer and magnetometer measurements.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionCompass.h"
#include "FusionAxes.h"
#include <math.h> // atan2f
//------------------------------------------------------------------------------
// Functions
/**
* @brief Calculates the magnetic heading.
* @param convention Earth axes convention.
* @param accelerometer Accelerometer measurement in any calibrated units.
* @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
}
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionCompass.h
+26
View File
@@ -0,0 +1,26 @@
/**
* @file FusionCompass.h
* @author Seb Madgwick
* @brief Tilt-compensated compass to calculate the magnetic heading using
* accelerometer and magnetometer measurements.
*/
#ifndef FUSION_COMPASS_H
#define FUSION_COMPASS_H
//------------------------------------------------------------------------------
// Includes
#include "FusionConvention.h"
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Function declarations
float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer,
const FusionVector magnetometer);
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionConvention.h
+25
View File
@@ -0,0 +1,25 @@
/**
* @file FusionConvention.h
* @author Seb Madgwick
* @brief Earth axes convention.
*/
#ifndef FUSION_CONVENTION_H
#define FUSION_CONVENTION_H
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Earth axes convention.
*/
typedef enum {
FusionConventionNwu, /* North-West-Up */
FusionConventionEnu, /* East-North-Up */
FusionConventionNed, /* North-East-Down */
} FusionConvention;
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionMath.h
+503
View File
@@ -0,0 +1,503 @@
/**
* @file FusionMath.h
* @author Seb Madgwick
* @brief Math library.
*/
#ifndef FUSION_MATH_H
#define FUSION_MATH_H
//------------------------------------------------------------------------------
// Includes
#include <math.h> // M_PI, sqrtf, atan2f, asinf
#include <stdbool.h>
#include <stdint.h>
//------------------------------------------------------------------------------
// Definitions
/**
* @brief 3D vector.
*/
typedef union {
float array[3];
struct {
float x;
float y;
float z;
} axis;
} FusionVector;
/**
* @brief Quaternion.
*/
typedef union {
float array[4];
struct {
float w;
float x;
float y;
float z;
} element;
} FusionQuaternion;
/**
* @brief 3x3 matrix in row-major order.
* See http://en.wikipedia.org/wiki/Row-major_order
*/
typedef union {
float array[3][3];
struct {
float xx;
float xy;
float xz;
float yx;
float yy;
float yz;
float zx;
float zy;
float zz;
} element;
} FusionMatrix;
/**
* @brief Euler angles. Roll, pitch, and yaw correspond to rotations around
* X, Y, and Z respectively.
*/
typedef union {
float array[3];
struct {
float roll;
float pitch;
float yaw;
} angle;
} FusionEuler;
/**
* @brief Vector of zeros.
*/
#define FUSION_VECTOR_ZERO ((FusionVector){.array = {0.0f, 0.0f, 0.0f}})
/**
* @brief Vector of ones.
*/
#define FUSION_VECTOR_ONES ((FusionVector){.array = {1.0f, 1.0f, 1.0f}})
/**
* @brief Identity quaternion.
*/
#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){.array = {1.0f, 0.0f, 0.0f, 0.0f}})
/**
* @brief Identity matrix.
*/
#define FUSION_IDENTITY_MATRIX ((FusionMatrix){.array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}})
/**
* @brief Euler angles of zero.
*/
#define FUSION_EULER_ZERO ((FusionEuler){.array = {0.0f, 0.0f, 0.0f}})
/**
* @brief Pi. May not be defined in math.h.
*/
#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif
/**
* @brief Include this definition or add as a preprocessor definition to use
* normal square root operations.
*/
// #define FUSION_USE_NORMAL_SQRT
//------------------------------------------------------------------------------
// Inline functions - Degrees and radians conversion
/**
* @brief Converts degrees to radians.
* @param degrees Degrees.
* @return Radians.
*/
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);
}
//------------------------------------------------------------------------------
// Inline functions - Arc sine
/**
* @brief Returns the arc sine of the value.
* @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);
}
//------------------------------------------------------------------------------
// Inline functions - Fast inverse square root
#ifndef FUSION_USE_NORMAL_SQRT
/**
* @brief Calculates the reciprocal of the square root.
* See https://pizer.wordpress.com/2008/10/12/fast-inverse-square-root/
* @param x Operand.
* @return Reciprocal of the square root of x.
*/
static inline float FusionFastInverseSqrt(const float x)
{
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);
}
#endif
//------------------------------------------------------------------------------
// Inline functions - Vector operations
/**
* @brief Returns true if the vector is zero.
* @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);
}
/**
* @brief Returns the sum of two vectors.
* @param vectorA Vector A.
* @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;
}
/**
* @brief Returns vector B subtracted from vector A.
* @param vectorA Vector A.
* @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;
}
/**
* @brief Returns the sum of the elements.
* @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;
}
/**
* @brief Returns the multiplication of a vector by a scalar.
* @param vector Vector.
* @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;
}
/**
* @brief Calculates the Hadamard product (element-wise multiplication).
* @param vectorA Vector A.
* @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;
}
/**
* @brief Returns the cross product.
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Cross product.
*/
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;
#undef A
#undef B
}
/**
* @brief Returns the dot product.
* @param vectorA Vector A.
* @param vectorB Vector B.
* @return Dot product.
*/
static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB)
{
return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB));
}
/**
* @brief Returns the vector magnitude squared.
* @param vector Vector.
* @return Vector magnitude squared.
*/
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));
}
/**
* @brief Returns the normalised vector.
* @param vector Vector.
* @return Normalised vector.
*/
static inline FusionVector FusionVectorNormalise(const FusionVector vector)
{
#ifdef FUSION_USE_NORMAL_SQRT
const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector));
#else
const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector));
#endif
return FusionVectorMultiplyScalar(vector, magnitudeReciprocal);
}
//------------------------------------------------------------------------------
// Inline functions - Quaternion operations
/**
* @brief Returns the sum of two quaternions.
* @param quaternionA Quaternion A.
* @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;
}
/**
* @brief Returns the multiplication of two quaternions.
* @param quaternionA Quaternion A (to be post-multiplied).
* @param quaternionB Quaternion B (to be pre-multiplied).
* @return Multiplication of two quaternions.
*/
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;
#undef A
#undef B
}
/**
* @brief Returns the multiplication of a quaternion with a vector. This is a
* normal quaternion multiplication where the vector is treated a
* quaternion with a W element value of zero. The quaternion is post-
* multiplied by the vector.
* @param quaternion Quaternion.
* @param vector Vector.
* @return Multiplication of a quaternion with a 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;
#undef Q
#undef V
}
/**
* @brief Returns the normalised quaternion.
* @param quaternion Quaternion.
* @return Normalised 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);
#else
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;
#undef Q
}
//------------------------------------------------------------------------------
// Inline functions - Matrix operations
/**
* @brief Returns the multiplication of a matrix with a vector.
* @param matrix Matrix.
* @param vector Vector.
* @return Multiplication of a matrix with a 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;
#undef R
}
//------------------------------------------------------------------------------
// Inline functions - Conversion operations
/**
* @brief Converts a quaternion to a rotation matrix.
* @param quaternion Quaternion.
* @return Rotation matrix.
*/
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;
#undef Q
}
/**
* @brief Converts a quaternion to ZYX Euler angles in degrees.
* @param quaternion Quaternion.
* @return Euler angles in degrees.
*/
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;
#undef Q
}
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionOffset.c
+80
View File
@@ -0,0 +1,80 @@
/**
* @file FusionOffset.c
* @author Seb Madgwick
* @brief Gyroscope offset correction algorithm for run-time calibration of the
* gyroscope offset.
*/
//------------------------------------------------------------------------------
// Includes
#include "FusionOffset.h"
#include <math.h> // fabsf
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Cutoff frequency in Hz.
*/
#define CUTOFF_FREQUENCY (0.02f)
/**
* @brief Timeout in seconds.
*/
#define TIMEOUT (5)
/**
* @brief Threshold in degrees per second.
*/
#define THRESHOLD (3.0f)
//------------------------------------------------------------------------------
// Functions
/**
* @brief Initialises the gyroscope offset algorithm.
* @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;
}
/**
* @brief Updates the gyroscope offset algorithm and returns the corrected
* gyroscope measurement.
* @param offset Gyroscope offset algorithm structure.
* @param gyroscope Gyroscope measurement in degrees per second.
* @return Corrected gyroscope measurement in degrees per second.
*/
FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope)
{
// 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;
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;
}
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/Fusion/FusionOffset.h
+40
View File
@@ -0,0 +1,40 @@
/**
* @file FusionOffset.h
* @author Seb Madgwick
* @brief Gyroscope offset correction algorithm for run-time calibration of the
* gyroscope offset.
*/
#ifndef FUSION_OFFSET_H
#define FUSION_OFFSET_H
//------------------------------------------------------------------------------
// Includes
#include "FusionMath.h"
//------------------------------------------------------------------------------
// Definitions
/**
* @brief Gyroscope offset algorithm structure. Structure members are used
* internally and must not be accessed by the application.
*/
typedef struct {
float filterCoefficient;
unsigned int timeout;
unsigned int timer;
FusionVector gyroscopeOffset;
} FusionOffset;
//------------------------------------------------------------------------------
// Function declarations
void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate);
FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope);
#endif
//------------------------------------------------------------------------------
// End of file
code/firmware-2.7.15.567b8ea/src/GPSStatus.h
+141
View File
@@ -0,0 +1,141 @@
#pragma once
#include "NodeDB.h"
#include "Status.h"
#include "configuration.h"
#include <Arduino.h>
namespace meshtastic
{
/// Describes the state of the GPS system.
class GPSStatus : public Status
{
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 isPowerSaving = false; // Are we in power saving state
meshtastic_Position p = meshtastic_Position_init_default;
/// Time of last valid GPS fix (millis since boot)
uint32_t lastFixMillis = 0;
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;
// 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;
}
}
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;
}
}
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 getDOP() const { return p.PDOP; }
uint32_t getHeading() const { return p.ground_track; }
uint32_t getNumSatellites() const { return p.sats_in_view; }
/// Return millis() when the last GPS fix occurred (0 = never)
uint32_t getLastFixMillis() const { return lastFixMillis; }
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);
#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");
}
initialized = true;
hasLock = newStatus->hasLock;
isConnected = newStatus->isConnected;
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;
}
};
} // namespace meshtastic
extern meshtastic::GPSStatus *gpsStatus;
code/firmware-2.7.15.567b8ea/src/GpioLogic.cpp
+102
View File
@@ -0,0 +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 GpioHwPin::set(bool value)
{
pinMode(num, OUTPUT);
digitalWrite(num, value);
}
GpioTransformer::GpioTransformer(GpioPin *outPin) : outPin(outPin) {}
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;
// 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
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
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;
// 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;
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);
}
}
set(newValue);
}
GpioSplitter::GpioSplitter(GpioPin *outPin1, GpioPin *outPin2) : outPin1(outPin1), outPin2(outPin2) {}
code/firmware-2.7.15.567b8ea/src/GpioLogic.h
+160
View File
@@ -0,0 +1,160 @@
#pragma once
#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.
Note: these classes are intended to be 99% inline for the common case so should have minimal impact on flash or RAM
requirements.
*/
/**
* A logical GPIO pin (not necessary raw hardware).
*/
class GpioPin
{
public:
virtual void set(bool value) = 0;
};
/**
* A physical GPIO hw pin.
*/
class GpioHwPin : public GpioPin
{
uint32_t num;
public:
explicit GpioHwPin(uint32_t num) : num(num) {}
void set(bool value);
};
class GpioTransformer;
class GpioNotTransformer;
class GpioBinaryTransformer;
/**
* A virtual GPIO pin.
*/
class GpioVirtPin : public GpioPin
{
friend class GpioBinaryTransformer;
friend class GpioUnaryTransformer;
public:
enum PinState { On = true, Off = false, Unset = 2 };
void set(bool value);
PinState get() const { return value; }
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)
*/
class GpioTransformer
{
public:
/**
* Update the output pin based on the current state of the input pin.
*/
virtual void update() = 0;
protected:
GpioTransformer(GpioPin *outPin);
void set(bool value);
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);
protected:
friend class GpioVirtPin;
/**
* Update the output pin based on the current state of the input pin.
*/
virtual void update();
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) {}
protected:
friend class GpioVirtPin;
/**
* 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
{
public:
enum Operation { And, Or, Xor };
GpioBinaryTransformer(GpioVirtPin *inPin1, GpioVirtPin *inPin2, GpioPin *outPin, Operation operation);
protected:
friend class GpioVirtPin;
/**
* Update the output pin based on the current state of the input pins.
*/
void update();
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
{
public:
GpioSplitter(GpioPin *outPin1, GpioPin *outPin2);
void set(bool value)
{
outPin1->set(value);
outPin2->set(value);
}
private:
GpioPin *outPin1;
GpioPin *outPin2;
};
code/firmware-2.7.15.567b8ea/src/Led.cpp
+66
View File
@@ -0,0 +1,66 @@
#include "Led.h"
#include "PowerMon.h"
#include "main.h"
#include "power.h"
GpioVirtPin ledForceOn, ledBlink;
#if defined(LED_PIN)
// Most boards have a GPIO for LED control
static GpioHwPin ledRawHwPin(LED_PIN);
#else
static GpioVirtPin ledRawHwPin; // Dummy pin for no hardware
#endif
#if LED_STATE_ON == 0
static GpioVirtPin ledHwPin;
static GpioNotTransformer ledInverter(&ledHwPin, &ledRawHwPin);
#else
static GpioPin &ledHwPin = ledRawHwPin;
#endif
#if defined(HAS_PMU)
/**
* 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);
}
}
} ledPmuHwPin;
// In some cases we need to drive a PMU LED and a normal LED
static GpioSplitter ledFinalPin(&ledHwPin, &ledPmuHwPin);
#else
static GpioPin &ledFinalPin = ledHwPin;
#endif
#ifdef USE_POWERMON
/**
* 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);
}
ledFinalPin.set(value);
}
} monitoredLedPin;
#else
static GpioPin &monitoredLedPin = ledFinalPin;
#endif
static GpioBinaryTransformer ledForcer(&ledForceOn, &ledBlink, &monitoredLedPin, GpioBinaryTransformer::Or);
code/firmware-2.7.15.567b8ea/src/Led.h
+7
View File
@@ -0,0 +1,7 @@
#include "GpioLogic.h"
#include "configuration.h"
/**
* ledForceOn and ledForceOff both override the normal ledBlinker behavior (which is controlled by main)
*/
extern GpioVirtPin ledForceOn, ledBlink;
code/firmware-2.7.15.567b8ea/src/NodeStatus.h
+68
View File
@@ -0,0 +1,68 @@
#pragma once
#include "Status.h"
#include "configuration.h"
#include <Arduino.h>
namespace meshtastic
{
/// Describes the state of the NodeDB system.
class NodeStatus : public Status
{
private:
CallbackObserver<NodeStatus, const NodeStatus *> statusObserver =
CallbackObserver<NodeStatus, const NodeStatus *>(this, &NodeStatus::updateStatus);
uint16_t numOnline = 0;
uint16_t numTotal = 0;
uint16_t lastNumTotal = 0;
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;
{
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;
}
};
} // namespace meshtastic
extern meshtastic::NodeStatus *nodeStatus;
code/firmware-2.7.15.567b8ea/src/Observer.cpp
+2
View File
@@ -0,0 +1,2 @@
#include "Observer.h"
#include "configuration.h"
code/firmware-2.7.15.567b8ea/src/Observer.h
+106
View File
@@ -0,0 +1,106 @@
#pragma once
#include <Arduino.h>
#include <list>
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;
public:
virtual ~Observer();
/// Stop watching the observable
void unobserve(Observable<T> *o);
/// Start watching a specified observable
void observe(Observable<T> *o);
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;
};
/**
* An observer that calls an arbitrary method
*/
template <class Callback, class T> class CallbackObserver : public Observer<T>
{
typedef int (Callback::*ObserverCallback)(T arg);
Callback *objPtr;
ObserverCallback method;
public:
CallbackObserver(Callback *_objPtr, ObserverCallback _method) : objPtr(_objPtr), method(_method) {}
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.
*/
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;
}
return 0;
}
private:
friend class Observer<T>;
// Not called directly, instead call observer.observe
void addObserver(Observer<T> *o) { observers.push_back(o); }
void removeObserver(Observer<T> *o) { observers.remove(o); }
};
template <class T> Observer<T>::~Observer()
{
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>::observe(Observable<T> *o)
{
observables.push_back(o);
o->addObserver(this);
}
code/firmware-2.7.15.567b8ea/src/Power.cpp
+1550
View File
File diff suppressed because it is too large Load Diff
code/firmware-2.7.15.567b8ea/src/PowerFSM.cpp
+408
View File
@@ -0,0 +1,408 @@
/**
* @file PowerFSM.cpp
* @brief Implements the finite state machine for power management.
*
* This file contains the implementation of the finite state machine (FSM) for power management.
* The FSM controls the power states of the device, including SDS (shallow deep sleep), LS (light sleep),
* NB (normal mode), and POWER (powered mode). The FSM also handles transitions between states and
* actions to be taken upon entering or exiting each state.
*/
#include "PowerFSM.h"
#include "Default.h"
#include "Led.h"
#include "MeshService.h"
#include "NodeDB.h"
#include "PowerMon.h"
#include "configuration.h"
#include "graphics/Screen.h"
#include "main.h"
#include "sleep.h"
#include "target_specific.h"
#if HAS_WIFI && !defined(ARCH_PORTDUINO) || defined(MESHTASTIC_EXCLUDE_WIFI)
#include "mesh/wifi/WiFiAPClient.h"
#endif
#ifndef SLEEP_TIME
#define SLEEP_TIME 30
#endif
#if MESHTASTIC_EXCLUDE_POWER_FSM
FakeFsm powerFSM;
void PowerFSM_setup(){};
#else
/// Should we behave as if we have AC power now?
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;
#endif
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;
/* 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.
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_DEBUG("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_DEBUG("State: Lower batt SDS");
doDeepSleep(Default::getConfiguredOrDefaultMs(config.power.sds_secs), false, true);
}
extern Power *power;
static void shutdownEnter()
{
LOG_DEBUG("State: SHUTDOWN");
shutdownAtMsec = millis();
}
#include "error.h"
static uint32_t secsSlept;
static void lsEnter()
{
LOG_INFO("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");
}
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;
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
ledBlink.set(true); // briefly turn on led
wakeCause2 = doLightSleep(100); // leave led on for 1ms
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;
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);
#else
bool pressed = false;
#endif
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 {
// Someone says we can't sleep now, so just save some power by sleeping the CPU for 100ms or so
delay(100);
}
} 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_INFO("Exit state: LS");
}
static void nbEnter()
{
LOG_DEBUG("State: NB");
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 darkEnter()
{
setBluetoothEnable(true);
if (screen)
screen->setOn(false);
}
static void serialEnter()
{
LOG_DEBUG("State: SERIAL");
setBluetoothEnable(false);
if (screen) {
screen->setOn(true);
}
}
static void serialExit()
{
// Turn bluetooth back on when we leave serial stream API
setBluetoothEnable(true);
}
static void powerEnter()
{
// LOG_DEBUG("State: POWER");
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 powerIdle()
{
if (!isPowered()) {
// If we got here, we are in the wrong state
LOG_INFO("Loss of power in Powered");
powerFSM.trigger(EVENT_POWER_DISCONNECTED);
}
}
static void powerExit()
{
if (screen)
screen->setOn(true);
setBluetoothEnable(true);
}
static void onEnter()
{
LOG_DEBUG("State: ON");
if (screen)
screen->setOn(true);
setBluetoothEnable(true);
}
static void 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_DEBUG("State: BOOT");
}
State stateSHUTDOWN(shutdownEnter, NULL, NULL, "SHUTDOWN");
State stateSDS(sdsEnter, NULL, NULL, "SDS");
State stateLowBattSDS(lowBattSDSEnter, NULL, NULL, "SDS");
State stateLS(lsEnter, lsIdle, lsExit, "LS");
State stateNB(nbEnter, NULL, NULL, "NB");
State stateDARK(darkEnter, NULL, NULL, "DARK");
State stateSERIAL(serialEnter, NULL, serialExit, "SERIAL");
State stateBOOT(bootEnter, NULL, NULL, "BOOT");
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();
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)
#ifdef ARCH_ESP32
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");
#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");
// 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 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
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");
// Removed 2.7: we don't show the nodes individually for every node on the screen anymore
// powerFSM.add_transition(&stateNB, &stateON, EVENT_NODEDB_UPDATED, NULL, "NodeDB update");
// powerFSM.add_transition(&stateDARK, &stateON, EVENT_NODEDB_UPDATED, NULL, "NodeDB update");
// powerFSM.add_transition(&stateON, &stateON, EVENT_NODEDB_UPDATED, NULL, "NodeDB update");
// 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 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");
// 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");
#ifdef USE_EINK
// 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");
}
// 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
#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;
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");
}
#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");
#endif
powerFSM.run_machine(); // run one iteration of the state machine, so we run our on enter tasks for the initial DARK state
}
#endif
code/firmware-2.7.15.567b8ea/src/PowerFSM.h
+51
View File
@@ -0,0 +1,51 @@
#pragma once
#include "configuration.h"
// See sw-design.md for documentation
#define EVENT_PRESS 1
#define EVENT_WAKE_TIMER 2
// #define EVENT_RECEIVED_PACKET 3
#define EVENT_PACKET_FOR_PHONE 4
#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_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
#define EVENT_SERIAL_DISCONNECTED 12
#define EVENT_POWER_CONNECTED 13
#define EVENT_POWER_DISCONNECTED 14
#define EVENT_FIRMWARE_UPDATE 15 // We just received a new firmware update packet from the phone
#define EVENT_SHUTDOWN 16 // force a full shutdown now (not just sleep)
#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; };
private:
bool serialConnected = false;
};
extern FakeFsm powerFSM;
void PowerFSM_setup();
#else
#include <Fsm.h>
extern Fsm powerFSM;
extern State stateON, statePOWER, stateSERIAL, stateDARK;
void PowerFSM_setup();
#endif
code/firmware-2.7.15.567b8ea/src/PowerFSMThread.h
+45
View File
@@ -0,0 +1,45 @@
#include "Default.h"
#include "NodeDB.h"
#include "PowerFSM.h"
#include "concurrency/OSThread.h"
#include "configuration.h"
#include "main.h"
#include "power.h"
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") {}
protected:
int32_t runOnce() override
{
#if !MESHTASTIC_EXCLUDE_POWER_FSM
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 (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;
#else
return INT32_MAX;
#endif
}
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/PowerMon.cpp
+47
View File
@@ -0,0 +1,47 @@
#include "PowerMon.h"
#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);
}
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);
}
#endif
}
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);
}
#endif
}
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);
#endif
}
PowerMon *powerMon;
void powerMonInit()
{
powerMon = new PowerMon();
}
code/firmware-2.7.15.567b8ea/src/PowerMon.h
+44
View File
@@ -0,0 +1,44 @@
#pragma once
#include "configuration.h"
#include "meshtastic/powermon.pb.h"
#ifndef MESHTASTIC_EXCLUDE_POWERMON
#define USE_POWERMON // FIXME turn this only for certain builds
#endif
/**
* The singleton class for monitoring power consumption of device
* subsystems/modes.
*
* For more information see the PowerMon docs.
*/
class PowerMon
{
uint64_t states = 0UL;
friend class PowerStressModule;
/**
* If stress testing we always want all events logged
*/
bool force_enabled = false;
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 = "");
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);
};
extern PowerMon *powerMon;
void powerMonInit();
code/firmware-2.7.15.567b8ea/src/PowerStatus.h
+103
View File
@@ -0,0 +1,103 @@
#pragma once
#include "Status.h"
#include "configuration.h"
#include <Arduino.h>
namespace meshtastic
{
/**
* A boolean where we have a third state of Unknown
*/
enum OptionalBool { OptFalse = 0, OptTrue = 1, OptUnknown = 2 };
/// Describes the state of the Power system.
class PowerStatus : public Status
{
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;
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;
{
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;
}
};
} // namespace meshtastic
extern meshtastic::PowerStatus *powerStatus;
code/firmware-2.7.15.567b8ea/src/RF95Configuration.h
+6
View File
@@ -0,0 +1,6 @@
// TODO refactor this out with better radio configuration system
#ifdef USE_RF95
#define RF95_RESET LORA_RESET
#define RF95_IRQ LORA_DIO0 // on SX1262 version this is a no connect DIO0
#define RF95_DIO1 LORA_DIO1 // Note: not really used for RF95, but used for pure SX127x
#endif
code/firmware-2.7.15.567b8ea/src/RedirectablePrint.cpp
+405
View File
@@ -0,0 +1,405 @@
#include "RedirectablePrint.h"
#include "NodeDB.h"
#include "RTC.h"
#include "concurrency/OSThread.h"
#include "configuration.h"
#include "main.h"
#include "memGet.h"
#include "mesh/generated/meshtastic/mesh.pb.h"
#include <assert.h>
#include <cstring>
#include <memory>
#include <stdexcept>
#include <sys/time.h>
#include <time.h>
#ifdef ARCH_PORTDUINO
#include "platform/portduino/PortduinoGlue.h"
#endif
#if HAS_NETWORKING
extern Syslog syslog;
#endif
void RedirectablePrint::rpInit()
{
#ifdef HAS_FREE_RTOS
inDebugPrint = xSemaphoreCreateMutexStatic(&this->_MutexStorageSpace);
#endif
}
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
#ifdef USE_SEGGER
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);
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;
#if ENABLE_JSON_LOGGING || ARCH_PORTDUINO
static char printBuf[512];
#else
static char printBuf[160];
#endif
#ifdef ARCH_PORTDUINO
bool color = !portduino_config.ascii_logs;
#else
bool color = true;
#endif
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 (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;
#ifdef ARCH_PORTDUINO
bool color = !portduino_config.ascii_logs;
#else
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);
}
uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); // display local time on logfile
if (rtc_sec > 0) {
long hms = rtc_sec % SEC_PER_DAY;
// hms += tz.tz_dsttime * SEC_PER_HOUR;
// hms -= tz.tz_minuteswest * SEC_PER_MIN;
// mod `hms` to ensure in positive range of [0...SEC_PER_DAY)
hms = (hms + SEC_PER_DAY) % SEC_PER_DAY;
// Tear apart hms into h:m:s
int hour = hms / SEC_PER_HOUR;
int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN;
int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN
#ifdef ARCH_PORTDUINO
::printf("%s ", logLevel);
if (color) {
::printf("\u001b[0m");
}
::printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
#else
printf("%s ", logLevel);
if (color) {
printf("\u001b[0m");
}
printf("| %02d:%02d:%02d %u ", hour, min, sec, millis() / 1000);
#endif
} else {
#ifdef ARCH_PORTDUINO
::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);
#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);
}
}
#endif
}
void RedirectablePrint::log_to_ble(const char *logLevel, const char *format, va_list arg)
{
#if !MESHTASTIC_EXCLUDE_BLUETOOTH
if (config.security.debug_log_api_enabled && !pauseBluetoothLogging) {
bool isBleConnected = false;
#ifdef ARCH_ESP32
isBleConnected = nimbleBluetooth && nimbleBluetooth->isActive() && nimbleBluetooth->isConnected();
#elif defined(ARCH_NRF52)
isBleConnected = nrf52Bluetooth != nullptr && nrf52Bluetooth->isConnected();
#endif
if (isBleConnected) {
char *message;
size_t initialLen;
size_t len;
initialLen = strlen(format);
message = new char[initialLen + 1];
len = vsnprintf(message, initialLen + 1, format, arg);
if (len > initialLen) {
delete[] message;
message = new char[len + 1];
vsnprintf(message, len + 1, format, arg);
}
auto thread = concurrency::OSThread::currentThread;
meshtastic_LogRecord logRecord = meshtastic_LogRecord_init_zero;
logRecord.level = getLogLevel(logLevel);
strcpy(logRecord.message, message);
if (thread)
strcpy(logRecord.source, thread->ThreadName.c_str());
logRecord.time = getValidTime(RTCQuality::RTCQualityDevice, true);
uint8_t *buffer = new uint8_t[meshtastic_LogRecord_size];
size_t size = pb_encode_to_bytes(buffer, meshtastic_LogRecord_size, meshtastic_LogRecord_fields, &logRecord);
#ifdef ARCH_ESP32
nimbleBluetooth->sendLog(buffer, size);
#elif defined(ARCH_NRF52)
nrf52Bluetooth->sendLog(buffer, size);
#endif
delete[] message;
delete[] buffer;
}
}
#else
(void)logLevel;
(void)format;
(void)arg;
#endif
}
meshtastic_LogRecord_Level RedirectablePrint::getLogLevel(const char *logLevel)
{
meshtastic_LogRecord_Level ll = meshtastic_LogRecord_Level_UNSET; // default to unset
switch (logLevel[0]) {
case 'D':
ll = meshtastic_LogRecord_Level_DEBUG;
break;
case 'I':
ll = meshtastic_LogRecord_Level_INFO;
break;
case 'W':
ll = meshtastic_LogRecord_Level_WARNING;
break;
case 'E':
ll = meshtastic_LogRecord_Level_ERROR;
break;
case 'C':
ll = meshtastic_LogRecord_Level_CRITICAL;
break;
}
return ll;
}
void RedirectablePrint::log(const char *logLevel, const char *format, ...)
{
// append \n to format
size_t len = strlen(format);
char *newFormat = new char[len + 2];
strcpy(newFormat, format);
newFormat[len] = '\n';
newFormat[len + 1] = '\0';
#if ARCH_PORTDUINO
// 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_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;
}
#ifdef HAS_FREE_RTOS
if (inDebugPrint != nullptr && xSemaphoreTake(inDebugPrint, portMAX_DELAY) == pdTRUE) {
#else
if (!inDebugPrint) {
inDebugPrint = true;
#endif
va_list arg;
va_start(arg, format);
log_to_serial(logLevel, newFormat, arg);
log_to_syslog(logLevel, newFormat, arg);
log_to_ble(logLevel, newFormat, arg);
va_end(arg);
#ifdef HAS_FREE_RTOS
xSemaphoreGive(inDebugPrint);
#else
inDebugPrint = false;
#endif
}
delete[] newFormat;
return;
}
void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len)
{
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
break;
}
return std::string(formatted.get());
}
code/firmware-2.7.15.567b8ea/src/RedirectablePrint.h
+59
View File
@@ -0,0 +1,59 @@
#pragma once
#include "../freertosinc.h"
#include "mesh/generated/meshtastic/mesh.pb.h"
#include <Print.h>
#include <stdarg.h>
#include <string>
/**
* A Printable that can be switched to squirt its bytes to a different sink.
* 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;
#ifdef HAS_FREE_RTOS
SemaphoreHandle_t inDebugPrint = nullptr;
StaticSemaphore_t _MutexStorageSpace;
#else
volatile bool inDebugPrint = false;
#endif
public:
explicit RedirectablePrint(Print *_dest) : dest(_dest) {}
/**
* Set a new destination
*/
void rpInit();
void setDestination(Print *dest);
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)));
/** 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);
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);
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);
};
code/firmware-2.7.15.567b8ea/src/SPILock.cpp
+12
View File
@@ -0,0 +1,12 @@
#include "SPILock.h"
#include "configuration.h"
#include <Arduino.h>
#include <assert.h>
concurrency::Lock *spiLock;
void initSPI()
{
assert(!spiLock);
spiLock = new concurrency::Lock();
}
code/firmware-2.7.15.567b8ea/src/SPILock.h
+12
View File
@@ -0,0 +1,12 @@
#pragma once
#include "../concurrency/LockGuard.h"
/**
* Used to provide mutual exclusion for access to the SPI bus. Usage:
* concurrency::LockGuard g(spiLock);
*/
extern concurrency::Lock *spiLock;
/** Setup SPI access and create the spiLock lock. */
void initSPI();
code/firmware-2.7.15.567b8ea/src/SafeFile.cpp
+123
View File
@@ -0,0 +1,123 @@
#include "SafeFile.h"
#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);
#ifdef ARCH_NRF52
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)
}
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);
// }
// 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);
}
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)
}
/**
* Atomically close the file (deleting any old versions) and readback the contents to confirm the hash matches
*
* @return false for failure
*/
bool SafeFile::close()
{
if (!f)
return false;
spiLock->lock();
f.close();
spiLock->unlock();
#ifdef ARCH_NRF52
return true;
#endif
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;
}
}
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;
}
/// Read our (closed) tempfile back in and compare the hash
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;
}
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;
}
return true;
}
#endif
code/firmware-2.7.15.567b8ea/src/SafeFile.h
+50
View File
@@ -0,0 +1,50 @@
#pragma once
#include "FSCommon.h"
#include "SPILock.h"
#include "configuration.h"
#ifdef FSCom
/**
* 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.
*
* 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.
*/
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);
/**
* 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();
String filename;
File f;
bool fullAtomic;
uint8_t hash = 0;
};
#endif
code/firmware-2.7.15.567b8ea/src/SerialConsole.cpp
+148
View File
@@ -0,0 +1,148 @@
#include "SerialConsole.h"
#include "Default.h"
#include "NodeDB.h"
#include "PowerFSM.h"
#include "Throttle.h"
#include "configuration.h"
#include "time.h"
#if defined(ARDUINO_USB_CDC_ON_BOOT) && ARDUINO_USB_CDC_ON_BOOT
#define IS_USB_SERIAL
#ifdef SERIAL_HAS_ON_RECEIVE
#undef SERIAL_HAS_ON_RECEIVE
#endif
#include "HWCDC.h"
#endif
#ifdef RP2040_SLOW_CLOCK
#define Port Serial2
#else
#ifdef USER_DEBUG_PORT // change by WayenWeng
#define Port USER_DEBUG_PORT
#else
#define Port Serial
#endif
#endif
// Defaulting to the formerly removed phone_timeout_secs value of 15 minutes
#define SERIAL_CONNECTION_TIMEOUT (15 * 60) * 1000UL
SerialConsole *console;
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(); });
#endif
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();
}
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);
#endif
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;
}
}
#endif
#if !ARCH_PORTDUINO
emitRebooted();
#endif
}
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;
}
#endif
int32_t delay = runOncePart();
#if defined(SERIAL_HAS_ON_RECEIVE) || defined(CONFIG_IDF_TARGET_ESP32S2)
return Port.available() ? delay : INT32_MAX;
#elif defined(IS_USB_SERIAL)
return HWCDC::isPlugged() ? delay : (1000 * 20);
#else
return delay;
#endif
}
void SerialConsole::flush()
{
Port.flush();
}
// trigger tx of serial data
void SerialConsole::onNowHasData(uint32_t fromRadioNum)
{
setIntervalFromNow(0);
}
// trigger rx of serial data
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);
}
/**
* 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;
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);
}
code/firmware-2.7.15.567b8ea/src/SerialConsole.h
+51
View File
@@ -0,0 +1,51 @@
#pragma once
#include "RedirectablePrint.h"
#include "StreamAPI.h"
/**
* 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;
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;
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;
void flush();
void rxInt();
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;
/// 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
void consolePrintf(const char *format, ...);
void consoleInit();
extern SerialConsole *console;
code/firmware-2.7.15.567b8ea/src/Status.h
+57
View File
@@ -0,0 +1,57 @@
#pragma once
#include "Observer.h"
// Constants for the various status types, so we can tell subclass instances apart
#define STATUS_TYPE_BASE 0
#define STATUS_TYPE_POWER 1
#define STATUS_TYPE_GPS 2
#define STATUS_TYPE_NODE 3
#define STATUS_TYPE_BLUETOOTH 4
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;
public:
// Allows us to generate observable events
Observable<const Status *> onNewStatus;
// Enable polymorphism ?
virtual ~Status() = default;
Status()
{
if (!statusType) {
statusType = STATUS_TYPE_BASE;
}
}
// 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); }
// 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; }
int getStatusType() const { return statusType; }
// Called when the Observable we're observing generates a new notification
int updateStatus(const Status *newStatus) { return 0; }
};
}; // namespace meshtastic
code/firmware-2.7.15.567b8ea/src/airtime.cpp
+210
View File
@@ -0,0 +1,210 @@
#include "airtime.h"
#include "NodeDB.h"
#include "configuration.h"
AirTime *airTime = NULL;
// Don't read out of this directly. Use the helper functions.
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)
{
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->airtimes.periodTX[0] = 0;
this->airtimes.periodRX[0] = 0;
this->airtimes.periodRX_ALL[0] = 0;
air_period_tx[0] = 0;
air_period_rx[0] = 0;
this->airtimes.lastPeriodIndex = this->currentPeriodIndex();
}
}
uint32_t *AirTime::airtimeReport(reportTypes reportType)
{
if (reportType == TX_LOG) {
return this->airtimes.periodTX;
} else if (reportType == RX_LOG) {
return this->airtimes.periodRX;
} else if (reportType == RX_ALL_LOG) {
return this->airtimes.periodRX_ALL;
}
return 0;
}
uint8_t AirTime::getPeriodsToLog()
{
return PERIODS_TO_LOG;
}
uint32_t AirTime::getSecondsPerPeriod()
{
return SECONDS_PER_PERIOD;
}
uint32_t AirTime::getSecondsSinceBoot()
{
return this->secSinceBoot;
}
float AirTime::channelUtilizationPercent()
{
uint32_t sum = 0;
for (uint32_t i = 0; i < CHANNEL_UTILIZATION_PERIODS; i++) {
sum += this->channelUtilization[i];
}
return (float(sum) / float(CHANNEL_UTILIZATION_PERIODS * 10 * 1000)) * 100;
}
float AirTime::utilizationTXPercent()
{
uint32_t sum = 0;
for (uint32_t i = 0; i < MINUTES_IN_HOUR; i++) {
sum += this->utilizationTX[i];
}
return (float(sum) / float(MS_IN_HOUR)) * 100;
}
bool AirTime::isTxAllowedChannelUtil(bool polite)
{
uint8_t percentage = (polite ? polite_channel_util_percent : max_channel_util_percent);
if (channelUtilizationPercent() < percentage) {
return true;
} else {
LOG_WARN("Ch. util >%d%%. Skip send", percentage);
return false;
}
}
bool AirTime::isTxAllowedAirUtil()
{
if (!config.lora.override_duty_cycle && myRegion->dutyCycle < 100) {
if (utilizationTXPercent() < myRegion->dutyCycle * polite_duty_cycle_percent / 100) {
return true;
} else {
LOG_WARN("TX air util. >%f%%. Skip send", myRegion->dutyCycle * polite_duty_cycle_percent / 100);
return false;
}
}
return true;
}
// 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;
}
return MINUTES_IN_HOUR;
}
AirTime::AirTime() : concurrency::OSThread("AirTime"), airtimes({}) {}
int32_t AirTime::runOnce()
{
secSinceBoot++;
uint8_t utilPeriod = this->getPeriodUtilMinute();
uint8_t utilPeriodTX = this->getPeriodUtilHour();
if (firstTime) {
// 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;
}
}
return (1000 * 1);
}
code/firmware-2.7.15.567b8ea/src/airtime.h
+89
View File
@@ -0,0 +1,89 @@
#pragma once
#include "MeshRadio.h"
#include "concurrency/OSThread.h"
#include "configuration.h"
#include <Arduino.h>
#include <functional>
/*
TX_LOG - Time on air this device has transmitted
RX_LOG - Time on air used by valid and routable mesh packets, does not include
TX air time
RX_ALL_LOG - Time of all received lora packets. This includes packets that are not
for meshtastic devices. Does not include TX air time.
Example analytics:
TX_LOG + RX_LOG = Total air time for a particular meshtastic channel.
TX_LOG + RX_LOG = Total air time for a particular meshtastic channel, including
other lora radios.
RX_ALL_LOG - RX_LOG = Other lora radios on our frequency channel.
*/
#define CHANNEL_UTILIZATION_PERIODS 6
#define SECONDS_PER_PERIOD 3600
#define PERIODS_TO_LOG 8
#define MINUTES_IN_HOUR 60
#define SECONDS_IN_MINUTE 60
#define MS_IN_MINUTE (SECONDS_IN_MINUTE * 1000)
#define MS_IN_HOUR (MINUTES_IN_HOUR * SECONDS_IN_MINUTE * 1000)
enum reportTypes { TX_LOG, RX_LOG, RX_ALL_LOG };
void logAirtime(reportTypes reportType, uint32_t airtime_ms);
uint32_t *airtimeReport(reportTypes reportType);
class AirTime : private concurrency::OSThread
{
public:
AirTime();
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};
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
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();
protected:
virtual int32_t runOnce() override;
};
extern AirTime *airTime;
code/firmware-2.7.15.567b8ea/src/buzz/BuzzerFeedbackThread.cpp
+61
View File
@@ -0,0 +1,61 @@
#include "BuzzerFeedbackThread.h"
#include "NodeDB.h"
#include "buzz.h"
#include "configuration.h"
BuzzerFeedbackThread *buzzerFeedbackThread;
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:
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
}
code/firmware-2.7.15.567b8ea/src/buzz/BuzzerFeedbackThread.h
+17
View File
@@ -0,0 +1,17 @@
#pragma once
#include "Observer.h"
#include "concurrency/OSThread.h"
#include "input/InputBroker.h"
class BuzzerFeedbackThread
{
CallbackObserver<BuzzerFeedbackThread, const InputEvent *> inputObserver =
CallbackObserver<BuzzerFeedbackThread, const InputEvent *>(this, &BuzzerFeedbackThread::handleInputEvent);
public:
BuzzerFeedbackThread();
int handleInputEvent(const InputEvent *event);
};
extern BuzzerFeedbackThread *buzzerFeedbackThread;
code/firmware-2.7.15.567b8ea/src/buzz/buzz.cpp
+167
View File
@@ -0,0 +1,167 @@
#include "buzz.h"
#include "NodeDB.h"
#include "configuration.h"
#if !defined(ARCH_ESP32) && !defined(ARCH_RP2040) && !defined(ARCH_PORTDUINO)
#include "Tone.h"
#endif
#if !defined(ARCH_PORTDUINO)
extern "C" void delay(uint32_t dwMs);
#endif
struct ToneDuration {
int frequency_khz;
int duration_ms;
};
// Some common frequencies.
#define NOTE_C3 131
#define NOTE_CS3 139
#define NOTE_D3 147
#define NOTE_DS3 156
#define NOTE_E3 165
#define NOTE_F3 175
#define NOTE_FS3 185
#define NOTE_G3 196
#define NOTE_GS3 208
#define NOTE_A3 220
#define NOTE_AS3 233
#define NOTE_B3 247
#define NOTE_CS4 277
const int DURATION_1_8 = 125; // 1/8 note
const int DURATION_1_4 = 250; // 1/4 note
const int DURATION_1_2 = 500; // 1/2 note
const int DURATION_3_4 = 750; // 1/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 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()
{
ToneDuration melody[] = {{NOTE_C3, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_CS4, DURATION_1_4}};
playTones(melody, sizeof(melody) / sizeof(ToneDuration));
}
void playGPSDisableBeep()
{
ToneDuration melody[] = {{NOTE_CS4, DURATION_1_8}, {NOTE_FS3, DURATION_1_4}, {NOTE_C3, 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 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 playChirp()
{
// A short, friendly "chirp" sound for key presses
ToneDuration melody[] = {{NOTE_AS3, 20}}; // Very 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 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
static int leadUpNoteIndex = 0;
static const ToneDuration leadUpNotes[] = {
{NOTE_C3, 100}, // Start low
{NOTE_E3, 100}, // Step up
{NOTE_G3, 100}, // Keep climbing
{NOTE_B3, 150} // Peak with longer note for emphasis
};
static const int leadUpNotesCount = sizeof(leadUpNotes) / sizeof(ToneDuration);
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
leadUpNoteIndex++;
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));
}
code/firmware-2.7.15.567b8ea/src/buzz/buzz.h
+14
View File
@@ -0,0 +1,14 @@
#pragma once
void playBeep();
void playLongBeep();
void playStartMelody();
void playShutdownMelody();
void playGPSEnableBeep();
void playGPSDisableBeep();
void playComboTune();
void playBoop();
void playChirp();
void playLongPressLeadUp();
bool playNextLeadUpNote(); // Play the next note in the lead-up sequence
void resetLeadUpSequence(); // Reset the lead-up sequence to start from beginning
code/firmware-2.7.15.567b8ea/src/commands.h
+18
View File
@@ -0,0 +1,18 @@
/**
* @brief This class enables on the fly software and hardware setup.
* It will contain all command messages to change internal settings.
*/
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
};
code/firmware-2.7.15.567b8ea/src/concurrency/BinarySemaphoreFreeRTOS.cpp
+40
View File
@@ -0,0 +1,40 @@
#include "concurrency/BinarySemaphoreFreeRTOS.h"
#include "configuration.h"
#include <assert.h>
#ifdef HAS_FREE_RTOS
namespace concurrency
{
BinarySemaphoreFreeRTOS::BinarySemaphoreFreeRTOS() : semaphore(xSemaphoreCreateBinary())
{
assert(semaphore);
}
BinarySemaphoreFreeRTOS::~BinarySemaphoreFreeRTOS()
{
vSemaphoreDelete(semaphore);
}
/**
* Returns false if we were interrupted
*/
bool BinarySemaphoreFreeRTOS::take(uint32_t msec)
{
return xSemaphoreTake(semaphore, pdMS_TO_TICKS(msec));
}
void BinarySemaphoreFreeRTOS::give()
{
xSemaphoreGive(semaphore);
}
IRAM_ATTR void BinarySemaphoreFreeRTOS::giveFromISR(BaseType_t *pxHigherPriorityTaskWoken)
{
xSemaphoreGiveFromISR(semaphore, pxHigherPriorityTaskWoken);
}
} // namespace concurrency
#endif
code/firmware-2.7.15.567b8ea/src/concurrency/BinarySemaphoreFreeRTOS.h
+30
View File
@@ -0,0 +1,30 @@
#pragma once
#include "../freertosinc.h"
namespace concurrency
{
#ifdef HAS_FREE_RTOS
class BinarySemaphoreFreeRTOS
{
SemaphoreHandle_t semaphore;
public:
BinarySemaphoreFreeRTOS();
~BinarySemaphoreFreeRTOS();
/**
* Returns false if we timed out
*/
bool take(uint32_t msec);
void give();
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
};
#endif
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/BinarySemaphorePosix.cpp
+28
View File
@@ -0,0 +1,28 @@
#include "concurrency/BinarySemaphorePosix.h"
#include "configuration.h"
#ifndef HAS_FREE_RTOS
namespace concurrency
{
BinarySemaphorePosix::BinarySemaphorePosix() {}
BinarySemaphorePosix::~BinarySemaphorePosix() {}
/**
* Returns false if we timed out
*/
bool BinarySemaphorePosix::take(uint32_t msec)
{
delay(msec); // FIXME
return false;
}
void BinarySemaphorePosix::give() {}
IRAM_ATTR void BinarySemaphorePosix::giveFromISR(BaseType_t *pxHigherPriorityTaskWoken) {}
} // namespace concurrency
#endif
code/firmware-2.7.15.567b8ea/src/concurrency/BinarySemaphorePosix.h
+30
View File
@@ -0,0 +1,30 @@
#pragma once
#include "../freertosinc.h"
namespace concurrency
{
#ifndef HAS_FREE_RTOS
class BinarySemaphorePosix
{
// SemaphoreHandle_t semaphore;
public:
BinarySemaphorePosix();
~BinarySemaphorePosix();
/**
* Returns false if we timed out
*/
bool take(uint32_t msec);
void give();
void giveFromISR(BaseType_t *pxHigherPriorityTaskWoken);
};
#endif
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/InterruptableDelay.cpp
+35
View File
@@ -0,0 +1,35 @@
#include "concurrency/InterruptableDelay.h"
#include "configuration.h"
namespace concurrency
{
InterruptableDelay::InterruptableDelay() {}
InterruptableDelay::~InterruptableDelay() {}
/**
* Returns false if we were interrupted
*/
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);
// LOG_DEBUG("interrupt=%d", r);
return !r;
}
void InterruptableDelay::interrupt()
{
semaphore.give();
}
IRAM_ATTR void InterruptableDelay::interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken)
{
semaphore.giveFromISR(pxHigherPriorityTaskWoken);
}
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/InterruptableDelay.h
+41
View File
@@ -0,0 +1,41 @@
#pragma once
#include "../freertosinc.h"
#ifdef HAS_FREE_RTOS
#include "concurrency/BinarySemaphoreFreeRTOS.h"
#define BinarySemaphore BinarySemaphoreFreeRTOS
#else
#include "concurrency/BinarySemaphorePosix.h"
#define BinarySemaphore BinarySemaphorePosix
#endif
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.
*
* This is implemented for FreeRTOS but should be easy to port to other operating systems.
*/
class InterruptableDelay
{
BinarySemaphore semaphore;
public:
InterruptableDelay();
~InterruptableDelay();
/**
* Returns false if we were interrupted
*/
bool delay(uint32_t msec);
void interrupt();
void interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken);
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/Lock.cpp
+38
View File
@@ -0,0 +1,38 @@
#include "Lock.h"
#include "configuration.h"
#include <cassert>
namespace concurrency
{
#ifdef HAS_FREE_RTOS
Lock::Lock() : handle(xSemaphoreCreateBinary())
{
assert(handle);
if (xSemaphoreGive(handle) == false) {
abort();
}
}
void Lock::lock()
{
if (xSemaphoreTake(handle, portMAX_DELAY) == false) {
abort();
}
}
void Lock::unlock()
{
if (xSemaphoreGive(handle) == false) {
abort();
}
}
#else
Lock::Lock() {}
void Lock::lock() {}
void Lock::unlock() {}
#endif
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/Lock.h
+35
View File
@@ -0,0 +1,35 @@
#pragma once
#include "../freertosinc.h"
namespace concurrency
{
/**
* @brief Simple wrapper around FreeRTOS API for implementing a mutex lock
*/
class Lock
{
public:
Lock();
Lock(const Lock &) = delete;
Lock &operator=(const Lock &) = delete;
/// Locks the lock.
//
// Must not be called from an ISR.
void lock();
// Unlocks the lock.
//
// Must not be called from an ISR.
void unlock();
private:
#ifdef HAS_FREE_RTOS
SemaphoreHandle_t handle;
#endif
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/LockGuard.cpp
+17
View File
@@ -0,0 +1,17 @@
#include "LockGuard.h"
#include "configuration.h"
namespace concurrency
{
LockGuard::LockGuard(Lock *lock) : lock(lock)
{
lock->lock();
}
LockGuard::~LockGuard()
{
lock->unlock();
}
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/LockGuard.h
+24
View File
@@ -0,0 +1,24 @@
#pragma once
#include "Lock.h"
namespace concurrency
{
/**
* @brief RAII lock guard
*/
class LockGuard
{
public:
explicit LockGuard(Lock *lock);
~LockGuard();
LockGuard(const LockGuard &) = delete;
LockGuard &operator=(const LockGuard &) = delete;
private:
Lock *lock;
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/NotifiedWorkerThread.cpp
+94
View File
@@ -0,0 +1,94 @@
#include "NotifiedWorkerThread.h"
#include "configuration.h"
#include "main.h"
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);
if (r)
mainDelay.interrupt();
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;
notification = v;
if (debugNotification) {
LOG_DEBUG("Set notification %d", v);
}
return true;
} else {
if (debugNotification) {
LOG_DEBUG("Drop notification %d", v);
}
return false;
}
}
/**
* Notify from an ISR
*
* 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);
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);
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;
}
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();
return RUN_SAME;
}
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/NotifiedWorkerThread.h
+56
View File
@@ -0,0 +1,56 @@
#pragma once
#include "OSThread.h"
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;
public:
NotifiedWorkerThread(const char *name) : OSThread(name) {}
/**
* 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);
/**
* 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;
/// 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();
private:
/**
* Notify this thread so it can run
*/
bool notifyCommon(uint32_t v, bool overwrite);
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/OSThread.cpp
+144
View File
@@ -0,0 +1,144 @@
#include "OSThread.h"
#include "configuration.h"
#include "memGet.h"
#include <assert.h>
namespace concurrency
{
/// Show debugging info for disabled threads
bool OSThread::showDisabled;
/// Show debugging info for threads when we run them
bool OSThread::showRun = false;
/// Show debugging info for threads we decide not to run;
bool OSThread::showWaiting = false;
const OSThread *OSThread::currentThread;
ThreadController mainController, timerController;
InterruptableDelay mainDelay;
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();
ThreadName = _name;
if (controller) {
bool added = controller->add(this);
assert(added);
}
}
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;
// 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);
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 (showDisabled && !enabled) {
LOG_DEBUG("Thread %s: disabled", ThreadName.c_str());
}
return r;
}
void OSThread::run()
{
#ifdef DEBUG_HEAP
auto heap = memGet.getFreeHeap();
#endif
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);
#endif
#ifdef DEBUG_LOOP_TIMING
LOG_DEBUG("====== Thread next run in: %d", newDelay);
#endif
runned();
if (newDelay >= 0)
setInterval(newDelay);
currentThread = NULL;
}
int32_t OSThread::disable()
{
enabled = false;
setInterval(INT32_MAX);
return INT32_MAX;
}
/**
* 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.
*/
bool hasBeenSetup;
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);
}
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/OSThread.h
+91
View File
@@ -0,0 +1,91 @@
#pragma once
#include <cstdlib>
#include <stdint.h>
#include "Thread.h"
#include "ThreadController.h"
#include "concurrency/InterruptableDelay.h"
namespace concurrency
{
extern ThreadController mainController, timerController;
extern InterruptableDelay mainDelay;
#define RUN_SAME -1
/**
* @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.
*
* TODO FIXME @geeksville
*
* move more things into OSThreads
* remove lock/lockguard
*
* move typedQueue into concurrency
* remove freertos from typedqueue
*/
class OSThread : public Thread
{
ThreadController *controller;
/// 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 we decide not to run;
static bool showWaiting;
public:
/// For debug printing only (might be null)
static const OSThread *currentThread;
OSThread(const char *name, uint32_t period = 0, ThreadController *controller = &mainController);
virtual ~OSThread();
virtual bool shouldRun(unsigned long time);
static void setup();
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);
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();
};
/**
* 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.
*/
extern bool hasBeenSetup;
void assertIsSetup();
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/concurrency/Periodic.h
+24
View File
@@ -0,0 +1,24 @@
#pragma once
#include "concurrency/OSThread.h"
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)();
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(); }
};
} // namespace concurrency
code/firmware-2.7.15.567b8ea/src/configuration.h
+487
View File
@@ -0,0 +1,487 @@
/*
TTGO T-BEAM Tracker for The Things Network
Copyright (C) 2018 by Xose Pérez <xose dot perez at gmail dot com>
This code requires LMIC library by Matthijs Kooijman
https://github.com/matthijskooijman/arduino-lmic
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <Arduino.h>
#if __has_include("Melopero_RV3028.h")
#include "Melopero_RV3028.h"
#endif
#if __has_include("pcf8563.h")
#include "pcf8563.h"
#endif
/* Offer chance for variant-specific defines */
#include "variant.h"
// -----------------------------------------------------------------------------
// Version
// -----------------------------------------------------------------------------
// If app version is not specified we assume we are not being invoked by the build script
#ifndef APP_VERSION
#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.
#ifndef HW_VERSION
#define HW_VERSION "1.0"
#endif
// -----------------------------------------------------------------------------
// Configuration
// -----------------------------------------------------------------------------
/// Convert a preprocessor name into a quoted string
#define xstr(s) ystr(s)
#define ystr(s) #s
/// Convert a preprocessor name into a quoted string and if that string is empty use "unset"
#define optstr(s) (xstr(s)[0] ? xstr(s) : "unset")
// Nop definition for these attributes that are specific to ESP32
#ifndef EXT_RAM_ATTR
#define EXT_RAM_ATTR
#endif
#ifndef IRAM_ATTR
#define IRAM_ATTR
#endif
#ifndef RTC_DATA_ATTR
#define RTC_DATA_ATTR
#endif
#ifndef EXT_RAM_BSS_ATTR
#define EXT_RAM_BSS_ATTR EXT_RAM_ATTR
#endif
// -----------------------------------------------------------------------------
// Regulatory overrides
// -----------------------------------------------------------------------------
// Override user saved region, for producing region-locked builds
// #define REGULATORY_LORA_REGIONCODE meshtastic_Config_LoRaConfig_RegionCode_SG_923
// Total system gain in dBm to subtract from Tx power to remain within regulatory and Tx PA limits
// The value consists of PA gain + antenna gain (if variant has a non-removable antenna)
// TX_GAIN_LORA should be set with definitions below for common modules, or in variant.h.
// Gain for common modules with transmit PAs
#ifdef EBYTE_E22_900M30S
// 10dB PA gain and 30dB rated output; based on measurements from
// https://github.com/S5NC/EBYTE_ESP32-S3/blob/main/E22-900M30S%20power%20output%20testing.txt
#define TX_GAIN_LORA 7
#define SX126X_MAX_POWER 22
#endif
#ifdef EBYTE_E22_900M33S
// 25dB PA gain and 33dB rated output; based on TX Power Curve from E22-900M33S_UserManual_EN_v1.0.pdf
#define TX_GAIN_LORA 25
#define SX126X_MAX_POWER 8
#endif
#ifdef NICERF_MINIF27
// Note that datasheet power level of 9 corresponds with SX1262 at 22dBm
// Maximum output power of 29dBm with VCC_PA = 5V
#define TX_GAIN_LORA 7
#define SX126X_MAX_POWER 22
#endif
#ifdef NICERF_F30_HF
// Maximum output power of 29.6dBm with VCC = 5V and SX1262 at 22dBm
#define TX_GAIN_LORA 8
#define SX126X_MAX_POWER 22
#endif
#ifdef NICERF_F30_LF
// Maximum output power of 32.0dBm with VCC = 5V and SX1262 at 22dBm
#define TX_GAIN_LORA 10
#define SX126X_MAX_POWER 22
#endif
#ifdef USE_GC1109_PA
// Power Amps are often non-linear, so we can use an array of values for the power curve
#define NUM_PA_POINTS 22
#define TX_GAIN_LORA 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 10, 10, 9, 9, 8, 7
#endif
#ifdef RAK13302
#define NUM_PA_POINTS 22
#define TX_GAIN_LORA 7, 8, 8, 8, 8, 8, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 9, 8
#endif
// Default system gain to 0 if not defined
#ifndef TX_GAIN_LORA
#define TX_GAIN_LORA 0
#endif
// -----------------------------------------------------------------------------
// Feature toggles
// -----------------------------------------------------------------------------
// Disable use of the NTP library and related features
// #define DISABLE_NTP
// Disable the welcome screen and allow
// #define DISABLE_WELCOME_UNSET
// -----------------------------------------------------------------------------
// OLED & Input
// -----------------------------------------------------------------------------
#if defined(SEEED_WIO_TRACKER_L1) && !defined(SEEED_WIO_TRACKER_L1_EINK)
#define SSD1306_ADDRESS 0x3D
#define USE_SH1106
#else
#define SSD1306_ADDRESS 0x3C
#endif
#define ST7567_ADDRESS 0x3F
// The SH1106 controller is almost, but not quite, the same as SSD1306
// Define this if you know you have that controller or your "SSD1306" misbehaves.
// #define USE_SH1106
// Define if screen should be mirrored left to right
// #define SCREEN_MIRROR
// I2C Keyboards (M5Stack, RAK14004, T-Deck, T-Deck Pro, T-Lora Pager, CardKB, BBQ10, MPR121, TCA8418)
#define CARDKB_ADDR 0x5F
#define TDECK_KB_ADDR 0x55
#define BBQ10_KB_ADDR 0x1F
#define MPR121_KB_ADDR 0x5A
#define TCA8418_KB_ADDR 0x34
// -----------------------------------------------------------------------------
// SENSOR
// -----------------------------------------------------------------------------
#define BME_ADDR 0x76
#define BME_ADDR_ALTERNATE 0x77
#define MCP9808_ADDR 0x18
#define INA_ADDR 0x40
#define INA_ADDR_ALTERNATE 0x41
#define INA_ADDR_WAVESHARE_UPS 0x43
#define INA3221_ADDR 0x42
#define MAX1704X_ADDR 0x36
#define QMC6310_ADDR 0x1C
#define QMI8658_ADDR 0x6B
#define QMC5883L_ADDR 0x0D
#define HMC5883L_ADDR 0x1E
#define SHTC3_ADDR 0x70
#define LPS22HB_ADDR 0x5C
#define LPS22HB_ADDR_ALT 0x5D
#define SHT31_4x_ADDR 0x44
#define SHT31_4x_ADDR_ALT 0x45
#define PMSA0031_ADDR 0x12
#define QMA6100P_ADDR 0x12
#define AHT10_ADDR 0x38
#define RCWL9620_ADDR 0x57
#define VEML7700_ADDR 0x10
#define TSL25911_ADDR 0x29
#define OPT3001_ADDR 0x45
#define OPT3001_ADDR_ALT 0x44
#define MLX90632_ADDR 0x3A
#define DFROBOT_LARK_ADDR 0x42
#define DFROBOT_RAIN_ADDR 0x1d
#define NAU7802_ADDR 0x2A
#define MAX30102_ADDR 0x57
#define SCD4X_ADDR 0x62
#define MLX90614_ADDR_DEF 0x5A
#define CGRADSENS_ADDR 0x66
#define LTR390UV_ADDR 0x53
#define XPOWERS_AXP192_AXP2101_ADDRESS 0x34 // same adress as TCA8418_KB
#define PCT2075_ADDR 0x37
#define BQ27220_ADDR 0x55 // same address as TDECK_KB
#define BQ25896_ADDR 0x6B
#define LTR553ALS_ADDR 0x23
// -----------------------------------------------------------------------------
// ACCELEROMETER
// -----------------------------------------------------------------------------
#define MPU6050_ADDR 0x68
#define STK8BXX_ADDR 0x18
#define LIS3DH_ADDR 0x18
#define LIS3DH_ADDR_ALT 0x19
#define BMA423_ADDR 0x19
#define LSM6DS3_ADDR 0x6A
#define BMX160_ADDR 0x69
#define ICM20948_ADDR 0x69
#define ICM20948_ADDR_ALT 0x68
#define BHI260AP_ADDR 0x28
#define BMM150_ADDR 0x13
#define DA217_ADDR 0x26
// -----------------------------------------------------------------------------
// LED
// -----------------------------------------------------------------------------
#define NCP5623_ADDR 0x38
#define LP5562_ADDR 0x30
// -----------------------------------------------------------------------------
// Security
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
// IO Expander
// -----------------------------------------------------------------------------
#define TCA9535_ADDR 0x20
#define TCA9555_ADDR 0x26
// -----------------------------------------------------------------------------
// Touchscreen
// -----------------------------------------------------------------------------
#define FT6336U_ADDR 0x48
#define CST328_ADDR 0x1A
#define CHSC6X_ADDR 0x2E
// -----------------------------------------------------------------------------
// RAK12035VB Soil Monitor (using RAK12023 up to 3 RAK12035 monitors can be connected)
// - the default i2c address for this sensor is 0x20, and users are instructed to
// set 0x21 and 0x22 for the second and third sensor if present.
// -----------------------------------------------------------------------------
#define RAK120351_ADDR 0x20
#define RAK120352_ADDR 0x21
#define RAK120353_ADDR 0x22
// -----------------------------------------------------------------------------
// BIAS-T Generator
// -----------------------------------------------------------------------------
#define TPS65233_ADDR 0x60
// convert 24-bit color to 16-bit (56K)
#define COLOR565(r, g, b) (((r & 0xF8) << 8) | ((g & 0xFC) << 3) | ((b & 0xF8) >> 3))
#if defined(VEXT_ENABLE) && !defined(VEXT_ON_VALUE)
// Older variant.h files might not be defining this value, so stay with the old default
#define VEXT_ON_VALUE LOW
#endif
// -----------------------------------------------------------------------------
// Rotary encoder
// -----------------------------------------------------------------------------
#ifndef ROTARY_DELAY
#define ROTARY_DELAY 5
#endif
// -----------------------------------------------------------------------------
// GPS
// -----------------------------------------------------------------------------
#ifndef GPS_BAUDRATE
#define GPS_BAUDRATE 9600
#define GPS_BAUDRATE_FIXED 0
#else
#define GPS_BAUDRATE_FIXED 1
#endif
#ifndef GPS_THREAD_INTERVAL
#define GPS_THREAD_INTERVAL 200
#endif
/* Step #2: follow with defines common to the architecture;
also enable HAS_ option not specifically disabled by variant.h */
#include "architecture.h"
#ifndef DEFAULT_REBOOT_SECONDS
#define DEFAULT_REBOOT_SECONDS 7
#endif
#ifndef DEFAULT_SHUTDOWN_SECONDS
#define DEFAULT_SHUTDOWN_SECONDS 2
#endif
#ifndef MINIMUM_SAFE_FREE_HEAP
#define MINIMUM_SAFE_FREE_HEAP 1500
#endif
#ifndef WIRE_INTERFACES_COUNT
// Officially an NRF52 macro
// Repurposed cross-platform to identify devices using Wire1
#if defined(I2C_SDA1) || defined(PIN_WIRE1_SDA)
#define WIRE_INTERFACES_COUNT 2
#elif HAS_WIRE
#define WIRE_INTERFACES_COUNT 1
#endif
#endif
/* Step #3: mop up with disabled values for HAS_ options not handled by the above two */
#ifndef HAS_WIFI
#define HAS_WIFI 0
#endif
#ifndef HAS_ETHERNET
#define HAS_ETHERNET 0
#endif
#ifndef HAS_SCREEN
#define HAS_SCREEN 0
#endif
#ifndef HAS_TFT
#define HAS_TFT 0
#endif
#ifndef HAS_WIRE
#define HAS_WIRE 0
#endif
#ifndef HAS_GPS
#define HAS_GPS 0
#endif
#ifndef HAS_BUTTON
#define HAS_BUTTON 0
#endif
#ifndef HAS_TRACKBALL
#define HAS_TRACKBALL 0
#endif
#ifndef HAS_TOUCHSCREEN
#define HAS_TOUCHSCREEN 0
#endif
#ifndef HAS_TELEMETRY
#define HAS_TELEMETRY 0
#endif
#ifndef HAS_SENSOR
#define HAS_SENSOR 0
#endif
#ifndef HAS_RADIO
#define HAS_RADIO 0
#endif
#ifndef HAS_RTC
#define HAS_RTC 0
#endif
#ifndef HAS_CPU_SHUTDOWN
#define HAS_CPU_SHUTDOWN 0
#endif
#ifndef HAS_BLUETOOTH
#define HAS_BLUETOOTH 0
#endif
#ifndef HW_VENDOR
#error HW_VENDOR must be defined
#endif
#ifndef TB_DOWN
#define TB_DOWN 255
#endif
#ifndef TB_UP
#define TB_UP 255
#endif
#ifndef TB_LEFT
#define TB_LEFT 255
#endif
#ifndef TB_RIGHT
#define TB_RIGHT 255
#endif
#ifndef TB_PRESS
#define TB_PRESS 255
#endif
// Support multiple RGB LED configuration
#if defined(HAS_NCP5623) || defined(HAS_LP5562) || defined(RGBLED_RED) || defined(HAS_NEOPIXEL) || defined(UNPHONE)
#define HAS_RGB_LED
#endif
// default mapping of pins
#if defined(PIN_BUTTON2) && !defined(CANCEL_BUTTON_PIN)
#define ALT_BUTTON_PIN PIN_BUTTON2
#endif
#if defined ALT_BUTTON_PIN
#ifndef ALT_BUTTON_ACTIVE_LOW
#define ALT_BUTTON_ACTIVE_LOW true
#endif
#ifndef ALT_BUTTON_ACTIVE_PULLUP
#define ALT_BUTTON_ACTIVE_PULLUP true
#endif
#endif
// -----------------------------------------------------------------------------
// Global switches to turn off features for a minimized build
// -----------------------------------------------------------------------------
// #define MESHTASTIC_MINIMIZE_BUILD 1
#ifdef MESHTASTIC_MINIMIZE_BUILD
#define MESHTASTIC_EXCLUDE_MODULES 1
#define MESHTASTIC_EXCLUDE_WIFI 1
#define MESHTASTIC_EXCLUDE_BLUETOOTH 1
#define MESHTASTIC_EXCLUDE_GPS 1
#define MESHTASTIC_EXCLUDE_SCREEN 1
#define MESHTASTIC_EXCLUDE_MQTT 1
#define MESHTASTIC_EXCLUDE_POWERMON 1
#define MESHTASTIC_EXCLUDE_I2C 1
#define MESHTASTIC_EXCLUDE_PKI 1
#define MESHTASTIC_EXCLUDE_POWER_FSM 1
#define MESHTASTIC_EXCLUDE_TZ 1
#endif
// Turn off all optional modules
#ifdef MESHTASTIC_EXCLUDE_MODULES
#define MESHTASTIC_EXCLUDE_AUDIO 1
#define MESHTASTIC_EXCLUDE_DETECTIONSENSOR 1
#define MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR 1
#define MESHTASTIC_EXCLUDE_HEALTH_TELEMETRY 1
#define MESHTASTIC_EXCLUDE_EXTERNALNOTIFICATION 1
#define MESHTASTIC_EXCLUDE_PAXCOUNTER 1
#define MESHTASTIC_EXCLUDE_POWER_TELEMETRY 1
#define MESHTASTIC_EXCLUDE_RANGETEST 1
#define MESHTASTIC_EXCLUDE_REMOTEHARDWARE 1
#define MESHTASTIC_EXCLUDE_STOREFORWARD 1
#define MESHTASTIC_EXCLUDE_TEXTMESSAGE 1
#define MESHTASTIC_EXCLUDE_ATAK 1
#define MESHTASTIC_EXCLUDE_CANNEDMESSAGES 1
#define MESHTASTIC_EXCLUDE_NEIGHBORINFO 1
#define MESHTASTIC_EXCLUDE_TRACEROUTE 1
#define MESHTASTIC_EXCLUDE_WAYPOINT 1
#define MESHTASTIC_EXCLUDE_INPUTBROKER 1
#define MESHTASTIC_EXCLUDE_SERIAL 1
#define MESHTASTIC_EXCLUDE_POWERSTRESS 1
#define MESHTASTIC_EXCLUDE_ADMIN 1
#endif
// // Turn off wifi even if HW supports wifi (webserver relies on wifi and is also disabled)
#ifdef MESHTASTIC_EXCLUDE_WIFI
#define MESHTASTIC_EXCLUDE_WEBSERVER 1
#undef HAS_WIFI
#define HAS_WIFI 0
#endif
// Allow code that needs internet to just check HAS_NETWORKING rather than HAS_WIFI || HAS_ETHERNET
#define HAS_NETWORKING (HAS_WIFI || HAS_ETHERNET)
// // Turn off Bluetooth
#ifdef MESHTASTIC_EXCLUDE_BLUETOOTH
#undef HAS_BLUETOOTH
#define HAS_BLUETOOTH 0
#endif
// // Turn off GPS
#ifdef MESHTASTIC_EXCLUDE_GPS
#undef HAS_GPS
#define HAS_GPS 0
#undef MESHTASTIC_EXCLUDE_RANGETEST
#define MESHTASTIC_EXCLUDE_RANGETEST 1
#endif
// Turn off Screen
#ifdef MESHTASTIC_EXCLUDE_SCREEN
#undef HAS_SCREEN
#define HAS_SCREEN 0
#endif
#include "DebugConfiguration.h"
#include "RF95Configuration.h"
code/firmware-2.7.15.567b8ea/src/detect/LoRaRadioType.h
+17
View File
@@ -0,0 +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
};
extern LoRaRadioType radioType;
code/firmware-2.7.15.567b8ea/src/detect/ScanI2C.cpp
+89
View File
@@ -0,0 +1,89 @@
#include "ScanI2C.h"
const ScanI2C::DeviceAddress ScanI2C::ADDRESS_NONE = ScanI2C::DeviceAddress();
const ScanI2C::FoundDevice ScanI2C::DEVICE_NONE = ScanI2C::FoundDevice(ScanI2C::DeviceType::NONE, ADDRESS_NONE);
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;
}
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_RX8130CE};
return firstOfOrNONE(3, 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;
}
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));
}
ScanI2C::FoundDevice::FoundDevice(ScanI2C::DeviceType type, ScanI2C::DeviceAddress address) : type(type), address(address) {}
code/firmware-2.7.15.567b8ea/src/detect/ScanI2C.h
+156
View File
@@ -0,0 +1,156 @@
#pragma once
#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_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
} DeviceType;
// 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;
explicit DeviceAddress(I2CPort port, uint8_t address);
DeviceAddress();
bool operator<(const DeviceAddress &other) const;
} DeviceAddress;
static const DeviceAddress ADDRESS_NONE;
typedef uint8_t RegisterAddress;
typedef struct FoundDevice {
DeviceType type;
DeviceAddress address;
explicit FoundDevice(DeviceType = DeviceType::NONE, DeviceAddress = ADDRESS_NONE);
} FoundDevice;
static const FoundDevice DEVICE_NONE;
public:
ScanI2C();
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();
FoundDevice firstScreen() const;
FoundDevice firstRTC() const;
FoundDevice firstKeyboard() const;
FoundDevice firstAccelerometer() const;
FoundDevice firstAQI() const;
FoundDevice firstRGBLED() const;
virtual FoundDevice find(DeviceType) const;
virtual bool exists(DeviceType) const;
virtual size_t countDevices() const;
protected:
virtual FoundDevice firstOfOrNONE(size_t, DeviceType[]) const;
private:
bool shouldSuppressScreen = false;
};
code/firmware-2.7.15.567b8ea/src/detect/ScanI2CConsumer.cpp
+16
View File
@@ -0,0 +1,16 @@
#include "ScanI2CConsumer.h"
#include <forward_list>
static std::forward_list<ScanI2CConsumer *> ScanI2CConsumers;
ScanI2CConsumer::ScanI2CConsumer()
{
ScanI2CConsumers.push_front(this);
}
void ScanI2CCompleted(ScanI2C *i2cScanner)
{
for (ScanI2CConsumer *consumer : ScanI2CConsumers) {
consumer->i2cScanFinished(i2cScanner);
}
}
code/firmware-2.7.15.567b8ea/src/detect/ScanI2CConsumer.h
+13
View File
@@ -0,0 +1,13 @@
#pragma once
#include "ScanI2C.h"
#include <stddef.h>
class ScanI2CConsumer
{
public:
ScanI2CConsumer();
virtual void i2cScanFinished(ScanI2C *i2cScanner) = 0;
};
void ScanI2CCompleted(ScanI2C *i2cScanner);
code/firmware-2.7.15.567b8ea/src/detect/ScanI2CTwoWire.cpp
+640
View File
@@ -0,0 +1,640 @@
#include "ScanI2CTwoWire.h"
#if !MESHTASTIC_EXCLUDE_I2C
#include "concurrency/LockGuard.h"
#if defined(ARCH_PORTDUINO)
#include "linux/LinuxHardwareI2C.h"
#endif
#if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL)
#include "meshUtils.h" // vformat
#endif
bool in_array(uint8_t *array, int size, uint8_t lookfor)
{
int i;
for (i = 0; i < size; i++)
if (lookfor == array[i])
return true;
return false;
}
ScanI2C::FoundDevice ScanI2CTwoWire::find(ScanI2C::DeviceType type) const
{
concurrency::LockGuard guard((concurrency::Lock *)&lock);
return exists(type) ? ScanI2C::FoundDevice(type, deviceAddresses.at(type)) : DEVICE_NONE;
}
bool ScanI2CTwoWire::exists(ScanI2C::DeviceType type) const
{
return deviceAddresses.find(type) != deviceAddresses.end();
}
ScanI2C::FoundDevice ScanI2CTwoWire::firstOfOrNONE(size_t count, DeviceType types[]) const
{
concurrency::LockGuard guard((concurrency::Lock *)&lock);
for (size_t k = 0; k < count; k++) {
ScanI2C::DeviceType current = types[k];
if (exists(current)) {
return ScanI2C::FoundDevice(current, deviceAddresses.at(current));
}
}
return DEVICE_NONE;
}
ScanI2C::DeviceType ScanI2CTwoWire::probeOLED(ScanI2C::DeviceAddress addr) const
{
TwoWire *i2cBus = fetchI2CBus(addr);
uint8_t r = 0;
uint8_t r_prev = 0;
uint8_t c = 0;
ScanI2C::DeviceType o_probe = ScanI2C::DeviceType::SCREEN_UNKNOWN;
do {
r_prev = r;
i2cBus->beginTransmission(addr.address);
i2cBus->write((uint8_t)0x00);
i2cBus->endTransmission();
i2cBus->requestFrom((int)addr.address, 1);
if (i2cBus->available()) {
r = i2cBus->read();
}
r &= 0x0f;
if (r == 0x08 || r == 0x00) {
logFoundDevice("SH1106", (uint8_t)addr.address);
o_probe = SCREEN_SH1106; // SH1106
} else if (r == 0x03 || r == 0x04 || r == 0x06 || r == 0x07) {
logFoundDevice("SSD1306", (uint8_t)addr.address);
o_probe = SCREEN_SSD1306; // SSD1306
}
c++;
} while ((r != r_prev) && (c < 4));
LOG_DEBUG("0x%x subtype probed in %i tries ", r, c);
return o_probe;
}
uint16_t ScanI2CTwoWire::getRegisterValue(const ScanI2CTwoWire::RegisterLocation &registerLocation,
ScanI2CTwoWire::ResponseWidth responseWidth, bool zeropad = false) const
{
uint16_t value = 0x00;
TwoWire *i2cBus = fetchI2CBus(registerLocation.i2cAddress);
i2cBus->beginTransmission(registerLocation.i2cAddress.address);
i2cBus->write(registerLocation.registerAddress);
if (zeropad) {
// Lark Commands need the argument list length in 2 bytes.
i2cBus->write((int)0);
i2cBus->write((int)0);
}
i2cBus->endTransmission();
delay(20);
i2cBus->requestFrom(registerLocation.i2cAddress.address, responseWidth);
if (i2cBus->available() > 1) {
// Read MSB, then LSB
value = (uint16_t)i2cBus->read() << 8;
value |= i2cBus->read();
} else if (i2cBus->available()) {
value = i2cBus->read();
}
// Drain excess bytes
for (uint8_t i = 0; i < responseWidth - 1; i++) {
if (i2cBus->available())
i2cBus->read();
}
LOG_DEBUG("Register value: 0x%x", value);
return value;
}
#define SCAN_SIMPLE_CASE(ADDR, T, ...) \
case ADDR: \
logFoundDevice(__VA_ARGS__); \
type = T; \
break;
void ScanI2CTwoWire::scanPort(I2CPort port, uint8_t *address, uint8_t asize)
{
concurrency::LockGuard guard((concurrency::Lock *)&lock);
LOG_DEBUG("Scan for I2C devices on port %d", port);
uint8_t err;
DeviceAddress addr(port, 0x00);
uint16_t registerValue = 0x00;
ScanI2C::DeviceType type;
TwoWire *i2cBus;
#ifdef RV3028_RTC
Melopero_RV3028 rtc;
#endif
#if WIRE_INTERFACES_COUNT == 2
if (port == I2CPort::WIRE1) {
i2cBus = &Wire1;
} else {
#endif
i2cBus = &Wire;
#if WIRE_INTERFACES_COUNT == 2
}
#endif
// We only need to scan 112 addresses, the rest is reserved for special purposes
// 0x00 General Call
// 0x01 CBUS addresses
// 0x02 Reserved for different bus formats
// 0x03 Reserved for future purposes
// 0x04-0x07 High Speed Master Code
// 0x78-0x7B 10-bit slave addressing
// 0x7C-0x7F Reserved for future purposes
for (addr.address = 8; addr.address < 120; addr.address++) {
if (asize != 0) {
if (!in_array(address, asize, (uint8_t)addr.address))
continue;
LOG_DEBUG("Scan address 0x%x", (uint8_t)addr.address);
}
i2cBus->beginTransmission(addr.address);
#ifdef ARCH_PORTDUINO
err = 2;
if ((addr.address >= 0x30 && addr.address <= 0x37) || (addr.address >= 0x50 && addr.address <= 0x5F)) {
if (i2cBus->read() != -1)
err = 0;
} else {
err = i2cBus->writeQuick((uint8_t)0);
}
if (err != 0)
err = 2;
#else
err = i2cBus->endTransmission();
#endif
type = NONE;
if (err == 0) {
switch (addr.address) {
case SSD1306_ADDRESS:
type = probeOLED(addr);
break;
#ifdef RV3028_RTC
case RV3028_RTC:
// foundDevices[addr] = RTC_RV3028;
type = RTC_RV3028;
logFoundDevice("RV3028", (uint8_t)addr.address);
rtc.initI2C(*i2cBus);
// Update RTC EEPROM settings, if necessary
if (rtc.readEEPROMRegister(0x35) != 0x07) {
rtc.writeEEPROMRegister(0x35, 0x07); // no Clkout
}
if (rtc.readEEPROMRegister(0x37) != 0xB4) {
rtc.writeEEPROMRegister(0x37, 0xB4);
}
break;
#endif
#ifdef PCF8563_RTC
SCAN_SIMPLE_CASE(PCF8563_RTC, RTC_PCF8563, "PCF8563", (uint8_t)addr.address)
#endif
#ifdef RX8130CE_RTC
SCAN_SIMPLE_CASE(RX8130CE_RTC, RTC_RX8130CE, "RX8130CE", (uint8_t)addr.address)
#endif
case CARDKB_ADDR:
// Do we have the RAK14006 instead?
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x04), 1);
if (registerValue == 0x02) {
// KEYPAD_VERSION
logFoundDevice("RAK14004", (uint8_t)addr.address);
type = RAK14004;
} else {
logFoundDevice("M5 cardKB", (uint8_t)addr.address);
type = CARDKB;
}
break;
case TDECK_KB_ADDR:
// Do we have the T-Deck keyboard or the T-Deck Pro battery sensor?
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x04), 1);
if (registerValue != 0) {
logFoundDevice("BQ27220", (uint8_t)addr.address);
type = BQ27220;
} else {
logFoundDevice("TDECKKB", (uint8_t)addr.address);
type = TDECKKB;
}
break;
SCAN_SIMPLE_CASE(BBQ10_KB_ADDR, BBQ10KB, "BB Q10", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(ST7567_ADDRESS, SCREEN_ST7567, "ST7567", (uint8_t)addr.address);
#ifdef HAS_NCP5623
SCAN_SIMPLE_CASE(NCP5623_ADDR, NCP5623, "NCP5623", (uint8_t)addr.address);
#endif
#ifdef HAS_LP5562
SCAN_SIMPLE_CASE(LP5562_ADDR, LP5562, "LP5562", (uint8_t)addr.address);
#endif
case XPOWERS_AXP192_AXP2101_ADDRESS:
// Do we have the axp2101/192 or the TCA8418
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x90), 1);
if (registerValue == 0x0) {
logFoundDevice("TCA8418", (uint8_t)addr.address);
type = TCA8418KB;
} else {
logFoundDevice("AXP192/AXP2101", (uint8_t)addr.address);
type = PMU_AXP192_AXP2101;
}
break;
case BME_ADDR:
case BME_ADDR_ALTERNATE:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0xD0), 1); // GET_ID
switch (registerValue) {
case 0x61:
logFoundDevice("BME680", (uint8_t)addr.address);
type = BME_680;
break;
case 0x60:
logFoundDevice("BME280", (uint8_t)addr.address);
type = BME_280;
break;
case 0x55:
logFoundDevice("BMP085/BMP180", (uint8_t)addr.address);
type = BMP_085;
break;
case 0x00:
// do we have a DPS310 instead?
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x0D), 1);
switch (registerValue) {
case 0x10:
logFoundDevice("DPS310", (uint8_t)addr.address);
type = DPS310;
break;
}
break;
default:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1); // GET_ID
switch (registerValue) {
case 0x50: // BMP-388 should be 0x50
logFoundDevice("BMP-388", (uint8_t)addr.address);
type = BMP_3XX;
break;
case 0x60: // BMP-390 should be 0x60
logFoundDevice("BMP-390", (uint8_t)addr.address);
type = BMP_3XX;
break;
case 0x58: // BMP-280 should be 0x58
default:
logFoundDevice("BMP-280", (uint8_t)addr.address);
type = BMP_280;
break;
}
break;
}
break;
#ifndef HAS_NCP5623
case AHT10_ADDR:
logFoundDevice("AHT10", (uint8_t)addr.address);
type = AHT10;
break;
#endif
#if !defined(M5STACK_UNITC6L)
case INA_ADDR:
case INA_ADDR_ALTERNATE:
case INA_ADDR_WAVESHARE_UPS:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0xFE), 2);
LOG_DEBUG("Register MFG_UID: 0x%x", registerValue);
if (registerValue == 0x5449) {
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0xFF), 2);
LOG_DEBUG("Register DIE_UID: 0x%x", registerValue);
if (registerValue == 0x2260) {
logFoundDevice("INA226", (uint8_t)addr.address);
type = INA226;
} else {
logFoundDevice("INA260", (uint8_t)addr.address);
type = INA260;
}
} else { // Assume INA219 if INA260 ID is not found
logFoundDevice("INA219", (uint8_t)addr.address);
type = INA219;
}
break;
case INA3221_ADDR:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0xFE), 2);
LOG_DEBUG("Register MFG_UID FE: 0x%x", registerValue);
if (registerValue == 0x5449) {
logFoundDevice("INA3221", (uint8_t)addr.address);
type = INA3221;
} else {
/* check the first 2 bytes of the 6 byte response register
LARK FW 1.0 should return:
RESPONSE_STATUS STATUS_SUCCESS (0x53)
RESPONSE_CMD CMD_GET_VERSION (0x05)
RESPONSE_LEN_L 0x02
RESPONSE_LEN_H 0x00
RESPONSE_PAYLOAD 0x01
RESPONSE_PAYLOAD+1 0x00
*/
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x05), 6, true);
LOG_DEBUG("Register MFG_UID 05: 0x%x", registerValue);
if (registerValue == 0x5305) {
logFoundDevice("DFRobot Lark", (uint8_t)addr.address);
type = DFROBOT_LARK;
}
// else: probably a RAK12500/UBLOX GPS on I2C
}
break;
#endif
case MCP9808_ADDR:
// We need to check for STK8BAXX first, since register 0x07 is new data flag for the z-axis and can produce some
// weird result. and register 0x00 doesn't seems to be colliding with MCP9808 and LIS3DH chips.
{
#ifdef HAS_STK8XXX
// Check register 0x00 for 0x8700 response to ID STK8BA53 chip.
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 2);
if (registerValue == 0x8700) {
type = STK8BAXX;
logFoundDevice("STK8BAXX", (uint8_t)addr.address);
break;
}
#endif
// Check register 0x07 for 0x0400 response to ID MCP9808 chip.
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x07), 2);
if (registerValue == 0x0400) {
type = MCP9808;
logFoundDevice("MCP9808", (uint8_t)addr.address);
break;
}
// Check register 0x0F for 0x3300 response to ID LIS3DH chip.
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x0F), 2);
if (registerValue == 0x3300 || registerValue == 0x3333) { // RAK4631 WisBlock has LIS3DH register at 0x3333
type = LIS3DH;
logFoundDevice("LIS3DH", (uint8_t)addr.address);
}
break;
}
case SHT31_4x_ADDR: // same as OPT3001_ADDR_ALT
case SHT31_4x_ADDR_ALT: // same as OPT3001_ADDR
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x7E), 2);
if (registerValue == 0x5449) {
type = OPT3001;
logFoundDevice("OPT3001", (uint8_t)addr.address);
} else if (getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x89), 2) != 0) { // unique SHT4x serial number
type = SHT4X;
logFoundDevice("SHT4X", (uint8_t)addr.address);
} else {
type = SHT31;
logFoundDevice("SHT31", (uint8_t)addr.address);
}
break;
SCAN_SIMPLE_CASE(SHTC3_ADDR, SHTC3, "SHTC3", (uint8_t)addr.address)
case RCWL9620_ADDR:
// get MAX30102 PARTID
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0xFF), 1);
if (registerValue == 0x15) {
type = MAX30102;
logFoundDevice("MAX30102", (uint8_t)addr.address);
break;
} else {
type = RCWL9620;
logFoundDevice("RCWL9620", (uint8_t)addr.address);
}
break;
case LPS22HB_ADDR_ALT:
SCAN_SIMPLE_CASE(LPS22HB_ADDR, LPS22HB, "LPS22HB", (uint8_t)addr.address)
SCAN_SIMPLE_CASE(QMC6310_ADDR, QMC6310, "QMC6310", (uint8_t)addr.address)
case QMI8658_ADDR:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x0A), 1); // get ID
if (registerValue == 0xC0) {
type = BQ24295;
logFoundDevice("BQ24295", (uint8_t)addr.address);
break;
}
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x14), 1); // get ID
if ((registerValue & 0b00000011) == 0b00000010) {
type = BQ25896;
logFoundDevice("BQ25896", (uint8_t)addr.address);
break;
}
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x0F), 1); // get ID
if (registerValue == 0x6A) {
type = LSM6DS3;
logFoundDevice("LSM6DS3", (uint8_t)addr.address);
} else {
type = QMI8658;
logFoundDevice("QMI8658", (uint8_t)addr.address);
}
break;
SCAN_SIMPLE_CASE(QMC5883L_ADDR, QMC5883L, "QMC5883L", (uint8_t)addr.address)
SCAN_SIMPLE_CASE(HMC5883L_ADDR, HMC5883L, "HMC5883L", (uint8_t)addr.address)
#ifdef HAS_QMA6100P
SCAN_SIMPLE_CASE(QMA6100P_ADDR, QMA6100P, "QMA6100P", (uint8_t)addr.address)
#else
SCAN_SIMPLE_CASE(PMSA0031_ADDR, PMSA0031, "PMSA0031", (uint8_t)addr.address)
#endif
case BMA423_ADDR: // this can also be LIS3DH_ADDR_ALT
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x0F), 2);
if (registerValue == 0x3300 || registerValue == 0x3333) { // RAK4631 WisBlock has LIS3DH register at 0x3333
type = LIS3DH;
logFoundDevice("LIS3DH", (uint8_t)addr.address);
} else {
type = BMA423;
logFoundDevice("BMA423", (uint8_t)addr.address);
}
break;
case TCA9535_ADDR:
case RAK120352_ADDR:
case RAK120353_ADDR:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x02), 1);
if (registerValue == addr.address) { // RAK12035 returns its I2C address at 0x02 (eg 0x20)
type = RAK12035;
logFoundDevice("RAK12035", (uint8_t)addr.address);
} else {
type = TCA9535;
logFoundDevice("TCA9535", (uint8_t)addr.address);
}
break;
SCAN_SIMPLE_CASE(LSM6DS3_ADDR, LSM6DS3, "LSM6DS3", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(VEML7700_ADDR, VEML7700, "VEML7700", (uint8_t)addr.address);
case TCA9555_ADDR:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x01), 1);
if (registerValue == 0x13) {
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1);
if (registerValue == 0x81) {
type = DA217;
logFoundDevice("DA217", (uint8_t)addr.address);
} else {
type = TCA9555;
logFoundDevice("TCA9555", (uint8_t)addr.address);
}
} else {
type = TCA9555;
logFoundDevice("TCA9555", (uint8_t)addr.address);
}
break;
case TSL25911_ADDR:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x12), 1);
if (registerValue == 0x50) {
type = TSL2591;
logFoundDevice("TSL25911", (uint8_t)addr.address);
} else {
type = TSL2561;
logFoundDevice("TSL2561", (uint8_t)addr.address);
}
break;
SCAN_SIMPLE_CASE(MLX90632_ADDR, MLX90632, "MLX90632", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(NAU7802_ADDR, NAU7802, "NAU7802", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(MAX1704X_ADDR, MAX17048, "MAX17048", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(DFROBOT_RAIN_ADDR, DFROBOT_RAIN, "DFRobot Rain Gauge", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(LTR390UV_ADDR, LTR390UV, "LTR390UV", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(PCT2075_ADDR, PCT2075, "PCT2075", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(CST328_ADDR, CST328, "CST328", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(CHSC6X_ADDR, CHSC6X, "CHSC6X", (uint8_t)addr.address);
case LTR553ALS_ADDR:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x86), 1); // Part ID register
if (registerValue == 0x92) { // LTR553ALS Part ID
type = LTR553ALS;
logFoundDevice("LTR553ALS", (uint8_t)addr.address);
} else {
// Test BH1750 - send power on command
i2cBus->beginTransmission(addr.address);
i2cBus->write(0x01); // Power On command
uint8_t bh1750_error = i2cBus->endTransmission();
if (bh1750_error == 0) {
type = BH1750;
logFoundDevice("BH1750", (uint8_t)addr.address);
} else {
LOG_INFO("Device found at address 0x%x was not able to be enumerated", (uint8_t)addr.address);
}
}
break;
SCAN_SIMPLE_CASE(BHI260AP_ADDR, BHI260AP, "BHI260AP", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(SCD4X_ADDR, SCD4X, "SCD4X", (uint8_t)addr.address);
SCAN_SIMPLE_CASE(BMM150_ADDR, BMM150, "BMM150", (uint8_t)addr.address);
#ifdef HAS_TPS65233
SCAN_SIMPLE_CASE(TPS65233_ADDR, TPS65233, "TPS65233", (uint8_t)addr.address);
#endif
case MLX90614_ADDR_DEF:
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x0e), 1);
if (registerValue == 0x5a) {
type = MLX90614;
logFoundDevice("MLX90614", (uint8_t)addr.address);
} else {
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1); // DRV2605_REG_STATUS
if (registerValue == 0xe0) {
type = DRV2605;
logFoundDevice("DRV2605", (uint8_t)addr.address);
} else {
type = MPR121KB;
logFoundDevice("MPR121KB", (uint8_t)addr.address);
}
}
break;
case ICM20948_ADDR: // same as BMX160_ADDR
case ICM20948_ADDR_ALT: // same as MPU6050_ADDR
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1);
if (registerValue == 0xEA) {
type = ICM20948;
logFoundDevice("ICM20948", (uint8_t)addr.address);
break;
} else if (addr.address == BMX160_ADDR) {
type = BMX160;
logFoundDevice("BMX160", (uint8_t)addr.address);
break;
} else {
type = MPU6050;
logFoundDevice("MPU6050", (uint8_t)addr.address);
break;
}
break;
case CGRADSENS_ADDR:
// Register 0x00 of the RadSens sensor contains is product identifier 0x7D
// Undocumented, but some devices return a product identifier of 0x7A
registerValue = getRegisterValue(ScanI2CTwoWire::RegisterLocation(addr, 0x00), 1);
if (registerValue == 0x7D || registerValue == 0x7A) {
type = CGRADSENS;
logFoundDevice("ClimateGuard RadSens", (uint8_t)addr.address);
break;
} else {
LOG_DEBUG("Unexpected Device ID for RadSense: addr=0x%x id=0x%x", CGRADSENS_ADDR, registerValue);
}
break;
case 0x48: {
i2cBus->beginTransmission(addr.address);
uint8_t getInfo[] = {0x5A, 0xC0, 0x00, 0xFF, 0xFC};
uint8_t expectedInfo[] = {0xa5, 0xE0, 0x00, 0x3F, 0x19};
uint8_t info[5];
size_t len = 0;
i2cBus->write(getInfo, 5);
i2cBus->endTransmission();
len = i2cBus->readBytes(info, 5);
if (len == 5 && memcmp(expectedInfo, info, len) == 0) {
LOG_INFO("NXP SE050 crypto chip found");
type = NXP_SE050;
} else {
LOG_INFO("FT6336U touchscreen found");
type = FT6336U;
}
break;
}
default:
LOG_INFO("Device found at address 0x%x was not able to be enumerated", (uint8_t)addr.address);
}
} else if (err == 4) {
LOG_ERROR("Unknown error at address 0x%x", (uint8_t)addr.address);
}
// Check if a type was found for the enumerated device - save, if so
if (type != NONE) {
deviceAddresses[type] = addr;
foundDevices[addr] = type;
}
}
}
void ScanI2CTwoWire::scanPort(I2CPort port)
{
scanPort(port, nullptr, 0);
}
TwoWire *ScanI2CTwoWire::fetchI2CBus(ScanI2C::DeviceAddress address)
{
if (address.port == ScanI2C::I2CPort::WIRE) {
return &Wire;
} else {
#if WIRE_INTERFACES_COUNT == 2
return &Wire1;
#else
return &Wire;
#endif
}
}
size_t ScanI2CTwoWire::countDevices() const
{
return foundDevices.size();
}
void ScanI2CTwoWire::logFoundDevice(const char *device, uint8_t address)
{
LOG_INFO("%s found at address 0x%x", device, address);
}
#endif
code/firmware-2.7.15.567b8ea/src/detect/ScanI2CTwoWire.h
+62
View File
@@ -0,0 +1,62 @@
#pragma once
#include "configuration.h"
#if !MESHTASTIC_EXCLUDE_I2C
#include <map>
#include <memory>
#include <stddef.h>
#include <stdint.h>
#include <Wire.h>
#include "ScanI2C.h"
#include "../concurrency/Lock.h"
class ScanI2CTwoWire : public ScanI2C
{
public:
void scanPort(ScanI2C::I2CPort) override;
void scanPort(ScanI2C::I2CPort, uint8_t *, uint8_t) override;
ScanI2C::FoundDevice find(ScanI2C::DeviceType) const override;
bool exists(ScanI2C::DeviceType) const override;
size_t countDevices() const override;
static TwoWire *fetchI2CBus(ScanI2C::DeviceAddress);
protected:
FoundDevice firstOfOrNONE(size_t, DeviceType[]) const override;
private:
typedef struct RegisterLocation {
DeviceAddress i2cAddress;
RegisterAddress registerAddress;
RegisterLocation(DeviceAddress deviceAddress, RegisterAddress registerAddress)
: i2cAddress(deviceAddress), registerAddress(registerAddress)
{
}
} RegisterLocation;
typedef uint8_t ResponseWidth;
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;
concurrency::Lock lock;
uint16_t getRegisterValue(const RegisterLocation &, ResponseWidth, bool) const;
DeviceType probeOLED(ScanI2C::DeviceAddress) const;
static void logFoundDevice(const char *device, uint8_t address);
};
#endif
code/firmware-2.7.15.567b8ea/src/detect/einkScan.h
+67
View File
@@ -0,0 +1,67 @@
#include "../configuration.h"
#ifdef RAK_4631
#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_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;
}
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
code/firmware-2.7.15.567b8ea/src/error.h
+12
View File
@@ -0,0 +1,12 @@
#pragma once
#include <Arduino.h>
#include "mesh/generated/meshtastic/mesh.pb.h" // For CriticalErrorCode
/// A macro that include filename and line
#define RECORD_CRITICALERROR(code) recordCriticalError(code, __LINE__, __FILE__)
/// Record an error that should be reported via analytics
void recordCriticalError(meshtastic_CriticalErrorCode code = meshtastic_CriticalErrorCode_UNSPECIFIED, uint32_t address = 0,
const char *filename = NULL);
code/firmware-2.7.15.567b8ea/src/freertosinc.h
+47
View File
@@ -0,0 +1,47 @@
#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
#if defined(ARDUINO_ARCH_ESP32)
#define HAS_FREE_RTOS
#include <freertos/FreeRTOS.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>
#include <freertos/task.h>
#endif
#if defined(ARDUINO_NRF52_ADAFRUIT) || defined(ARDUINO_ARCH_RP2040)
#define HAS_FREE_RTOS
#include <FreeRTOS.h>
#include <queue.h>
#include <semphr.h>
#include <task.h>
#endif
#ifdef HAS_FREE_RTOS
// Include real FreeRTOS defs above
#else
// Include placeholder fake FreeRTOS defs
#include <Arduino.h>
typedef uint32_t TickType_t;
typedef uint32_t BaseType_t;
#define portMAX_DELAY UINT32_MAX
#define tskIDLE_PRIORITY 0
#define configMAX_PRIORITIES 10 // Highest priority level
// Don't do anything on non free rtos platforms when done with the ISR
#define portYIELD_FROM_ISR(x)
enum eNotifyAction { eNoAction, eSetValueWithoutOverwrite, eSetValueWithOverwrite };
#endif
code/firmware-2.7.15.567b8ea/src/gps/GPS.cpp
+1906
View File
File diff suppressed because it is too large Load Diff
code/firmware-2.7.15.567b8ea/src/gps/GPS.h
+253
View File
@@ -0,0 +1,253 @@
#pragma once
#include "configuration.h"
#if !MESHTASTIC_EXCLUDE_GPS
#include "GPSStatus.h"
#include "GpioLogic.h"
#include "Observer.h"
#include "TinyGPS++.h"
#include "concurrency/OSThread.h"
#include "input/RotaryEncoderInterruptImpl1.h"
#include "input/UpDownInterruptImpl1.h"
#include "modules/PositionModule.h"
// Allow defining the polarity of the ENABLE output. default is active high
#ifndef GPS_EN_ACTIVE
#define GPS_EN_ACTIVE 1
#endif
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
} GnssModel_t;
typedef enum {
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
};
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
};
/**
* 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;
/** 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();
/** We will notify this observable anytime GPS state has changed meaningfully */
Observable<const meshtastic::GPSStatus *> newStatus;
/**
* Returns true if we succeeded
*/
virtual bool setup();
// re-enable the thread
void enable();
// Disable the thread
int32_t disable() override;
// 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);
/// 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();
/// 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; }
// 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();
// Wake the GPS hardware - ready for an update
void up();
// Let the GPS hardware save power between updates
void down();
private:
GPS() : concurrency::OSThread("GPS") {}
/// 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();
/**
* 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();
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;
#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
#endif
uint32_t fixHoldEnds = 0;
uint32_t rx_gpio = 0;
uint32_t tx_gpio = 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
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 GPSInitFinished = false; // Init thread finished?
bool GPSInitStarted = false; // Init thread finished?
GPSPowerState powerState = GPS_OFF; // GPS_ACTIVE if we want a location right now
uint8_t numSatellites = 0;
CallbackObserver<GPS, void *> notifyDeepSleepObserver = CallbackObserver<GPS, void *>(this, &GPS::prepareDeepSleep);
/** If !NULL we will use this serial port to construct our GPS */
#if defined(ARCH_RP2040)
static SerialUART *_serial_gps;
#else
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);
// scratch space for creating ublox packets
uint8_t UBXscratch[250] = {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);
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);
/** 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 GPS power with PMU, if relevant
*/
void setPowerPMU(bool on);
/** Set UBLOX power, if relevant
*/
void setPowerUBLOX(bool on, uint32_t sleepMs = 0);
/**
* Tell users we have new GPS readings
*/
void publishUpdate();
virtual int32_t runOnce() override;
GnssModel_t getProbeResponse(unsigned long timeout, const std::vector<ChipInfo> &responseMap, 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;
};
extern GPS *gps;
#endif // Exclude GPS
code/firmware-2.7.15.567b8ea/src/gps/GPSUpdateScheduling.cpp
+118
View File
@@ -0,0 +1,118 @@
#include "GPSUpdateScheduling.h"
#include "Default.h"
// Mark the time when searching for GPS position begins
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();
}
// Clear old lock-time prediction data.
// When re-enabling GPS with user button.
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();
// 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;
// If we should have already started (negative value), start ASAP
if (remainingMs < 0)
remainingMs = 0;
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;
// 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);
}
// 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;
// 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;
}
// Updates the predicted time-to-get-lock, by exponentially smoothing the latest observation
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;
// 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;
// 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
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;
}
code/firmware-2.7.15.567b8ea/src/gps/GPSUpdateScheduling.h
+29
View File
@@ -0,0 +1,29 @@
#pragma once
#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
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?
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".
};
code/firmware-2.7.15.567b8ea/src/gps/GeoCoord.cpp
+596
View File
@@ -0,0 +1,596 @@
#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(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::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 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';
double latDeg = lat;
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;
if (lon < 0)
dms.lonCP = 'W';
else
dms.lonCP = 'E';
double lonDeg = lon;
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;
}
/**
* 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)
{
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;
}
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
}
// 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;
}
/**
* 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);
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 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);
}
}
/**
* 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));
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;
}
} else {
lat_val /= pow(5, 5);
lng_val /= pow(4, 5);
}
pos = 10;
for (size_t i = 0; i < 5; i++) { // Compute pair section of code
int lat_ndx = lat_val % 20;
int lng_ndx = lng_val % 20;
tempCode[pos--] = kAlphabet[lng_ndx];
tempCode[pos--] = kAlphabet[lat_ndx];
lat_val /= 20;
lng_val /= 20;
if (i == 0)
tempCode[pos--] = '+';
}
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);
// 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
}
/// 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;
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);
}
/**
* Computes the bearing in degrees between two points on Earth. Ported from my
* old Gaggle android app.
*
* @param lat1
* Latitude of the first point
* @param lon1
* Longitude of the first point
* @param lat2
* Latitude of the second point
* @param lon2
* Longitude of the second point
* @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);
}
/**
* Ported from http://www.edwilliams.org/avform147.htm#Intro
* @brief Convert from meters to range in radians on a great circle
* @param range_meters
* 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;
}
/**
* Ported from http://www.edwilliams.org/avform147.htm#Intro
* @brief Convert from radians to range in meters on a great circle
* @param range_radians
* 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;
}
// 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);
}
// 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);
}
/**
* Create a new point bassed on the passed in poin
* Ported from http://www.edwilliams.org/avform147.htm#LL
* @param bearing
* The bearing in raidans
* @param range_meters
* 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;
return std::make_shared<GeoCoord>(double(lat), double(lon), this->getAltitude());
}
/**
* Convert bearing to degrees
* @param bearing
* 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;
}
/**
* Convert bearing to string
* @param degrees
* 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";
}
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::toDegrees(double r)
{
return r * 180 / PI;
}
code/firmware-2.7.15.567b8ea/src/gps/GeoCoord.h
+148
View File
@@ -0,0 +1,148 @@
#pragma once
#include <algorithm>
#include <cstdint>
#include <cstring>
#include <math.h>
#include <memory>
#include <stdexcept>
#include <stdint.h>
#include <string>
#define PI 3.1415926535897932384626433832795
#define OLC_CODE_LEN 11
#define DEG_CONVERT (180 / PI)
// 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;
};
// 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;
};
// 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;
};
// A struct to hold the data for a OSGR coordinate
struct OSGR {
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
};
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 = {};
bool _dirty = true;
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);
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);
// 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);
// 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; }
// 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; }
// 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
};
code/firmware-2.7.15.567b8ea/src/gps/NMEAWPL.cpp
+102
View File
@@ -0,0 +1,102 @@
#if !MESHTASTIC_EXCLUDE_GPS
#include "NMEAWPL.h"
#include "GeoCoord.h"
#include "RTC.h"
#include <time.h>
/* -------------------------------------------
* 1 2 3 4 5 6
* | | | | | |
* $--WPL,llll.ll,a,yyyyy.yy,a,c--c*hh<CR><LF>
*
* Field Number:
* 1 Latitude
* 2 N or S (North or South)
* 3 Longitude
* 4 E or W (East or West)
* 5 Waypoint name
* 6 Checksum
* -------------------------------------------
*/
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;
}
/* -------------------------------------------
* 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
* | | | | | | | | | | | | | | |
* $--GGA,hhmmss.ss,ddmm.mm,a,ddmm.mm,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh<CR><LF>
*
* Field Number:
* 1 UTC of this position report, hh is hours, mm is minutes, ss.ss is seconds.
* 2 Latitude
* 3 N or S (North or South)
* 4 Longitude
* 5 E or W (East or West)
* 6 GPS Quality Indicator (non null)
* 7 Number of satellites in use, 00 - 12
* 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
* -------------------------------------------
*/
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);
}
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;
}
#endif
code/firmware-2.7.15.567b8ea/src/gps/NMEAWPL.h
+8
View File
@@ -0,0 +1,8 @@
#pragma once
#include "main.h"
#include <Arduino.h>
uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_Position &pos, const char *name, bool isCaltopoMode = false);
uint32_t printWPL(char *buf, size_t bufsz, const meshtastic_PositionLite &pos, const char *name, bool isCaltopoMode = false);
uint32_t printGGA(char *buf, size_t bufsz, const meshtastic_Position &pos);
code/firmware-2.7.15.567b8ea/src/gps/RTC.cpp
+422
View File
@@ -0,0 +1,422 @@
#include "RTC.h"
#include "configuration.h"
#include "detect/ScanI2C.h"
#include "main.h"
#include <Throttle.h>
#include <sys/time.h>
#include <time.h>
static RTCQuality currentQuality = RTCQualityNone;
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;
}
// 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 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*/
#ifdef 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);
#else
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
#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;
}
#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;
}
return RTCSetResultSuccess;
}
#elif defined(PCF8563_RTC)
if (rtc_found.address == PCF8563_RTC) {
uint32_t now = millis();
PCF8563_Class rtc;
#if WIRE_INTERFACES_COUNT == 2
rtc.begin(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else
rtc.begin();
#endif
auto tc = rtc.getDateTime();
tm t;
t.tm_year = tc.year - 1900;
t.tm_mon = tc.month - 1;
t.tm_mday = tc.day;
t.tm_hour = tc.hour;
t.tm_min = tc.minute;
t.tm_sec = tc.second;
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 PCF8563 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);
if (currentQuality == RTCQualityNone) {
timeStartMsec = now;
zeroOffsetSecs = tv.tv_sec;
currentQuality = RTCQualityDevice;
}
return RTCSetResultSuccess;
}
#elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) {
uint32_t now = millis();
ArtronShop_RX8130CE rtc(&Wire);
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;
return RTCSetResultSuccess;
}
#endif
return RTCSetResultNotSet;
}
/**
* Sets the RTC (Real-Time Clock) if the provided time is of higher quality than the current RTC time.
*
* @param q The quality of the provided time.
* @param tv A pointer to a timeval struct containing the time to potentially set the RTC to.
* @return RTCSetResult
*
* 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
#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;
} 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);
}
#elif defined(PCF8563_RTC)
if (rtc_found.address == PCF8563_RTC) {
PCF8563_Class rtc;
#if WIRE_INTERFACES_COUNT == 2
rtc.begin(rtc_found.port == ScanI2C::I2CPort::WIRE1 ? Wire1 : Wire);
#else
rtc.begin();
#endif
tm *t = gmtime(&tv->tv_sec);
rtc.setDateTime(t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec);
LOG_DEBUG("PCF8563_RTC 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);
}
#elif defined(RX8130CE_RTC)
if (rtc_found.address == RX8130CE_RTC) {
ArtronShop_RX8130CE rtc(&Wire);
tm *t = gmtime(&tv->tv_sec);
if (rtc.setTime(*t)) {
LOG_DEBUG("RX8130CE setDateTime %02d-%02d-%02d %02d:%02d:%02d (%ld)", t->tm_year + 1900, t->tm_mon + 1,
t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec, printableEpoch);
} else {
LOG_WARN("Failed to set time for RX8130CE");
}
}
#elif defined(ARCH_ESP32)
settimeofday(tv, NULL);
#endif
// nrf52 doesn't have a readable RTC (yet - software not written)
#if HAS_RTC
readFromRTC();
#endif
return RTCSetResultSuccess;
} else {
return RTCSetResultNotSet; // RTC was already set with a higher quality time
}
}
const char *RtcName(RTCQuality quality)
{
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";
}
}
/**
* Sets the RTC time if the provided time is of higher quality than the current RTC time.
*
* @param q The quality of the provided time.
* @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
#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();
}
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);
}
}
/**
* Returns the timezone offset in seconds.
*
* @return The timezone offset in seconds.
*/
int32_t getTZOffset()
{
#if MESHTASTIC_EXCLUDE_TZ
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));
#endif
}
/**
* Returns the current time in seconds since the Unix epoch (January 1, 1970).
*
* @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;
}
}
/**
* Returns the current time from the RTC if the quality of the time is at least minQuality.
*
* @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;
}
time_t gm_mktime(struct tm *tm)
{
#if !MESHTASTIC_EXCLUDE_TZ
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);
// 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;
}
// 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;
// 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;
#else
return mktime(tm);
#endif
}
code/firmware-2.7.15.567b8ea/src/gps/RTC.h
+64
View File
@@ -0,0 +1,64 @@
#pragma once
#include "configuration.h"
#include "sys/time.h"
#include <Arduino.h>
#ifdef RX8130CE_RTC
#include <ArtronShop_RX8130CE.h>
#endif
enum RTCQuality {
/// We haven't had our RTC set yet
RTCQualityNone = 0,
/// We got time from an onboard peripheral after boot.
RTCQualityDevice = 1,
/// 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 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
};
RTCQuality getRTCQuality();
extern uint32_t lastSetFromPhoneNtpOrGps;
/// 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 = false);
RTCSetResult perhapsSetRTC(RTCQuality q, struct tm &t);
/// Return a string name for the quality
const char *RtcName(RTCQuality quality);
/// Return time since 1970 in secs. While quality is RTCQualityNone we will be returning time based at zero
uint32_t getTime(bool local = false);
/// Return time since 1970 in secs. If quality is RTCQualityNone return zero
uint32_t getValidTime(RTCQuality minQuality, bool local = false);
RTCSetResult readFromRTC();
time_t gm_mktime(struct tm *tm);
#define SEC_PER_DAY 86400
#define SEC_PER_HOUR 3600
#define SEC_PER_MIN 60
#ifdef BUILD_EPOCH
static constexpr uint64_t FORTY_YEARS = (40ULL * 365 * SEC_PER_DAY); // Use 64-bit arithmetic to prevent overflow
#endif
code/firmware-2.7.15.567b8ea/src/gps/cas.h
+63
View File
@@ -0,0 +1,63 @@
#pragma once
// CASIC binary message definitions
// Reference: https://www.icofchina.com/d/file/xiazai/2020-09-22/20f1b42b3a11ac52089caf3603b43fb5.pdf
// ATGM33H-5N: https://www.icofchina.com/pro/mokuai/2016-08-01/4.html
// (https://www.icofchina.com/d/file/xiazai/2016-12-05/b5c57074f4b1fcc62ba8c7868548d18a.pdf)
// NEMA (Class ID - 0x4e) message IDs
#define CAS_NEMA_GGA 0x00
#define CAS_NEMA_GLL 0x01
#define CAS_NEMA_GSA 0x02
#define CAS_NEMA_GSV 0x03
#define CAS_NEMA_RMC 0x04
#define CAS_NEMA_VTG 0x05
#define CAS_NEMA_GST 0x07
#define CAS_NEMA_ZDA 0x08
#define CAS_NEMA_DHV 0x0D
// Size of a CAS-ACK-(N)ACK message (14 bytes)
#define CAS_ACK_NACK_MSG_SIZE 0x0E
// CFG-RST (0x06, 0x02)
// Factory reset
static const uint8_t _message_CAS_CFG_RST_FACTORY[] = {
0xFF, 0x03, // Fields to clear
0x01, // Reset Mode: Controlled Software reset
0x03 // Startup Mode: Factory
};
// CFG_RATE (0x06, 0x01)
// 1HZ update rate, this should always be the case after
// factory reset but update it regardless
static const uint8_t _message_CAS_CFG_RATE_1HZ[] = {
0xE8, 0x03, // Update Rate: 0x03E8 = 1000ms
0x00, 0x00 // Reserved
};
// CFG-NAVX (0x06, 0x07)
// Initial ATGM33H-5N configuration, Updates for Dynamic Mode, Fix Mode, and SV system
// Qwirk: The ATGM33H-5N-31 should only support GPS+BDS, however it will happily enable
// and use GPS+BDS+GLONASS iff the correct CFG_NAVX command is used.
static const uint8_t _message_CAS_CFG_NAVX_CONF[] = {
0x03, 0x01, 0x00, 0x00, // Update Mask: Dynamic Mode, Fix Mode, Nav Settings
0x03, // Dynamic Mode: Automotive
0x03, // Fix Mode: Auto 2D/3D
0x00, // Min SV
0x00, // Max SVs
0x00, // Min CNO
0x00, // Reserved1
0x00, // Init 3D fix
0x00, // Min Elevation
0x00, // Dr Limit
0x07, // Nav System: 2^0 = GPS, 2^1 = BDS 2^2 = GLONASS: 2^3
// 3=GPS+BDS, 7=GPS+BDS+GLONASS
0x00, 0x00, // Rollover Week
0x00, 0x00, 0x00, 0x00, // Fix Altitude
0x00, 0x00, 0x00, 0x00, // Fix Height Error
0x00, 0x00, 0x00, 0x00, // PDOP Maximum
0x00, 0x00, 0x00, 0x00, // TDOP Maximum
0x00, 0x00, 0x00, 0x00, // Position Accuracy Max
0x00, 0x00, 0x00, 0x00, // Time Accuracy Max
0x00, 0x00, 0x00, 0x00 // Static Hold Threshold
};
code/firmware-2.7.15.567b8ea/src/gps/ubx.h
+480
View File
@@ -0,0 +1,480 @@
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)
// Power Management
static uint8_t _message_PMREQ[] PROGMEM = {
0x00, 0x00, 0x00, 0x00, // 4 bytes duration of request task (milliseconds)
0x02, 0x00, 0x00, 0x00 // Bitfield, set backup = 1
};
static uint8_t _message_PMREQ_10[] PROGMEM = {
0x00, // version (0 for this version)
0x00, 0x00, 0x00, // Reserved 1
0x00, 0x00, 0x00, 0x00, // 4 bytes duration of request task (milliseconds)
0x06, 0x00, 0x00, 0x00, // Bitfield, set backup =1 and force =1
0x08, 0x00, 0x00, 0x00 // wakeupSources Wake on uartrx
};
static const uint8_t _message_CFG_RXM_PSM[] PROGMEM = {
0x08, // Reserved
0x01 // Power save mode
};
// only for Neo-6
static const uint8_t _message_CFG_RXM_ECO[] PROGMEM = {
0x08, // Reserved
0x04 // eco mode
};
static const uint8_t _message_CFG_PM2[] PROGMEM = {
0x01, // version
0x00, // Reserved 1, set to 0x06 by u-Center
0x00, // Reserved 2
0x00, // Reserved 1
0x00, 0x11, 0x03, 0x00, // flags-> cyclic mode, wait for normal fix ok, do not wake to update RTC, doNotEnterOff,
// LimitPeakCurrent
0xE8, 0x03, 0x00, 0x00, // update period 1000 ms
0x10, 0x27, 0x00, 0x00, // search period 10s
0x00, 0x00, 0x00, 0x00, // Grid offset 0
0x01, 0x00, // onTime 1 second
0x00, 0x00, // min search time 0
0x00, 0x00, // 0x2C, 0x01, // reserved 4
0x00, 0x00, // 0x00, 0x00, // reserved 5
0x00, 0x00, 0x00, 0x00, // 0x4F, 0xC1, 0x03, 0x00, // reserved 6
0x00, 0x00, 0x00, 0x00, // 0x87, 0x02, 0x00, 0x00, // reserved 7
0x00, // 0xFF, // reserved 8
0x00, // 0x00, // reserved 9
0x00, 0x00, // 0x00, 0x00, // reserved 10
0x00, 0x00, 0x00, 0x00 // 0x64, 0x40, 0x01, 0x00 // reserved 11
};
// Constallation setup, none required for Neo-6
// For Neo-7 GPS & SBAS
static const uint8_t _message_GNSS_7[] = {
0x00, // msgVer (0 for this version)
0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0)
0xff, // numTrkChUse (max number of channels to use, 0xff = max available)
0x02, // numConfigBlocks (number of GNSS systems), most modules support maximum 3 GNSS systems
// GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags
0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x00, 0x01, // GPS
0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x00, 0x01 // SBAS
};
// It's not critical if the module doesn't acknowledge this configuration.
// The module should operate adequately with its factory or previously saved settings.
// It appears that there is a firmware bug in some GPS modules: When an attempt is made
// to overwrite a saved state with identical values, no ACK/NAK is received, contrary to
// what is specified in the Ublox documentation.
// There is also a possibility that the module may be GPS-only.
// For M8 GPS, GLONASS, Galileo, SBAS, QZSS
static const uint8_t _message_GNSS_8[] = {
0x00, // msgVer (0 for this version)
0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0)
0xff, // numTrkChUse (max number of channels to use, 0xff = max available)
0x05, // numConfigBlocks (number of GNSS systems)
// GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags
0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS
0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS
0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo
0x05, 0x00, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // QZSS
0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS
};
/*
// For M8 GPS, GLONASS, BeiDou, SBAS, QZSS
static const uint8_t _message_GNSS_8_B[] = {
0x00, // msgVer (0 for this version)
0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0)
0xff, // numTrkChUse (max number of channels to use, 0xff = max available) read only for protocol >23
0x05, // numConfigBlocks (number of GNSS systems)
// GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags
0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS
0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS
0x03, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // BeiDou
0x05, 0x00, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // QZSS
0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS
};
*/
// For M8 we want to enable NMEA version 4.10 messages to allow for Galileo and or BeiDou
static const uint8_t _message_NMEA[]{
0x00, // filter flags
0x41, // NMEA Version
0x00, // Max number of SVs to report per TaklerId
0x02, // flags
0x00, 0x00, 0x00, 0x00, // gnssToFilter
0x00, // svNumbering
0x00, // mainTalkerId
0x00, // gsvTalkerId
0x01, // Message version
0x00, 0x00, // bdsTalkerId 2 chars 0=default
0x00, 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved
};
// Enable jamming/interference monitor
// For Neo-6, Max-7 and Neo-7
static const uint8_t _message_JAM_6_7[] = {
0xf3, 0xac, 0x62, 0xad, // config1 bbThreshold = 3, cwThreshold = 15, enable = 1, reserved bits 0x16B156
0x1e, 0x03, 0x00, 0x00 // config2 antennaSetting Unknown = 0, reserved 3, = 0x00,0x00, reserved 2 = 0x31E
};
// For M8
static const uint8_t _message_JAM_8[] = {
0xf3, 0xac, 0x62, 0xad, // config1 bbThreshold = 3, cwThreshold = 15, enable1 = 1, reserved bits 0x16B156
0x1e, 0x43, 0x00, 0x00 // config2 antennaSetting Unknown = 0, enable2 = 1, generalBits = 0x31E
};
// Configure navigation engine expert settings:
// there are many variations of what were Reserved fields for the Neo-6 in later versions
// ToDo: check UBX-MON-VER for module type and protocol version
// For the Neo-6
static const uint8_t _message_NAVX5[] = {
0x00, 0x00, // msgVer (0 for this version)
0x4c, 0x66, // mask1
0x00, 0x00, 0x00, 0x00, // Reserved 0
0x00, // Reserved 1
0x00, // Reserved 2
0x03, // minSVs (Minimum number of satellites for navigation) = 3
0x10, // maxSVs (Maximum number of satellites for navigation) = 16
0x06, // minCNO (Minimum satellite signal level for navigation) = 6 dBHz
0x00, // Reserved 5
0x00, // iniFix3D (Initial fix must be 3D) (0 = false 1 = true)
0x00, // Reserved 6
0x00, // Reserved 7
0x00, // Reserved 8
0x00, 0x00, // wknRollover 0 = firmware default
0x00, 0x00, 0x00, 0x00, // Reserved 9
0x00, // Reserved 10
0x00, // Reserved 11
0x00, // usePPP (Precice Point Positioning) (0 = false, 1 = true)
0x01, // useAOP (AssistNow Autonomous configuration) = 1 (enabled)
0x00, // Reserved 12
0x00, // Reserved 13
0x00, 0x00, // aopOrbMaxErr = 0 to reset to firmware default
0x00, // Reserved 14
0x00, // Reserved 15
0x00, 0x00, // Reserved 3
0x00, 0x00, 0x00, 0x00 // Reserved 4
};
// For the M8
static const uint8_t _message_NAVX5_8[] = {
0x02, 0x00, // msgVer (2 for this version)
0x4c, 0x66, // mask1
0x00, 0x00, 0x00, 0x00, // mask2
0x00, 0x00, // Reserved 1
0x03, // minSVs (Minimum number of satellites for navigation) = 3
0x10, // maxSVs (Maximum number of satellites for navigation) = 16
0x06, // minCNO (Minimum satellite signal level for navigation) = 6 dBHz
0x00, // Reserved 2
0x00, // iniFix3D (Initial fix must be 3D) (0 = false 1 = true)
0x00, 0x00, // Reserved 3
0x00, // ackAiding
0x00, 0x00, // wknRollover 0 = firmware default
0x00, // sigAttenCompMode
0x00, // Reserved 4
0x00, 0x00, // Reserved 5
0x00, 0x00, // Reserved 6
0x00, // usePPP (Precice Point Positioning) (0 = false, 1 = true)
0x01, // aopCfg (AssistNow Autonomous configuration) = 1 (enabled)
0x00, 0x00, // Reserved 7
0x00, 0x00, // aopOrbMaxErr = 0 to reset to firmware default
0x00, 0x00, 0x00, 0x00, // Reserved 8
0x00, 0x00, 0x00, // Reserved 9
0x00 // useAdr
};
// Set GPS update rate to 1Hz
// Lowering the update rate helps to save power.
// Additionally, for some new modules like the M9/M10, an update rate lower than 5Hz
// is recommended to avoid a known issue with satellites disappearing.
// The module defaults for M8, M9, M10 are the same as we use here so no update is necessary
static const uint8_t _message_1HZ[] = {
0xE8, 0x03, // Measurement Rate (1000ms for 1Hz)
0x01, 0x00, // Navigation rate, always 1 in GPS mode
0x01, 0x00 // Time reference
};
// Disable GLL. GLL - Geographic position (latitude and longitude), which provides the current geographical
// coordinates.
static const uint8_t _message_GLL[] = {
0xF0, 0x01, // NMEA ID for GLL
0x00, // Rate for DDC
0x00, // Rate for UART1
0x00, // Rate for UART2
0x00, // Rate for USB
0x00, // Rate for SPI
0x00 // Reserved
};
// Disable GSA. GSA - GPS DOP and active satellites, used for detailing the satellites used in the positioning and
// the DOP (Dilution of Precision)
static const uint8_t _message_GSA[] = {
0xF0, 0x02, // NMEA ID for GSA
0x00, // Rate for DDC
0x00, // Rate for UART1
0x00, // Rate for UART2
0x00, // Rate for USB useful for native linux
0x00, // Rate for SPI
0x00 // Reserved
};
// Disable GSV. GSV - Satellites in view, details the number and location of satellites in view.
static const uint8_t _message_GSV[] = {
0xF0, 0x03, // NMEA ID for GSV
0x00, // Rate for DDC
0x00, // Rate for UART1
0x00, // Rate for UART2
0x00, // Rate for USB
0x00, // Rate for SPI
0x00 // Reserved
};
// Disable VTG. VTG - Track made good and ground speed, which provides course and speed information relative to
// the ground.
static const uint8_t _message_VTG[] = {
0xF0, 0x05, // NMEA ID for VTG
0x00, // Rate for DDC
0x00, // Rate for UART1
0x00, // Rate for UART2
0x00, // Rate for USB
0x00, // Rate for SPI
0x00 // Reserved
};
// Enable RMC. RMC - Recommended Minimum data, the essential gps pvt (position, velocity, time) data.
static const uint8_t _message_RMC[] = {
0xF0, 0x04, // NMEA ID for RMC
0x00, // Rate for DDC
0x01, // Rate for UART1
0x00, // Rate for UART2
0x01, // Rate for USB useful for native linux
0x00, // Rate for SPI
0x00 // Reserved
};
// Enable GGA. GGA - Global Positioning System Fix Data, which provides 3D location and accuracy data.
static const uint8_t _message_GGA[] = {
0xF0, 0x00, // NMEA ID for GGA
0x00, // Rate for DDC
0x01, // Rate for UART1
0x00, // Rate for UART2
0x01, // Rate for USB, useful for native linux
0x00, // Rate for SPI
0x00 // Reserved
};
// Disable UBX-AID-ALPSRV as it may confuse TinyGPS. The Neo-6 seems to send this message
// whether the AID Autonomous is enabled or not
static const uint8_t _message_AID[] = {
0x0B, 0x32, // NMEA ID for UBX-AID-ALPSRV
0x00, // Rate for DDC
0x00, // Rate for UART1
0x00, // Rate for UART2
0x00, // Rate for USB
0x00, // Rate for SPI
0x00 // Reserved
};
// Turn off TEXT INFO Messages for all but M10 series
// B5 62 06 02 0A 00 01 00 00 00 03 03 00 03 03 00 1F 20
static const uint8_t _message_DISABLE_TXT_INFO[] = {
0x01, // Protocol ID for NMEA
0x00, 0x00, 0x00, // Reserved
0x03, // I2C
0x03, // I/O Port 1
0x00, // I/O Port 2
0x03, // USB
0x03, // SPI
0x00 // Reserved
};
// The Power Management configuration allows the GPS module to operate in different power modes for optimized
// power consumption. The modes supported are: 0x00 = Full power: The module operates at full power with no power
// saving. 0x01 = Balanced: The module dynamically adjusts the tracking behavior to balance power consumption.
// 0x02 = Interval: The module operates in a periodic mode, cycling between tracking and power saving states.
// 0x03 = Aggressive with 1 Hz: The module operates in a power saving mode with a 1 Hz update rate.
// 0x04 = Aggressive with 2 Hz: The module operates in a power saving mode with a 2 Hz update rate.
// 0x05 = Aggressive with 4 Hz: The module operates in a power saving mode with a 4 Hz update rate.
// The 'period' field specifies the position update and search period. It is only valid when the powerSetupValue
// is set to Interval; otherwise, it must be set to '0'. The 'onTime' field specifies the duration of the ON phase
// and must be smaller than the period. It is only valid when the powerSetupValue is set to Interval; otherwise,
// it must be set to '0'.
// This command applies to M8 products
static const uint8_t _message_PMS[] = {
0x00, // Version (0)
0x03, // Power setup value 3 = Agresssive 1Hz
0x00, 0x00, // period: not applicable, set to 0
0x00, 0x00, // onTime: not applicable, set to 0
0x00, 0x00 // reserved, generated by u-center
};
static const uint8_t _message_SAVE[] = {
0x00, 0x00, 0x00, 0x00, // clearMask: no sections cleared
0xFF, 0xFF, 0x00, 0x00, // saveMask: save all sections
0x00, 0x00, 0x00, 0x00, // loadMask: no sections loaded
0x17 // deviceMask: BBR, Flash, EEPROM, and SPI Flash
};
static const uint8_t _message_SAVE_10[] = {
0x00, 0x00, 0x00, 0x00, // clearMask: no sections cleared
0xFF, 0xFF, 0x00, 0x00, // saveMask: save all sections
0x00, 0x00, 0x00, 0x00, // loadMask: no sections loaded
0x01 // deviceMask: only save to BBR
};
// As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR.
// BBR will survive a restart, and power off for a while, but modules with small backup
// 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
// VALSET Commands for M10
// Please refer to the M10 Protocol Specification:
// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf
// Where the VALSET/VALGET/VALDEL commands are described in detail.
// and:
// https://content.u-blox.com/sites/default/files/u-blox-M10-ROM-5.10_ReleaseNotes_UBX-22001426.pdf
// for interesting insights.
//
// Integration manual:
// https://content.u-blox.com/sites/default/files/documents/SAM-M10Q_IntegrationManual_UBX-22020019.pdf
// has details on low-power modes
/*
OPERATEMODE E1 2 (0 | 1 | 2)
POSUPDATEPERIOD U4 5
ACQPERIOD U4 10
GRIDOFFSET U4 0
ONTIME U2 1
MINACQTIME U1 0
MAXACQTIME U1 0
DONOTENTEROFF L 1
WAITTIMEFIX L 1
UPDATEEPH L 1
EXTINTWAKE L 0 no ext ints
EXTINTBACKUP L 0 no ext ints
EXTINTINACTIVE L 0 no ext ints
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
// 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
// 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,
0x05, 0x00, 0x00, 0x00, 0x05, 0x00, 0xd0, 0x30, 0x01, 0x00, 0x08, 0x00, 0xd0,
0x10, 0x01, 0x09, 0x00, 0xd0, 0x10, 0x01, 0x10, 0x00, 0xd0, 0x10, 0x01};
static const uint8_t _message_VALSET_PM_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40,
0x05, 0x00, 0x00, 0x00, 0x05, 0x00, 0xd0, 0x30, 0x01, 0x00, 0x08, 0x00, 0xd0,
0x10, 0x01, 0x09, 0x00, 0xd0, 0x10, 0x01, 0x10, 0x00, 0xd0, 0x10, 0x01};
/*
CFG-ITFM replaced by 5 valset messages which can be combined into one for RAM and one for BBR
20410001 bbthreshold U1 3
20410002 cwthreshold U1 15
1041000d enable L 0 -> 1
20410010 ant E1 0
10410013 enable aux L 0 -> 1
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};
// 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
// 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, 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};
static const uint8_t _message_VALSET_DISABLE_NMEA_BBR[] = {0x00, 0x02, 0x00, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5,
0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00};
// Turn off text info messages:
// Ram layer config message:
// b5 62 06 8a 09 00 00 01 00 00 07 00 92 20 06 59 50
// BBR layer config message:
// b5 62 06 8a 09 00 00 02 00 00 07 00 92 20 06 5a 58
// Turn NMEA GGA, RMC messages on:
// Layer config messages:
// RAM:
// b5 62 06 8a 0e 00 00 01 00 00 bb 00 91 20 01 ac 00 91 20 01 6a 8f
// BBR:
// b5 62 06 8a 0e 00 00 02 00 00 bb 00 91 20 01 ac 00 91 20 01 6b 9c
// FLASH:
// b5 62 06 8a 0e 00 00 04 00 00 bb 00 91 20 01 ac 00 91 20 01 6d b6
// Doing this for the FLASH layer isn't really required since we save the config to flash later
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};
/*
Operational issues with the M10:
PowerSave doesn't work with SBAS, seems like you can have SBAS enabled, but it will never lock
onto the SBAS sats.
PowerSave doesn't work with BDS B1C, u-blox says use B1l instead.
BDS B1l cannot be enabled with BDS B1C or GLONASS L1OF, so GLONASS will work with B1C, but not B1l
So no powersave with GLONASS and BDS B1l enabled.
So disable GLONASS and use BDS B1l, which is part of the default M10 config.
GNSS configuration:
Default GNSS configuration is: GPS, Galileo, BDS B1l, with QZSS and SBAS enabled.
The PMREQ puts the receiver to sleep and wakeup re-acquires really fast and seems to not need
the PM config. Lets try without it.
PMREQ sort of works with SBAS, but the awake time is too short to re-acquire any SBAS sats.
The defination of "Got Fix" doesn't seem to include SBAS. Much more too this...
Even if it was, it can take minutes (up to 12.5),
even under good sat visibility conditions to re-acquire the SBAS data.
Another effect fo the quick transition to sleep is that no other sats will be acquired so the
sat count will tend to remain at what the initial fix was.
*/
// GNSS disable SBAS as recommended by u-blox if using GNSS defaults and power save mode
/*
Ram layer config message:
b5 62 06 8a 0e 00 00 01 00 00 20 00 31 10 00 05 00 31 10 00 46 87
BBR layer config message:
b5 62 06 8a 0e 00 00 02 00 00 20 00 31 10 00 05 00 31 10 00 47 94
*/
code/firmware-2.7.15.567b8ea/src/graphics/EInkDisplay2.cpp
+284
View File
@@ -0,0 +1,284 @@
#include "configuration.h"
#ifdef USE_EINK
#include "EInkDisplay2.h"
#include "SPILock.h"
#include "main.h"
#include <SPI.h>
#ifdef GXEPD2_DRIVER_0
#include "einkDetect.h"
#endif
/*
The macros EINK_DISPLAY_MODEL, EINK_WIDTH, and EINK_HEIGHT are defined as build_flags in a variant's platformio.ini
Previously, these macros were defined at the top of this file.
For archival reasons, note that the following configurations had also been tested during this period:
* ifdef RAK4631
- 4.2 inch
EINK_DISPLAY_MODEL: GxEPD2_420_M01
EINK_WIDTH: 300
EINK_WIDTH: 400
- 2.9 inch
EINK_DISPLAY_MODEL: GxEPD2_290_T5D
EINK_WIDTH: 296
EINK_HEIGHT: 128
- 1.54 inch
EINK_DISPLAY_MODEL: GxEPD2_154_M09
EINK_WIDTH: 200
EINK_HEIGHT: 200
*/
// 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;
// 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);
}
/**
* 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);
uint32_t now = millis();
uint32_t sinceLast = now - lastDrawMsec;
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
#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);
}
}
#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);
}
}
#endif
// Trigger the refresh in GxEPD2
LOG_DEBUG("Update E-Paper");
adafruitDisplay->nextPage();
// End the update process
endUpdate();
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();
}
// 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)
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::setDetected(uint8_t detected)
{
(void)detected;
}
// Connect to the display - variant specific
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);
#ifdef ELECROW_ThinkNode_M1
// ThinkNode M1 has a hardware dimmable backlight. Start enabled
digitalWrite(PIN_EINK_EN, HIGH);
#else
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);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
#if defined(ELECROW_ThinkNode_M1) || defined(T_ECHO_LITE)
adafruitDisplay->setRotation(4);
#else
adafruitDisplay->setRotation(3);
#endif
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(ELECROW_ThinkNode_M5)
{
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
adafruitDisplay->setRotation(4);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(MESHLINK)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, SPI1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
}
#elif defined(RAK4630) || defined(MAKERPYTHON)
{
if (eink_found) {
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 10, false, SPI1, SPISettings(4000000, MSBFIRST, SPI_MODE0));
// RAK14000 2.13 inch b/w 250x122 does actually now support fast refresh
adafruitDisplay->setRotation(3);
// Fast refresh support for 1.54, 2.13 RAK14000 b/w , 2.9 and 4.2
// adafruitDisplay->setRotation(1);
adafruitDisplay->setPartialWindow(0, 0, displayWidth, displayHeight);
} else {
(void)adafruitDisplay;
}
}
#elif defined(HELTEC_WIRELESS_PAPER_V1_0) || defined(HELTEC_VISION_MASTER_E290) || defined(TLORA_T3S3_EPAPER) || \
defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER)
{
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
// VExt already enabled in setup()
// RTC GPIO hold disabled in setup()
// Create GxEPD2 objects
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
#if defined(CROWPANEL_ESP32S3_5_EPAPER) || defined(CROWPANEL_ESP32S3_4_EPAPER)
adafruitDisplay->setRotation(0);
#endif
}
#elif defined(PCA10059) || defined(ME25LS01)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI1, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(0);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(M5_COREINK) || defined(T_DECK_PRO)
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(0);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
#elif defined(my) || defined(ESP32_S3_PICO)
{
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
adafruitDisplay->init(115200, true, 40, false, SPI, SPISettings(4000000, MSBFIRST, SPI_MODE0));
adafruitDisplay->setRotation(1);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK)
{
spi1 = &SPI1;
spi1->begin();
// VExt already enabled in setup()
// RTC GPIO hold disabled in setup()
// Create GxEPD2 objects
auto lowLevel = new EINK_DISPLAY_MODEL(PIN_EINK_CS, PIN_EINK_DC, PIN_EINK_RES, PIN_EINK_BUSY, *spi1);
adafruitDisplay = new GxEPD2_BW<EINK_DISPLAY_MODEL, EINK_DISPLAY_MODEL::HEIGHT>(*lowLevel);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
adafruitDisplay->setPartialWindow(0, 0, EINK_WIDTH, EINK_HEIGHT);
}
#elif defined(HELTEC_WIRELESS_PAPER) || defined(HELTEC_VISION_MASTER_E213)
// Detect display model, before starting SPI
EInkDetectionResult displayModel = detectEInk();
// Start HSPI
hspi = new SPIClass(HSPI);
hspi->begin(PIN_EINK_SCLK, -1, PIN_EINK_MOSI, PIN_EINK_CS); // SCLK, MISO, MOSI, SS
// Create GxEPD2 object
adafruitDisplay = new GxEPD2_Multi<GXEPD2_DRIVER_0, GXEPD2_DRIVER_1>((uint8_t)displayModel, PIN_EINK_CS, PIN_EINK_DC,
PIN_EINK_RES, PIN_EINK_BUSY, *hspi);
// Init GxEPD2
adafruitDisplay->init();
adafruitDisplay->setRotation(3);
#endif
return true;
}
#endif
code/firmware-2.7.15.567b8ea/src/graphics/EInkDisplay2.h
+96
View File
@@ -0,0 +1,96 @@
#pragma once
#ifdef USE_EINK
#include "GxEPD2_BW.h"
#include <OLEDDisplay.h>
#ifdef GXEPD2_DRIVER_0 // If variant has multiple possible display models
#include "GxEPD2Multi.h"
#endif
/**
* An adapter class that allows using the GxEPD2 library as if it was an OLEDDisplay implementation.
*
* Note: EInkDynamicDisplay derives from this class.
*
* Remaining TODO:
* optimize display() to only draw changed pixels (see other OLED subclasses for examples)
* implement displayOn/displayOff to turn off the TFT device (and backlight)
* Use the fast NRF52 SPI API rather than the slow standard arduino version
*
* 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;
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;
/**
* 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();
/**
* 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; }
// Send a command to the display (low level function)
virtual void sendCommand(uint8_t com) 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;
#else
// 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) || \
defined(CROWPANEL_ESP32S3_4_EPAPER) || defined(CROWPANEL_ESP32S3_2_EPAPER) || defined(ELECROW_ThinkNode_M5)
SPIClass *hspi = NULL;
#endif
#if defined(HELTEC_MESH_POCKET) || defined(SEEED_WIO_TRACKER_L1_EINK) || defined(HELTEC_MESH_SOLAR_EINK)
SPIClass *spi1 = NULL;
#endif
private:
// FIXME quick hack to limit drawing to a very slow rate
uint32_t lastDrawMsec = 0;
};
#endif
code/firmware-2.7.15.567b8ea/src/graphics/EInkDynamicDisplay.cpp
+564
View File
@@ -0,0 +1,564 @@
#include "Throttle.h"
#include "configuration.h"
#if defined(USE_EINK) && defined(USE_EINK_DYNAMICDISPLAY)
#include "EInkDynamicDisplay.h"
// 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
#ifdef EINK_LIMIT_GHOSTING_PX
dirtyPixels = new uint8_t[EInkDisplay::displayBufferSize](); // Init with zeros
#endif
}
// Destructor
EInkDynamicDisplay::~EInkDynamicDisplay()
{
// If we were tracking ghost pixels, free the memory
#ifdef EINK_LIMIT_GHOSTING_PX
delete[] dirtyPixels;
#endif
}
// Screen requests a BACKGROUND frame
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
}
// 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);
}
// GxEPD2 code to set fast refresh
void EInkDynamicDisplay::configForFastRefresh()
{
// Variant-specific code can go here
#if defined(PRIVATE_HW)
#else
// 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
#if defined(PRIVATE_HW)
#else
// 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;
}
// Change from FAST back to FULL
else if (currentConfig == FAST && refresh == FULL) {
configForFullRefresh();
currentConfig = FULL;
}
}
// Update fastRefreshCount
void EInkDynamicDisplay::adjustRefreshCounters()
{
if (refresh == FAST)
fastRefreshCount++;
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
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
#ifdef HAS_EINK_ASYNCFULL
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
}
}
// 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
#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();
}
#endif
}
// Assess situation, pick a refresh type
bool EInkDynamicDisplay::determineMode()
{
checkInitialized();
checkForPromotion();
#if defined(HAS_EINK_ASYNCFULL)
checkBusyAsyncRefresh();
#endif
checkRateLimiting();
// If too soon for a new frame, or display busy, abort early
if (refresh == SKIPPED)
return false; // No refresh
// -- 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
// Once mode determined, any remaining checks will bypass
checkCosmetic();
checkDemandingFast();
checkFrameMatchesPrevious();
checkConsecutiveFastRefreshes();
#ifdef EINK_LIMIT_GHOSTING_PX
checkExcessiveGhosting();
#endif
checkFastRequested();
if (refresh == UNSPECIFIED)
LOG_WARN("There was a flaw in the determineMode() logic");
// -- Decision has been reached --
applyRefreshMode();
adjustRefreshCounters();
#ifdef EINK_LIMIT_GHOSTING_PX
// 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
}
// Is this the very first frame?
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();
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);
}
}
// 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
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;
// 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;
// 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;
// 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;
// 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;
// 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;
}
#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);
}
// 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;
// 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);
}
}
// No objections, we can perform fast-refresh, if desired
void EInkDynamicDisplay::checkFastRequested()
{
if (refresh != UNSPECIFIED)
return;
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);
#else
// 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);
}
}
// Reset the timer used for rate-limiting
void EInkDynamicDisplay::resetRateLimiting()
{
previousRunMs = millis();
}
// Generate a hash of this frame, to compare against previous update
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;
}
}
// Store the results of determineMode() for future use, and reset for next call
void EInkDynamicDisplay::storeAndReset()
{
previousFrameFlags = frameFlags;
previousRefresh = refresh;
previousReason = reason;
// Only store image hash if the display will update
if (refresh != SKIPPED) {
previousImageHash = imageHash;
}
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;
// 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++) {
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++;
// 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);
}
// 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;
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);
}
}
// 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);
}
#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;
}
}
#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;
LOG_DEBUG("Join an async refresh in progress");
// 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");
// 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;
// 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");
// 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;
// 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;
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
}
// Hold control while an async refresh runs
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
}
#endif // HAS_EINK_ASYNCFULL
#endif // USE_EINK_DYNAMICDISPLAY
code/firmware-2.7.15.567b8ea/src/graphics/EInkDynamicDisplay.h
+154
View File
@@ -0,0 +1,154 @@
#pragma once
#include "configuration.h"
#if defined(USE_EINK) && defined(USE_EINK_DYNAMICDISPLAY)
#include "EInkDisplay2.h"
#include "GxEPD2_BW.h"
#include "concurrency/NotifiedWorkerThread.h"
/*
Derives from the EInkDisplay adapter class.
Accepts suggestions from Screen class about frame type.
Determines which refresh type is most suitable.
(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();
// 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);
// 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,
};
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()
// 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
// 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
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.
#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
#endif
// 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
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
protected:
void pollAsyncRefresh() {} // Dummy method. In theory, not reachable
#endif
};
// Hide the ugly casts used in Screen.cpp
#define EINK_ADD_FRAMEFLAG(display, flag) static_cast<EInkDynamicDisplay *>(display)->addFrameFlag(EInkDynamicDisplay::flag)
#define EINK_JOIN_ASYNCREFRESH(display) static_cast<EInkDynamicDisplay *>(display)->joinAsyncRefresh()
#else // !USE_EINK_DYNAMICDISPLAY
// Dummy-macro, removes the need for include guards
#define EINK_ADD_FRAMEFLAG(display, flag)
#define EINK_JOIN_ASYNCREFRESH(display)
#endif
code/firmware-2.7.15.567b8ea/src/graphics/GxEPD2Multi.h
+135
View File
@@ -0,0 +1,135 @@
// Wrapper class for GxEPD2_BW
// Generic signature at build-time, so that we can detect display model at run-time
// 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
{
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;
};
code/firmware-2.7.15.567b8ea/src/graphics/NeoPixel.h
+4
View File
@@ -0,0 +1,4 @@
#ifdef HAS_NEOPIXEL
#include <Adafruit_NeoPixel.h>
extern Adafruit_NeoPixel pixels;
#endif
code/firmware-2.7.15.567b8ea/src/graphics/NomadStarLED.h
+5
View File
@@ -0,0 +1,5 @@
#ifdef HAS_LP5562
#include <LP5562.h>
extern LP5562 rgbw;
#endif
code/firmware-2.7.15.567b8ea/src/graphics/Panel_sdl.cpp
+687
View File
@@ -0,0 +1,687 @@
/*----------------------------------------------------------------------------/
Lovyan GFX - Graphics library for embedded devices.
Original Source:
https://github.com/lovyan03/LovyanGFX/
Licence:
[FreeBSD](https://github.com/lovyan03/LovyanGFX/blob/master/license.txt)
Author:
[lovyan03](https://twitter.com/lovyan03)
Contributors:
[ciniml](https://github.com/ciniml)
[mongonta0716](https://github.com/mongonta0716)
[tobozo](https://github.com/tobozo)
Porting for SDL:
[imliubo](https://github.com/imliubo)
/----------------------------------------------------------------------------*/
#include "Panel_sdl.hpp"
#if defined(SDL_h_)
// #include "../common.hpp"
// #include "../../misc/common_function.hpp"
// #include "../../Bus.hpp"
#include <list>
#include <math.h>
#include <vector>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
namespace lgfx
{
inline namespace v1
{
SDL_Keymod Panel_sdl::_keymod = KMOD_NONE;
static SDL_semaphore *_update_in_semaphore = nullptr;
static SDL_semaphore *_update_out_semaphore = nullptr;
volatile static uint32_t _in_step_exec = 0;
volatile static uint32_t _msec_step_exec = 512;
static bool _inited = false;
static bool _all_close = false;
volatile uint8_t Panel_sdl::_gpio_dummy_values[EMULATED_GPIO_MAX];
static inline void *heap_alloc_dma(size_t length)
{
return malloc(length);
} // aligned_alloc(16, length);
static inline void heap_free(void *buf)
{
free(buf);
}
static std::list<monitor_t *> _list_monitor;
static monitor_t *const getMonitorByWindowID(uint32_t windowID)
{
for (auto &m : _list_monitor) {
if (SDL_GetWindowID(m->window) == windowID) {
return m;
}
}
return nullptr;
}
//----------------------------------------------------------------------------
static std::vector<Panel_sdl::KeyCodeMapping_t> _key_code_map;
void Panel_sdl::addKeyCodeMapping(SDL_KeyCode keyCode, uint8_t gpio)
{
if (gpio > EMULATED_GPIO_MAX)
return;
KeyCodeMapping_t map;
map.keycode = keyCode;
map.gpio = gpio;
_key_code_map.push_back(map);
}
int Panel_sdl::getKeyCodeMapping(SDL_KeyCode keyCode)
{
for (const auto &i : _key_code_map) {
if (i.keycode == keyCode)
return i.gpio;
}
return -1;
}
void Panel_sdl::_event_proc(void)
{
SDL_Event event;
while (SDL_PollEvent(&event)) {
if ((event.type == SDL_KEYDOWN) || (event.type == SDL_KEYUP)) {
auto mon = getMonitorByWindowID(event.button.windowID);
int gpio = -1;
/// Check key mapping
gpio = getKeyCodeMapping((SDL_KeyCode)event.key.keysym.sym);
if (gpio < 0) {
switch (event.key.keysym.sym) { /// M5StackのBtnA～BtnCのエミュレート;
// case SDLK_LEFT: gpio = 39; break;
// case SDLK_DOWN: gpio = 38; break;
// case SDLK_RIGHT: gpio = 37; break;
// case SDLK_UP: gpio = 36; break;
/// L/Rキーで画面回転
case SDLK_r:
case SDLK_l:
if (event.type == SDL_KEYDOWN && event.key.keysym.mod == _keymod) {
if (mon != nullptr) {
mon->frame_rotation = (mon->frame_rotation += event.key.keysym.sym == SDLK_r ? 1 : -1);
int x, y, w, h;
SDL_GetWindowSize(mon->window, &w, &h);
SDL_GetWindowPosition(mon->window, &x, &y);
SDL_SetWindowSize(mon->window, h, w);
SDL_SetWindowPosition(mon->window, x + (w - h) / 2, y + (h - w) / 2);
mon->panel->sdl_invalidate();
}
}
break;
/// 1～6キーで画面拡大率変更
case SDLK_1:
case SDLK_2:
case SDLK_3:
case SDLK_4:
case SDLK_5:
case SDLK_6:
if (event.type == SDL_KEYDOWN && event.key.keysym.mod == _keymod) {
if (mon != nullptr) {
int size = 1 + (event.key.keysym.sym - SDLK_1);
_update_scaling(mon, size, size);
}
}
break;
default:
continue;
}
}
if (event.type == SDL_KEYDOWN) {
Panel_sdl::gpio_lo(gpio);
} else {
Panel_sdl::gpio_hi(gpio);
}
} else if (event.type == SDL_MOUSEBUTTONDOWN || event.type == SDL_MOUSEBUTTONUP || event.type == SDL_MOUSEMOTION) {
auto mon = getMonitorByWindowID(event.button.windowID);
if (mon != nullptr) {
{
int x, y, w, h;
SDL_GetWindowSize(mon->window, &w, &h);
SDL_GetMouseState(&x, &y);
float sf = sinf(mon->frame_angle * M_PI / 180);
float cf = cosf(mon->frame_angle * M_PI / 180);
x -= w / 2.0f;
y -= h / 2.0f;
float nx = y * sf + x * cf;
float ny = y * cf - x * sf;
if (mon->frame_rotation & 1) {
std::swap(w, h);
}
x = (nx * mon->frame_width / w) + (mon->frame_width >> 1);
y = (ny * mon->frame_height / h) + (mon->frame_height >> 1);
mon->touch_x = x - mon->frame_inner_x;
mon->touch_y = y - mon->frame_inner_y;
}
if (event.type == SDL_MOUSEBUTTONDOWN && event.button.button == SDL_BUTTON_LEFT) {
mon->touched = true;
}
if (event.type == SDL_MOUSEBUTTONUP && event.button.button == SDL_BUTTON_LEFT) {
mon->touched = false;
}
}
} else if (event.type == SDL_WINDOWEVENT) {
auto monitor = getMonitorByWindowID(event.window.windowID);
if (monitor) {
if (event.window.event == SDL_WINDOWEVENT_RESIZED) {
int mw, mh;
SDL_GetRendererOutputSize(monitor->renderer, &mw, &mh);
if (monitor->frame_rotation & 1) {
std::swap(mw, mh);
}
monitor->scaling_x = (mw * 2 / monitor->frame_width) / 2.0f;
monitor->scaling_y = (mh * 2 / monitor->frame_height) / 2.0f;
monitor->panel->sdl_invalidate();
} else if (event.window.event == SDL_WINDOWEVENT_CLOSE) {
monitor->closing = true;
}
}
} else if (event.type == SDL_QUIT) {
for (auto &m : _list_monitor) {
m->closing = true;
}
}
}
}
/// デバッガでステップ実行されていることを検出するスレッド用関数。
static int detectDebugger(bool *running)
{
uint32_t prev_ms = SDL_GetTicks();
do {
SDL_Delay(1);
uint32_t ms = SDL_GetTicks();
/// 時間間隔が広すぎる場合はステップ実行中 (ブレークポイントで止まった)と判断する。
/// また、解除されたと判断した後も1023msecほど状態を維持する。
if (ms - prev_ms > 64) {
_in_step_exec = _msec_step_exec;
} else if (_in_step_exec) {
--_in_step_exec;
}
prev_ms = ms;
} while (*running);
return 0;
}
void Panel_sdl::_update_proc(void)
{
for (auto it = _list_monitor.begin(); it != _list_monitor.end();) {
if ((*it)->closing) {
if ((*it)->texture_frameimage) {
SDL_DestroyTexture((*it)->texture_frameimage);
}
SDL_DestroyTexture((*it)->texture);
SDL_DestroyRenderer((*it)->renderer);
SDL_DestroyWindow((*it)->window);
_list_monitor.erase(it++);
if (_list_monitor.empty()) {
_all_close = true;
return;
}
continue;
}
(*it)->panel->sdl_update();
++it;
}
}
int Panel_sdl::setup(void)
{
if (_inited)
return 1;
_inited = true;
/// Add default keycode mapping
/// M5StackのBtnA～BtnCのエミュレート;
addKeyCodeMapping(SDLK_LEFT, 39);
addKeyCodeMapping(SDLK_DOWN, 38);
addKeyCodeMapping(SDLK_RIGHT, 37);
addKeyCodeMapping(SDLK_UP, 36);
SDL_CreateThread((SDL_ThreadFunction)detectDebugger, "dbg", &_inited);
_update_in_semaphore = SDL_CreateSemaphore(0);
_update_out_semaphore = SDL_CreateSemaphore(0);
for (size_t pin = 0; pin < EMULATED_GPIO_MAX; ++pin) {
gpio_hi(pin);
}
/*Initialize the SDL*/
SDL_Init(SDL_INIT_VIDEO);
SDL_StartTextInput();
// SDL_SetThreadPriority(SDL_ThreadPriority::SDL_THREAD_PRIORITY_HIGH);
return 0;
}
int Panel_sdl::loop(void)
{
if (!_inited)
return 1;
_event_proc();
SDL_SemWaitTimeout(_update_in_semaphore, 1);
_update_proc();
_event_proc();
if (SDL_SemValue(_update_out_semaphore) == 0) {
SDL_SemPost(_update_out_semaphore);
}
return _all_close;
}
int Panel_sdl::close(void)
{
if (!_inited)
return 1;
_inited = false;
SDL_StopTextInput();
SDL_DestroySemaphore(_update_in_semaphore);
SDL_DestroySemaphore(_update_out_semaphore);
SDL_Quit();
return 0;
}
int Panel_sdl::main(int (*fn)(bool *), uint32_t msec_step_exec)
{
_msec_step_exec = msec_step_exec;
/// SDLの準備
if (0 != Panel_sdl::setup()) {
return 1;
}
/// ユーザコード関数の動作・停止フラグ
bool running = true;
/// ユーザコード関数を起動する
auto thread = SDL_CreateThread((SDL_ThreadFunction)fn, "fn", &running);
/// 全部のウィンドウが閉じられるまでSDLのイベント・描画処理を継続
while (0 == Panel_sdl::loop()) {
};
/// ユーザコード関数を終了する
running = false;
SDL_WaitThread(thread, nullptr);
/// SDLを終了する
return Panel_sdl::close();
}
void Panel_sdl::setScaling(uint_fast8_t scaling_x, uint_fast8_t scaling_y)
{
monitor.scaling_x = scaling_x;
monitor.scaling_y = scaling_y;
}
void Panel_sdl::setFrameImage(const void *frame_image, int frame_width, int frame_height, int inner_x, int inner_y)
{
monitor.frame_image = frame_image;
monitor.frame_width = frame_width;
monitor.frame_height = frame_height;
monitor.frame_inner_x = inner_x;
monitor.frame_inner_y = inner_y;
}
void Panel_sdl::setFrameRotation(uint_fast16_t frame_rotation)
{
monitor.frame_rotation = frame_rotation;
monitor.frame_angle = (monitor.frame_rotation) * 90;
}
Panel_sdl::~Panel_sdl(void)
{
_list_monitor.remove(&monitor);
SDL_DestroyMutex(_sdl_mutex);
}
Panel_sdl::Panel_sdl(void) : Panel_FrameBufferBase()
{
_sdl_mutex = SDL_CreateMutex();
_auto_display = true;
monitor.panel = this;
}
bool Panel_sdl::init(bool use_reset)
{
initFrameBuffer(_cfg.panel_width * 4, _cfg.panel_height);
bool res = Panel_FrameBufferBase::init(use_reset);
_list_monitor.push_back(&monitor);
return res;
}
color_depth_t Panel_sdl::setColorDepth(color_depth_t depth)
{
auto bits = depth & color_depth_t::bit_mask;
if (bits >= 16) {
depth = (bits > 16) ? rgb888_3Byte : rgb565_2Byte;
} else {
depth = (depth == color_depth_t::grayscale_8bit) ? grayscale_8bit : rgb332_1Byte;
}
_write_depth = depth;
_read_depth = depth;
return depth;
}
Panel_sdl::lock_t::lock_t(Panel_sdl *parent) : _parent{parent}
{
SDL_LockMutex(parent->_sdl_mutex);
};
Panel_sdl::lock_t::~lock_t(void)
{
++_parent->_modified_counter;
SDL_UnlockMutex(_parent->_sdl_mutex);
if (SDL_SemValue(_update_in_semaphore) < 2) {
SDL_SemPost(_update_in_semaphore);
if (!_in_step_exec) {
SDL_SemWaitTimeout(_update_out_semaphore, 1);
}
}
};
void Panel_sdl::drawPixelPreclipped(uint_fast16_t x, uint_fast16_t y, uint32_t rawcolor)
{
lock_t lock(this);
Panel_FrameBufferBase::drawPixelPreclipped(x, y, rawcolor);
}
void Panel_sdl::writeFillRectPreclipped(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, uint32_t rawcolor)
{
lock_t lock(this);
Panel_FrameBufferBase::writeFillRectPreclipped(x, y, w, h, rawcolor);
}
void Panel_sdl::writeBlock(uint32_t rawcolor, uint32_t length)
{
// lock_t lock(this);
Panel_FrameBufferBase::writeBlock(rawcolor, length);
}
void Panel_sdl::writeImage(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, pixelcopy_t *param, bool use_dma)
{
lock_t lock(this);
Panel_FrameBufferBase::writeImage(x, y, w, h, param, use_dma);
}
void Panel_sdl::writeImageARGB(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h, pixelcopy_t *param)
{
lock_t lock(this);
Panel_FrameBufferBase::writeImageARGB(x, y, w, h, param);
}
void Panel_sdl::writePixels(pixelcopy_t *param, uint32_t len, bool use_dma)
{
lock_t lock(this);
Panel_FrameBufferBase::writePixels(param, len, use_dma);
}
void Panel_sdl::display(uint_fast16_t x, uint_fast16_t y, uint_fast16_t w, uint_fast16_t h)
{
(void)x;
(void)y;
(void)w;
(void)h;
if (_in_step_exec) {
if (_display_counter != _modified_counter) {
do {
SDL_SemPost(_update_in_semaphore);
SDL_SemWaitTimeout(_update_out_semaphore, 1);
} while (_display_counter != _modified_counter);
SDL_Delay(1);
}
}
}
uint_fast8_t Panel_sdl::getTouchRaw(touch_point_t *tp, uint_fast8_t count)
{
(void)count;
tp->x = monitor.touch_x;
tp->y = monitor.touch_y;
tp->size = monitor.touched ? 1 : 0;
tp->id = 0;
return monitor.touched;
}
void Panel_sdl::setWindowTitle(const char *title)
{
_window_title = title;
if (monitor.window) {
SDL_SetWindowTitle(monitor.window, _window_title);
}
}
void Panel_sdl::_update_scaling(monitor_t *mon, float sx, float sy)
{
mon->scaling_x = sx;
mon->scaling_y = sy;
int nw = mon->frame_width;
int nh = mon->frame_height;
if (mon->frame_rotation & 1) {
std::swap(nw, nh);
}
int x, y, w, h;
int rw, rh;
SDL_GetRendererOutputSize(mon->renderer, &rw, &rh);
SDL_GetWindowSize(mon->window, &w, &h);
nw = nw * sx * w / rw;
nh = nh * sy * h / rh;
SDL_GetWindowPosition(mon->window, &x, &y);
SDL_SetWindowSize(mon->window, nw, nh);
SDL_SetWindowPosition(mon->window, x + (w - nw) / 2, y + (h - nh) / 2);
mon->panel->sdl_invalidate();
}
void Panel_sdl::sdl_create(monitor_t *m)
{
int flag = SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI;
#if SDL_FULLSCREEN
flag |= SDL_WINDOW_FULLSCREEN;
#endif
if (m->frame_width < _cfg.panel_width) {
m->frame_width = _cfg.panel_width;
}
if (m->frame_height < _cfg.panel_height) {
m->frame_height = _cfg.panel_height;
}
int window_width = m->frame_width * m->scaling_x;
int window_height = m->frame_height * m->scaling_y;
int scaling_x = m->scaling_x;
int scaling_y = m->scaling_y;
if (m->frame_rotation & 1) {
std::swap(window_width, window_height);
std::swap(scaling_x, scaling_y);
}
{
m->window = SDL_CreateWindow(_window_title, SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, window_width, window_height,
flag); /*last param. SDL_WINDOW_BORDERLESS to hide borders*/
}
m->renderer = SDL_CreateRenderer(m->window, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
m->texture =
SDL_CreateTexture(m->renderer, SDL_PIXELFORMAT_RGB24, SDL_TEXTUREACCESS_STREAMING, _cfg.panel_width, _cfg.panel_height);
SDL_SetTextureBlendMode(m->texture, SDL_BLENDMODE_NONE);
if (m->frame_image) {
// 枠画像用のサーフェイスを作成
auto sf = SDL_CreateRGBSurfaceFrom((void *)m->frame_image, m->frame_width, m->frame_height, 32, m->frame_width * 4,
0xFF000000, 0xFF0000, 0xFF00, 0xFF);
if (sf != nullptr) {
// 枠画像からテクスチャを作成
m->texture_frameimage = SDL_CreateTextureFromSurface(m->renderer, sf);
SDL_FreeSurface(sf);
}
}
SDL_SetTextureBlendMode(m->texture_frameimage, SDL_BLENDMODE_BLEND);
_update_scaling(m, scaling_x, scaling_y);
}
void Panel_sdl::sdl_update(void)
{
if (monitor.renderer == nullptr) {
sdl_create(&monitor);
}
bool step_exec = _in_step_exec;
if (_texupdate_counter != _modified_counter) {
pixelcopy_t pc(nullptr, color_depth_t::rgb888_3Byte, _write_depth, false);
if (_write_depth == rgb565_2Byte) {
pc.fp_copy = pixelcopy_t::copy_rgb_fast<bgr888_t, swap565_t>;
} else if (_write_depth == rgb888_3Byte) {
pc.fp_copy = pixelcopy_t::copy_rgb_fast<bgr888_t, bgr888_t>;
} else if (_write_depth == rgb332_1Byte) {
pc.fp_copy = pixelcopy_t::copy_rgb_fast<bgr888_t, rgb332_t>;
} else if (_write_depth == grayscale_8bit) {
pc.fp_copy = pixelcopy_t::copy_rgb_fast<bgr888_t, grayscale_t>;
}
if (0 == SDL_LockMutex(_sdl_mutex)) {
_texupdate_counter = _modified_counter;
for (int y = 0; y < _cfg.panel_height; ++y) {
pc.src_x32 = 0;
pc.src_data = _lines_buffer[y];
pc.fp_copy(&_texturebuf[y * _cfg.panel_width], 0, _cfg.panel_width, &pc);
}
SDL_UnlockMutex(_sdl_mutex);
SDL_UpdateTexture(monitor.texture, nullptr, _texturebuf, _cfg.panel_width * sizeof(rgb888_t));
}
}
int angle = monitor.frame_angle;
int target = (monitor.frame_rotation) * 90;
angle = (((target * 4) + (angle * 4) + (angle < target ? 8 : 0)) >> 3);
if (monitor.frame_angle != angle) { // 表示する向きを変える
monitor.frame_angle = angle;
sdl_invalidate();
} else if (monitor.frame_rotation & ~3u) {
monitor.frame_rotation &= 3;
monitor.frame_angle = (monitor.frame_rotation) * 90;
sdl_invalidate();
}
if (_invalidated || (_display_counter != _texupdate_counter)) {
SDL_RendererInfo info;
if (0 == SDL_GetRendererInfo(monitor.renderer, &info)) {
// ステップ実行中はVSYNCを待機しない
if (((bool)(info.flags & SDL_RENDERER_PRESENTVSYNC)) == step_exec) {
SDL_RenderSetVSync(monitor.renderer, !step_exec);
}
}
{
int red = 0;
int green = 0;
int blue = 0;
#if defined(M5GFX_BACK_COLOR)
red = ((M5GFX_BACK_COLOR) >> 16) & 0xFF;
green = ((M5GFX_BACK_COLOR) >> 8) & 0xFF;
blue = ((M5GFX_BACK_COLOR)) & 0xFF;
#endif
SDL_SetRenderDrawColor(monitor.renderer, red, green, blue, 0xFF);
}
SDL_RenderClear(monitor.renderer);
if (_invalidated) {
_invalidated = false;
int mw, mh;
SDL_GetRendererOutputSize(monitor.renderer, &mw, &mh);
}
render_texture(monitor.texture, monitor.frame_inner_x, monitor.frame_inner_y, _cfg.panel_width, _cfg.panel_height, angle);
render_texture(monitor.texture_frameimage, 0, 0, monitor.frame_width, monitor.frame_height, angle);
SDL_RenderPresent(monitor.renderer);
_display_counter = _texupdate_counter;
if (_invalidated) {
_invalidated = false;
SDL_SetRenderDrawColor(monitor.renderer, 0, 0, 0, 0xFF);
SDL_RenderClear(monitor.renderer);
render_texture(monitor.texture, monitor.frame_inner_x, monitor.frame_inner_y, _cfg.panel_width, _cfg.panel_height,
angle);
render_texture(monitor.texture_frameimage, 0, 0, monitor.frame_width, monitor.frame_height, angle);
SDL_RenderPresent(monitor.renderer);
}
}
}
void Panel_sdl::render_texture(SDL_Texture *texture, int tx, int ty, int tw, int th, float angle)
{
SDL_Point pivot;
pivot.x = (monitor.frame_width / 2.0f - tx) * (float)monitor.scaling_x;
pivot.y = (monitor.frame_height / 2.0f - ty) * (float)monitor.scaling_y;
SDL_Rect dstrect;
dstrect.w = tw * monitor.scaling_x;
dstrect.h = th * monitor.scaling_y;
int mw, mh;
SDL_GetRendererOutputSize(monitor.renderer, &mw, &mh);
dstrect.x = mw / 2.0f - pivot.x;
dstrect.y = mh / 2.0f - pivot.y;
SDL_RenderCopyEx(monitor.renderer, texture, nullptr, &dstrect, angle, &pivot, SDL_RendererFlip::SDL_FLIP_NONE);
}
bool Panel_sdl::initFrameBuffer(size_t width, size_t height)
{
uint8_t **lineArray = (uint8_t **)heap_alloc_dma(height * sizeof(uint8_t *));
if (nullptr == lineArray) {
return false;
}
_texturebuf = (rgb888_t *)heap_alloc_dma(width * height * sizeof(rgb888_t));
/// 8byte alignment;
width = (width + 7) & ~7u;
_lines_buffer = lineArray;
memset(lineArray, 0, height * sizeof(uint8_t *));
uint8_t *framebuffer = (uint8_t *)heap_alloc_dma(width * height + 16);
auto fb = framebuffer;
{
for (size_t y = 0; y < height; ++y) {
lineArray[y] = fb;
fb += width;
}
}
return true;
}
void Panel_sdl::deinitFrameBuffer(void)
{
auto lines = _lines_buffer;
_lines_buffer = nullptr;
if (lines != nullptr) {
heap_free(lines[0]);
heap_free(lines);
}
if (_texturebuf) {
heap_free(_texturebuf);
_texturebuf = nullptr;
}
}
//----------------------------------------------------------------------------
} // namespace v1
} // namespace lgfx
#endif

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