diff --git a/Dockerfile b/Dockerfile index fee6c62d4..08cb3925d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -48,6 +48,7 @@ USER mesh WORKDIR /home/mesh COPY --from=builder /tmp/firmware/release/meshtasticd /home/mesh/ +RUN mkdir data VOLUME /home/mesh/data CMD [ "sh", "-cx", "./meshtasticd -d /home/mesh/data --hwid=${HWID:-$RANDOM}" ] diff --git a/arch/nrf52/nrf52.ini b/arch/nrf52/nrf52.ini index f41ef0edc..1a371e920 100644 --- a/arch/nrf52/nrf52.ini +++ b/arch/nrf52/nrf52.ini @@ -1,6 +1,6 @@ [nrf52_base] ; Instead of the standard nordicnrf52 platform, we use our fork which has our added variant files -platform = platformio/nordicnrf52@^10.4.0 +platform = platformio/nordicnrf52@^10.5.0 extends = arduino_base build_type = debug diff --git a/platformio.ini b/platformio.ini index 85bce3279..d8d398775 100644 --- a/platformio.ini +++ b/platformio.ini @@ -31,6 +31,9 @@ default_envs = tbeam ;default_envs = rak4631 ;default_envs = rak10701 ;default_envs = wio-e5 +;default_envs = radiomaster_900_bandit_nano +;default_envs = radiomaster_900_bandit_micro +;default_envs = heltec_capsule_sensor_v3 extra_configs = arch/*/*.ini @@ -70,12 +73,13 @@ build_flags = -Wno-missing-field-initializers -DRADIOLIB_EXCLUDE_FSK4 -DRADIOLIB_EXCLUDE_APRS -DRADIOLIB_EXCLUDE_LORAWAN + -DMESHTASTIC_EXCLUDE_DROPZONE=1 monitor_speed = 115200 lib_deps = jgromes/RadioLib@~6.6.0 - https://github.com/meshtastic/esp8266-oled-ssd1306.git#ee628ee6c9588d4c56c9e3da35f0fc9448ad54a8 ; ESP8266_SSD1306 + https://github.com/meshtastic/esp8266-oled-ssd1306.git#69ba98fa30e67b12d4577b121f210f3eb7049d6b ; ESP8266_SSD1306 mathertel/OneButton@^2.5.0 ; OneButton library for non-blocking button debounce https://github.com/meshtastic/arduino-fsm.git#7db3702bf0cfe97b783d6c72595e3f38e0b19159 https://github.com/meshtastic/TinyGPSPlus.git#71a82db35f3b973440044c476d4bcdc673b104f4 @@ -118,10 +122,7 @@ lib_deps = adafruit/Adafruit BMP280 Library@^2.6.8 adafruit/Adafruit BMP085 Library@^1.2.4 adafruit/Adafruit BME280 Library@^2.2.2 - https://github.com/boschsensortec/Bosch-BSEC2-Library#v1.7.2502 - boschsensortec/BME68x Sensor Library@^1.1.40407 adafruit/Adafruit MCP9808 Library@^2.0.0 - https://github.com/KodinLanewave/INA3221@^1.0.0 adafruit/Adafruit INA260 Library@^1.5.0 adafruit/Adafruit INA219@^1.2.0 adafruit/Adafruit SHTC3 Library@^1.0.0 @@ -131,13 +132,22 @@ lib_deps = adafruit/Adafruit MPU6050@^2.2.4 adafruit/Adafruit LIS3DH@^1.2.4 adafruit/Adafruit AHTX0@^2.0.5 - lewisxhe/SensorLib@^0.2.0 adafruit/Adafruit LSM6DS@^4.7.2 - mprograms/QMC5883LCompass@^1.2.0 adafruit/Adafruit VEML7700 Library@^2.1.6 adafruit/Adafruit SHT4x Library@^1.0.4 adafruit/Adafruit TSL2591 Library@^1.4.5 + sparkfun/SparkFun Qwiic Scale NAU7802 Arduino Library@^1.0.5 ClosedCube OPT3001@^1.1.2 emotibit/EmotiBit MLX90632@^1.0.8 dfrobot/DFRobot_RTU@^1.0.3 - https://github.com/meshtastic/DFRobot_LarkWeatherStation#0e884fc86b7a0b602c7ff3d26b893b997f15c6ac \ No newline at end of file + + + https://github.com/boschsensortec/Bosch-BSEC2-Library#v1.7.2502 + boschsensortec/BME68x Sensor Library@^1.1.40407 + https://github.com/KodinLanewave/INA3221@^1.0.0 + lewisxhe/SensorLib@^0.2.0 + mprograms/QMC5883LCompass@^1.2.0 + + + https://github.com/meshtastic/DFRobot_LarkWeatherStation#dee914270dc7cb3e43fbf034edd85a63a16a12ee + diff --git a/protobufs b/protobufs index a641c5ce4..dc066c89f 160000 --- a/protobufs +++ b/protobufs @@ -1 +1 @@ -Subproject commit a641c5ce4fca158d18ca3cffc92ac7a10f9b6a04 +Subproject commit dc066c89f73fce882e5a47648cba18a1967a7f56 diff --git a/src/AccelerometerThread.h b/src/AccelerometerThread.h index ad40cd9bd..f45511cca 100644 --- a/src/AccelerometerThread.h +++ b/src/AccelerometerThread.h @@ -14,6 +14,10 @@ #include #include #include +#ifdef RAK_4631 +#include "Fusion/Fusion.h" +#include +#endif #define ACCELEROMETER_CHECK_INTERVAL_MS 100 #define ACCELEROMETER_CLICK_THRESHOLD 40 @@ -50,12 +54,13 @@ class AccelerometerThread : public concurrency::OSThread return; } acceleremoter_type = type; - +#ifndef RAK_4631 if (!config.display.wake_on_tap_or_motion && !config.device.double_tap_as_button_press) { LOG_DEBUG("AccelerometerThread disabling due to no interested configurations\n"); disable(); return; } +#endif init(); } @@ -87,6 +92,72 @@ class AccelerometerThread : public concurrency::OSThread wakeScreen(); return 500; } +#ifdef RAK_4631 + } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160) { + sBmx160SensorData_t magAccel; + sBmx160SensorData_t gAccel; + + /* Get a new sensor event */ + bmx160.getAllData(&magAccel, NULL, &gAccel); + + // expirimental calibrate routine. Limited to between 10 and 30 seconds after boot + if (millis() > 10 * 1000 && millis() < 30 * 1000) { + if (magAccel.x > highestX) + highestX = magAccel.x; + if (magAccel.x < lowestX) + lowestX = magAccel.x; + if (magAccel.y > highestY) + highestY = magAccel.y; + if (magAccel.y < lowestY) + lowestY = magAccel.y; + if (magAccel.z > highestZ) + highestZ = magAccel.z; + if (magAccel.z < lowestZ) + lowestZ = magAccel.z; + } + + int highestRealX = highestX - (highestX + lowestX) / 2; + + magAccel.x -= (highestX + lowestX) / 2; + magAccel.y -= (highestY + lowestY) / 2; + magAccel.z -= (highestZ + lowestZ) / 2; + FusionVector ga, ma; + ga.axis.x = -gAccel.x; // default location for the BMX160 is on the rear of the board + ga.axis.y = -gAccel.y; + ga.axis.z = gAccel.z; + ma.axis.x = -magAccel.x; + ma.axis.y = -magAccel.y; + ma.axis.z = magAccel.z * 3; + + // If we're set to one of the inverted positions + if (config.display.compass_orientation > meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270) { + ma = FusionAxesSwap(ma, FusionAxesAlignmentNXNYPZ); + ga = FusionAxesSwap(ga, FusionAxesAlignmentNXNYPZ); + } + + float heading = FusionCompassCalculateHeading(FusionConventionNed, ga, ma); + + switch (config.display.compass_orientation) { + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0: + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED: + heading += 90; + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED: + heading += 180; + break; + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270: + case meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED: + heading += 270; + break; + } + + screen->setHeading(heading); + +#endif } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.shake()) { wakeScreen(); return 500; @@ -149,6 +220,11 @@ class AccelerometerThread : public concurrency::OSThread bmaSensor.enableTiltIRQ(); // It corresponds to isDoubleClick interrupt bmaSensor.enableWakeupIRQ(); +#ifdef RAK_4631 + } else if (acceleremoter_type == ScanI2C::DeviceType::BMX160 && bmx160.begin()) { + bmx160.ODR_Config(BMX160_ACCEL_ODR_100HZ, BMX160_GYRO_ODR_100HZ); // set output data rate + +#endif } else if (acceleremoter_type == ScanI2C::DeviceType::LSM6DS3 && lsm.begin_I2C(accelerometer_found.address)) { LOG_DEBUG("LSM6DS3 initializing\n"); // Default threshold of 2G, less sensitive options are 4, 8 or 16G @@ -179,6 +255,10 @@ class AccelerometerThread : public concurrency::OSThread Adafruit_LIS3DH lis; Adafruit_LSM6DS3TRC lsm; SensorBMA423 bmaSensor; +#ifdef RAK_4631 + RAK_BMX160 bmx160; + float highestX = 0, lowestX = 0, highestY = 0, lowestY = 0, highestZ = 0, lowestZ = 0; +#endif bool BMA_IRQ = false; }; diff --git a/src/ButtonThread.cpp b/src/ButtonThread.cpp index 7e678d69d..4b3bb3fbc 100644 --- a/src/ButtonThread.cpp +++ b/src/ButtonThread.cpp @@ -41,7 +41,11 @@ ButtonThread::ButtonThread() : OSThread("Button") } #elif defined(BUTTON_PIN) int pin = config.device.button_gpio ? config.device.button_gpio : BUTTON_PIN; // Resolved button pin +#if defined(HELTEC_CAPSULE_SENSOR_V3) + this->userButton = OneButton(pin, false, false); +#else this->userButton = OneButton(pin, true, true); +#endif LOG_DEBUG("Using GPIO%02d for button\n", pin); #endif diff --git a/src/Fusion/Fusion.h b/src/Fusion/Fusion.h new file mode 100644 index 000000000..48f5198c5 --- /dev/null +++ b/src/Fusion/Fusion.h @@ -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 diff --git a/src/Fusion/FusionAhrs.c b/src/Fusion/FusionAhrs.c new file mode 100644 index 000000000..d6c1d0215 --- /dev/null +++ b/src/Fusion/FusionAhrs.c @@ -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 // FLT_MAX +#include // 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 diff --git a/src/Fusion/FusionAhrs.h b/src/Fusion/FusionAhrs.h new file mode 100644 index 000000000..aa2326e43 --- /dev/null +++ b/src/Fusion/FusionAhrs.h @@ -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 + +//------------------------------------------------------------------------------ +// 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 diff --git a/src/Fusion/FusionAxes.h b/src/Fusion/FusionAxes.h new file mode 100644 index 000000000..9673c88ff --- /dev/null +++ b/src/Fusion/FusionAxes.h @@ -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 diff --git a/src/Fusion/FusionCalibration.h b/src/Fusion/FusionCalibration.h new file mode 100644 index 000000000..be7102b73 --- /dev/null +++ b/src/Fusion/FusionCalibration.h @@ -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 diff --git a/src/Fusion/FusionCompass.c b/src/Fusion/FusionCompass.c new file mode 100644 index 000000000..6a6f9591a --- /dev/null +++ b/src/Fusion/FusionCompass.c @@ -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 // 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 diff --git a/src/Fusion/FusionCompass.h b/src/Fusion/FusionCompass.h new file mode 100644 index 000000000..a3d0b466a --- /dev/null +++ b/src/Fusion/FusionCompass.h @@ -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 diff --git a/src/Fusion/FusionConvention.h b/src/Fusion/FusionConvention.h new file mode 100644 index 000000000..0b0d43adc --- /dev/null +++ b/src/Fusion/FusionConvention.h @@ -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 diff --git a/src/Fusion/FusionMath.h b/src/Fusion/FusionMath.h new file mode 100644 index 000000000..c3fc34b2d --- /dev/null +++ b/src/Fusion/FusionMath.h @@ -0,0 +1,503 @@ +/** + * @file FusionMath.h + * @author Seb Madgwick + * @brief Math library. + */ + +#ifndef FUSION_MATH_H +#define FUSION_MATH_H + +//------------------------------------------------------------------------------ +// Includes + +#include // M_PI, sqrtf, atan2f, asinf +#include +#include + +//------------------------------------------------------------------------------ +// 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 diff --git a/src/Fusion/FusionOffset.c b/src/Fusion/FusionOffset.c new file mode 100644 index 000000000..d4334c874 --- /dev/null +++ b/src/Fusion/FusionOffset.c @@ -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 // 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 diff --git a/src/Fusion/FusionOffset.h b/src/Fusion/FusionOffset.h new file mode 100644 index 000000000..51ae4a896 --- /dev/null +++ b/src/Fusion/FusionOffset.h @@ -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 diff --git a/src/Power.cpp b/src/Power.cpp index b80d8a0d5..d80bfd55c 100644 --- a/src/Power.cpp +++ b/src/Power.cpp @@ -335,13 +335,20 @@ class AnalogBatteryLevel : public HasBatteryLevel virtual bool isVbusIn() override { #ifdef EXT_PWR_DETECT - // if external powered that pin will be pulled up - if (digitalRead(EXT_PWR_DETECT) == HIGH) { - return true; - } - // if it's not HIGH - check the battery + #ifdef HELTEC_CAPSULE_SENSOR_V3 + // if external powered that pin will be pulled down + if (digitalRead(EXT_PWR_DETECT) == LOW) { + return true; + } + // if it's not LOW - check the battery + #else + // if external powered that pin will be pulled up + if (digitalRead(EXT_PWR_DETECT) == HIGH) { + return true; + } + // if it's not HIGH - check the battery + #endif #endif - return getBattVoltage() > chargingVolt; } @@ -421,7 +428,11 @@ Power::Power() : OSThread("Power") bool Power::analogInit() { #ifdef EXT_PWR_DETECT - pinMode(EXT_PWR_DETECT, INPUT); + #ifdef HELTEC_CAPSULE_SENSOR_V3 + pinMode(EXT_PWR_DETECT, INPUT_PULLUP); + #else + pinMode(EXT_PWR_DETECT, INPUT); + #endif #endif #ifdef EXT_CHRG_DETECT pinMode(EXT_CHRG_DETECT, ext_chrg_detect_mode); diff --git a/src/configuration.h b/src/configuration.h index a8b059d2c..3d10feeaa 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -142,6 +142,7 @@ along with this program. If not, see . #define OPT3001_ADDR_ALT 0x44 #define MLX90632_ADDR 0x3A #define DFROBOT_LARK_ADDR 0x42 +#define NAU7802_ADDR 0x2A // ----------------------------------------------------------------------------- // ACCELEROMETER @@ -150,6 +151,7 @@ along with this program. If not, see . #define LIS3DH_ADR 0x18 #define BMA423_ADDR 0x19 #define LSM6DS3_ADDR 0x6A +#define BMX160_ADDR 0x69 // ----------------------------------------------------------------------------- // LED diff --git a/src/detect/ScanI2C.cpp b/src/detect/ScanI2C.cpp index 149bb95f0..3231f7054 100644 --- a/src/detect/ScanI2C.cpp +++ b/src/detect/ScanI2C.cpp @@ -36,8 +36,8 @@ ScanI2C::FoundDevice ScanI2C::firstKeyboard() const ScanI2C::FoundDevice ScanI2C::firstAccelerometer() const { - ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3}; - return firstOfOrNONE(4, types); + ScanI2C::DeviceType types[] = {MPU6050, LIS3DH, BMA423, LSM6DS3, BMX160}; + return firstOfOrNONE(5, types); } ScanI2C::FoundDevice ScanI2C::find(ScanI2C::DeviceType) const diff --git a/src/detect/ScanI2C.h b/src/detect/ScanI2C.h index 13dd66763..dcc1f40ae 100644 --- a/src/detect/ScanI2C.h +++ b/src/detect/ScanI2C.h @@ -49,7 +49,9 @@ class ScanI2C OPT3001, MLX90632, AHT10, + BMX160, DFROBOT_LARK, + NAU7802 } DeviceType; // typedef uint8_t DeviceAddress; diff --git a/src/detect/ScanI2CTwoWire.cpp b/src/detect/ScanI2CTwoWire.cpp index 86099ad19..6766db014 100644 --- a/src/detect/ScanI2CTwoWire.cpp +++ b/src/detect/ScanI2CTwoWire.cpp @@ -342,6 +342,7 @@ void ScanI2CTwoWire::scanPort(I2CPort port) SCAN_SIMPLE_CASE(PMSA0031_ADDR, PMSA0031, "PMSA0031 air quality sensor found\n") SCAN_SIMPLE_CASE(MPU6050_ADDR, MPU6050, "MPU6050 accelerometer found\n"); + SCAN_SIMPLE_CASE(BMX160_ADDR, BMX160, "BMX160 accelerometer found\n"); SCAN_SIMPLE_CASE(BMA423_ADDR, BMA423, "BMA423 accelerometer found\n"); SCAN_SIMPLE_CASE(LSM6DS3_ADDR, LSM6DS3, "LSM6DS3 accelerometer found at address 0x%x\n", (uint8_t)addr.address); SCAN_SIMPLE_CASE(TCA9555_ADDR, TCA9555, "TCA9555 I2C expander found\n"); @@ -349,6 +350,7 @@ void ScanI2CTwoWire::scanPort(I2CPort port) SCAN_SIMPLE_CASE(TSL25911_ADDR, TSL2591, "TSL2591 light sensor found\n"); SCAN_SIMPLE_CASE(OPT3001_ADDR, OPT3001, "OPT3001 light sensor found\n"); SCAN_SIMPLE_CASE(MLX90632_ADDR, MLX90632, "MLX90632 IR temp sensor found\n"); + SCAN_SIMPLE_CASE(NAU7802_ADDR, NAU7802, "NAU7802 based scale found\n"); default: LOG_INFO("Device found at address 0x%x was not able to be enumerated\n", addr.address); diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 17088910a..8d46742ba 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -28,6 +28,12 @@ #define GPS_STANDBY_THRESHOLD_MINUTES 15 #endif +// How many seconds of sleep make it worthwhile for the GPS to use powered-on standby +// Shorter than this, and we'll just wait instead +#ifndef GPS_IDLE_THRESHOLD_SECONDS +#define GPS_IDLE_THRESHOLD_SECONDS 10 +#endif + #if defined(NRF52840_XXAA) || defined(NRF52833_XXAA) || defined(ARCH_ESP32) || defined(ARCH_PORTDUINO) HardwareSerial *GPS::_serial_gps = &Serial1; #else @@ -776,14 +782,22 @@ void GPS::setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime) { // Record the current powerState if (on) - powerState = GPS_AWAKE; - else if (!on && standbyOnly) + powerState = GPS_ACTIVE; + else if (!enabled) // User has disabled with triple press + powerState = GPS_OFF; + else if (sleepTime <= GPS_IDLE_THRESHOLD_SECONDS * 1000UL) + powerState = GPS_IDLE; + else if (standbyOnly) powerState = GPS_STANDBY; else powerState = GPS_OFF; LOG_DEBUG("GPS::powerState=%d\n", powerState); + // If the next update is due *really soon*, don't actually power off or enter standby. Just wait it out. + if (!on && powerState == GPS_IDLE) + return; + if (on) { clearBuffer(); // drop any old data waiting in the buffer before re-enabling if (en_gpio) @@ -880,54 +894,69 @@ void GPS::setConnected() void GPS::setAwake(bool wantAwake) { - // If user has disabled GPS, make sure it is off, not just in standby + // If user has disabled GPS, make sure it is off, not just in standby or idle if (!wantAwake && !enabled && powerState != GPS_OFF) { setGPSPower(false, false, 0); return; } // If GPS power state needs to change - if ((wantAwake && powerState != GPS_AWAKE) || (!wantAwake && powerState == GPS_AWAKE)) { + if ((wantAwake && powerState != GPS_ACTIVE) || (!wantAwake && powerState == GPS_ACTIVE)) { LOG_DEBUG("WANT GPS=%d\n", wantAwake); // Calculate how long it takes to get a GPS lock if (wantAwake) { + // Record the time we start looking for a lock lastWakeStartMsec = millis(); } else { + // Record by how much we missed our ideal target postion.gps_update_interval (for logging only) + // Need to calculate this before we update lastSleepStartMsec, to make the new prediction + int32_t lateByMsec = (int32_t)(millis() - lastSleepStartMsec) - (int32_t)getSleepTime(); + + // Record the time we finish looking for a lock lastSleepStartMsec = millis(); - if (GPSCycles == 1) { // Skipping initial lock time, as it will likely be much longer than average - averageLockTime = lastSleepStartMsec - lastWakeStartMsec; - } else if (GPSCycles > 1) { - averageLockTime += ((int32_t)(lastSleepStartMsec - lastWakeStartMsec) - averageLockTime) / (int32_t)GPSCycles; + + // How long did it take to get GPS lock this time? + uint32_t lockTime = lastSleepStartMsec - lastWakeStartMsec; + + // Update the lock-time prediction + // Used pre-emptively, attempting to hit target of gps.position_update_interval + switch (GPSCycles) { + case 0: + LOG_DEBUG("Initial GPS lock took %ds\n", lockTime / 1000); + break; + case 1: + predictedLockTime = lockTime; // Avoid slow ramp-up - start with a real value + LOG_DEBUG("GPS Lock took %ds\n", lockTime / 1000); + break; + default: + // Predict lock-time using exponential smoothing: respond slowly to changes + predictedLockTime = (lockTime * 0.2) + (predictedLockTime * 0.8); // Latest lock time has 20% weight on prediction + LOG_INFO("GPS Lock took %ds. %s by %ds. Next lock predicted to take %ds.\n", lockTime / 1000, + (lateByMsec > 0) ? "Late" : "Early", abs(lateByMsec) / 1000, predictedLockTime / 1000); } GPSCycles++; - LOG_DEBUG("GPS Lock took %d, average %d\n", (lastSleepStartMsec - lastWakeStartMsec) / 1000, averageLockTime / 1000); } + // How long to wait before attempting next GPS update + // Aims to hit position.gps_update_interval by using the lock-time prediction + uint32_t compensatedSleepTime = (getSleepTime() > predictedLockTime) ? (getSleepTime() - predictedLockTime) : 0; + // If long interval between updates: power off between updates - if ((int32_t)getSleepTime() - averageLockTime > GPS_STANDBY_THRESHOLD_MINUTES * MS_IN_MINUTE) { - setGPSPower(wantAwake, false, getSleepTime() - averageLockTime); - return; + if (compensatedSleepTime > GPS_STANDBY_THRESHOLD_MINUTES * MS_IN_MINUTE) { + setGPSPower(wantAwake, false, getSleepTime() - predictedLockTime); } - // If waking frequently: standby only. Would use more power trying to reacquire lock each time - else if ((int32_t)getSleepTime() - averageLockTime > 10000) { // 10 seconds is enough for standby + // If waking relatively frequently: don't power off. Would use more energy trying to reacquire lock each time + // We'll either use a "powered-on" standby, or just wait it out, depending on how soon the next update is due + // Will decide which inside setGPSPower method + else { #ifdef GPS_UC6580 - setGPSPower(wantAwake, false, getSleepTime() - averageLockTime); + setGPSPower(wantAwake, false, compensatedSleepTime); #else - setGPSPower(wantAwake, true, getSleepTime() - averageLockTime); + setGPSPower(wantAwake, true, compensatedSleepTime); #endif - return; } - - // Gradually recover from an abnormally long "time to get lock" - if (averageLockTime > 20000) { - averageLockTime -= 1000; // eventually want to sleep again. - } - - // Make sure we don't have a fallthrough where GPS is stuck off - if (wantAwake) - setGPSPower(true, true, 0); } } @@ -1033,14 +1062,14 @@ int32_t GPS::runOnce() uint32_t timeAsleep = now - lastSleepStartMsec; auto sleepTime = getSleepTime(); - if (powerState != GPS_AWAKE && (sleepTime != UINT32_MAX) && - ((timeAsleep > sleepTime) || (isInPowersave && timeAsleep > (sleepTime - averageLockTime)))) { + if (powerState != GPS_ACTIVE && (sleepTime != UINT32_MAX) && + ((timeAsleep > sleepTime) || (isInPowersave && timeAsleep > (sleepTime - predictedLockTime)))) { // We now want to be awake - so wake up the GPS setAwake(true); } // While we are awake - if (powerState == GPS_AWAKE) { + if (powerState == GPS_ACTIVE) { // LOG_DEBUG("looking for location\n"); // If we've already set time from the GPS, no need to ask the GPS bool gotTime = (getRTCQuality() >= RTCQualityGPS); @@ -1086,7 +1115,7 @@ int32_t GPS::runOnce() // 9600bps is approx 1 byte per msec, so considering our buffer size we never need to wake more often than 200ms // if not awake we can run super infrquently (once every 5 secs?) to see if we need to wake. - return (powerState == GPS_AWAKE) ? GPS_THREAD_INTERVAL : 5000; + return (powerState == GPS_ACTIVE) ? GPS_THREAD_INTERVAL : 5000; } // clear the GPS rx buffer as quickly as possible @@ -1617,9 +1646,9 @@ bool GPS::whileIdle() { unsigned int charsInBuf = 0; bool isValid = false; - if (powerState != GPS_AWAKE) { + if (powerState != GPS_ACTIVE) { clearBuffer(); - return (powerState == GPS_AWAKE); + return (powerState == GPS_ACTIVE); } #ifdef SERIAL_BUFFER_SIZE if (_serial_gps->available() >= SERIAL_BUFFER_SIZE - 1) { @@ -1650,6 +1679,10 @@ bool GPS::whileIdle() } void GPS::enable() { + // Clear the old lock-time prediction + GPSCycles = 0; + predictedLockTime = 0; + enabled = true; setInterval(GPS_THREAD_INTERVAL); setAwake(true); diff --git a/src/gps/GPS.h b/src/gps/GPS.h index e9ec111a7..55bd42d0f 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -39,9 +39,10 @@ typedef enum { } GPS_RESPONSE; enum GPSPowerState : uint8_t { - GPS_OFF = 0, - GPS_AWAKE = 1, - GPS_STANDBY = 2, + GPS_OFF = 0, // Physically powered off + GPS_ACTIVE = 1, // Awake and want a position + GPS_STANDBY = 2, // Physically powered on, but soft-sleeping + GPS_IDLE = 3, // Awake, but not wanting another position yet }; // Generate a string representation of DOP @@ -72,7 +73,7 @@ class GPS : private concurrency::OSThread uint32_t rx_gpio = 0; uint32_t tx_gpio = 0; uint32_t en_gpio = 0; - int32_t averageLockTime = 0; + uint32_t predictedLockTime = 0; uint32_t GPSCycles = 0; int speedSelect = 0; @@ -93,7 +94,7 @@ class GPS : private concurrency::OSThread bool GPSInitFinished = false; // Init thread finished? bool GPSInitStarted = false; // Init thread finished? - GPSPowerState powerState = GPS_OFF; // GPS_AWAKE if we want a location right now + GPSPowerState powerState = GPS_OFF; // GPS_ACTIVE if we want a location right now uint8_t numSatellites = 0; diff --git a/src/gps/ubx.h b/src/gps/ubx.h index 0a382a8a3..0852c331d 100644 --- a/src/gps/ubx.h +++ b/src/gps/ubx.h @@ -319,6 +319,8 @@ const uint8_t GPS::_message_SAVE[] = { // 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: @@ -327,40 +329,42 @@ const uint8_t GPS::_message_SAVE[] = { // 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 + /* CFG-PM2 has been replaced by many CFG-PM commands -OPERATEMODE E1 2 (0 | 1 | 2) -POSUPDATEPERIOD U4 1000ms for M10 must be >= 5s try 5 -ACQPERIOD U4 10 seems ok for M10 def ok -GRIDOFFSET U4 0 seems ok for M10 def ok -ONTIME U2 1 will try 1 -MINACQTIME U1 0 will try 0 def ok -MAXACQTIME U1 stick with default of 0 def ok -DONOTENTEROFF L 1 stay at 1 -WAITTIMEFIX L 1 stay with 1 -UPDATEEPH L 1 changed to 1 for gps rework default is 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 stay with 1 -*/ -// CFG-PMS has been removed +CFG-PMS has been removed + +CFG-PM-OPERATEMODE E1 (0 | 1 | 2) -> 1 (PSMOO), because sporadic position updates are required instead of continous tracking <10s +(PSMCT) CFG-PM-POSUPDATEPERIOD U4 -> 0ms, no self-timed wakup because receiver power mode is controlled via "software standby +mode" by legacy UBX-RXM-PMREQ request CFG-PM-ACQPERIOD U4 -> 0ms, because receiver power mode is controlled via "software standby +mode" by legacy UBX-RXM-PMREQ request CFG-PM-ONTIME U4 -> 0ms, optional I guess CFG-PM-EXTINTBACKUP L -> 1, force receiver into +BACKUP mode when EXTINT (should be connected to GPS_EN_PIN) pin is "low" + +This is required because the receiver never enters low power mode if microcontroller is in deep-sleep. +Maybe the changing UART_RX levels trigger a wakeup but even with UBX-RXM-PMREQ[12] = 0x00 (all external wakeup sources disabled) +the receivcer remains in aquisition state -> potentially a bug + +Workaround: Control the EXTINT pin by the GPS_EN_PIN signal + +As mentioned in the M10 operational issues down below, power save won't allow the use of BDS B1C. +CFG-SIGNAL-BDS_B1C_ENA L -> 0 // 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 +// 01 01 00 00 01 00 D0 20 01 02 00 D0 40 00 00 00 00 03 00 D0 40 00 00 00 00 05 00 D0 30 00 00 0D 00 D0 10 01 // 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 - -const uint8_t GPS::_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}; -const uint8_t GPS::_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}; +// 01 02 00 00 01 00 D0 20 01 02 00 D0 40 00 00 00 00 03 00 D0 40 00 00 00 00 05 00 D0 30 00 00 0D 00 D0 10 01 +*/ +const uint8_t GPS::_message_VALSET_PM_RAM[] = {0x01, 0x01, 0x00, 0x00, 0x0F, 0x00, 0x31, 0x10, 0x00, 0x01, 0x00, 0xD0, 0x20, 0x01, + 0x02, 0x00, 0xD0, 0x40, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0xD0, 0x40, 0x00, 0x00, + 0x00, 0x00, 0x05, 0x00, 0xD0, 0x30, 0x00, 0x00, 0x0D, 0x00, 0xD0, 0x10, 0x01}; +const uint8_t GPS::_message_VALSET_PM_BBR[] = {0x01, 0x02, 0x00, 0x00, 0x0F, 0x00, 0x31, 0x10, 0x00, 0x01, 0x00, 0xD0, 0x20, 0x01, + 0x02, 0x00, 0xD0, 0x40, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0xD0, 0x40, 0x00, 0x00, + 0x00, 0x00, 0x05, 0x00, 0xD0, 0x30, 0x00, 0x00, 0x0D, 0x00, 0xD0, 0x10, 0x01}; /* CFG-ITFM replaced by 5 valset messages which can be combined into one for RAM and one for BBR diff --git a/src/graphics/Screen.cpp b/src/graphics/Screen.cpp index 1c9484f62..60168cffc 100644 --- a/src/graphics/Screen.cpp +++ b/src/graphics/Screen.cpp @@ -277,6 +277,30 @@ static void drawFunctionOverlay(OLEDDisplay *display, OLEDDisplayUiState *state) } } +/// Check if the display can render a string (detect special chars; emoji) +static bool haveGlyphs(const char *str) +{ +#if defined(OLED_UA) || defined(OLED_RU) + // Don't want to make any assumptions about custom language support + return true; +#endif + + // Check each character with the lookup function for the OLED library + // We're not really meant to use this directly.. + bool have = true; + for (uint16_t i = 0; i < strlen(str); i++) { + uint8_t result = Screen::customFontTableLookup((uint8_t)str[i]); + // If font doesn't support a character, it is substituted for ¿ + if (result == 191 && (uint8_t)str[i] != 191) { + have = false; + break; + } + } + + LOG_DEBUG("haveGlyphs=%d\n", have); + return have; +} + #ifdef USE_EINK /// Used on eink displays while in deep sleep static void drawDeepSleepScreen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y) @@ -301,14 +325,15 @@ static void drawScreensaverOverlay(OLEDDisplay *display, OLEDDisplayUiState *sta display->setTextAlignment(TEXT_ALIGN_LEFT); const char *pauseText = "Screen Paused"; const char *idText = owner.short_name; + const bool useId = haveGlyphs(idText); // This bool is used to hide the idText box if we can't render the short name constexpr uint16_t padding = 5; constexpr uint8_t dividerGap = 1; constexpr uint8_t imprecision = 5; // How far the box origins can drift from center. Combat burn-in. // Dimensions - const uint16_t idTextWidth = display->getStringWidth(idText, strlen(idText)); + const uint16_t idTextWidth = display->getStringWidth(idText, strlen(idText), true); // "true": handle utf8 chars const uint16_t pauseTextWidth = display->getStringWidth(pauseText, strlen(pauseText)); - const uint16_t boxWidth = padding + idTextWidth + padding + padding + pauseTextWidth + padding; + const uint16_t boxWidth = padding + (useId ? idTextWidth + padding + padding : 0) + pauseTextWidth + padding; const uint16_t boxHeight = padding + FONT_HEIGHT_SMALL + padding; // Position @@ -318,7 +343,7 @@ static void drawScreensaverOverlay(OLEDDisplay *display, OLEDDisplayUiState *sta const int16_t boxBottom = boxTop + boxHeight - 1; const int16_t idTextLeft = boxLeft + padding; const int16_t idTextTop = boxTop + padding; - const int16_t pauseTextLeft = boxLeft + padding + idTextWidth + padding + padding; + const int16_t pauseTextLeft = boxLeft + (useId ? padding + idTextWidth + padding : 0) + padding; const int16_t pauseTextTop = boxTop + padding; const int16_t dividerX = boxLeft + padding + idTextWidth + padding; const int16_t dividerTop = boxTop + 1 + dividerGap; @@ -331,12 +356,14 @@ static void drawScreensaverOverlay(OLEDDisplay *display, OLEDDisplayUiState *sta display->drawRect(boxLeft, boxTop, boxWidth, boxHeight); // Draw: Text - display->drawString(idTextLeft, idTextTop, idText); + if (useId) + display->drawString(idTextLeft, idTextTop, idText); display->drawString(pauseTextLeft, pauseTextTop, pauseText); display->drawString(pauseTextLeft + 1, pauseTextTop, pauseText); // Faux bold // Draw: divider - display->drawLine(dividerX, dividerTop, dividerX, dividerBottom); + if (useId) + display->drawLine(dividerX, dividerTop, dividerX, dividerBottom); } #endif @@ -1516,9 +1543,13 @@ static void drawNodeInfo(OLEDDisplay *display, OLEDDisplayUiState *state, int16_ } bool hasNodeHeading = false; - if (ourNode && hasValidPosition(ourNode)) { + if (ourNode && (hasValidPosition(ourNode) || screen->hasHeading())) { const meshtastic_PositionLite &op = ourNode->position; - float myHeading = estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i)); + float myHeading; + if (screen->hasHeading()) + myHeading = (screen->getHeading()) * PI / 180; // gotta convert compass degrees to Radians + else + myHeading = estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i)); drawCompassNorth(display, compassX, compassY, myHeading); if (hasValidPosition(node)) { diff --git a/src/graphics/Screen.h b/src/graphics/Screen.h index 7f8d078e7..f4d719715 100644 --- a/src/graphics/Screen.h +++ b/src/graphics/Screen.h @@ -204,6 +204,17 @@ class Screen : public concurrency::OSThread enqueueCmd(cmd); } + // Function to allow the AccelerometerThread to set the heading if a sensor provides it + // Mutex needed? + void setHeading(long _heading) + { + hasCompass = true; + compassHeading = _heading; + } + + bool hasHeading() { return hasCompass; } + + long getHeading() { return compassHeading; } // functions for display brightness void increaseBrightness(); void decreaseBrightness(); @@ -428,6 +439,8 @@ class Screen : public concurrency::OSThread // Implementation to Adjust Brightness uint8_t brightness = BRIGHTNESS_DEFAULT; // H = 254, MH = 192, ML = 130 L = 103 + bool hasCompass = false; + float compassHeading; /// Holds state for debug information DebugInfo debugInfo; diff --git a/src/graphics/TFTDisplay.cpp b/src/graphics/TFTDisplay.cpp index b19e402b8..8ea90c523 100644 --- a/src/graphics/TFTDisplay.cpp +++ b/src/graphics/TFTDisplay.cpp @@ -117,8 +117,16 @@ class LGFX : public lgfx::LGFX_Device static LGFX *tft = nullptr; #elif defined(RAK14014) +#include #include TFT_eSPI *tft = nullptr; +FT6336U ft6336u; + +static uint8_t _rak14014_touch_int = false; // TP interrupt generation flag. +static void rak14014_tpIntHandle(void) +{ + _rak14014_touch_int = true; +} #elif defined(ST7789_CS) #include // Graphics and font library for ST7735 driver chip @@ -642,8 +650,12 @@ void TFTDisplay::sendCommand(uint8_t com) void TFTDisplay::setDisplayBrightness(uint8_t _brightness) { +#ifdef RAK14014 + // todo +#else tft->setBrightness(_brightness); LOG_DEBUG("Brightness is set to value: %i \n", _brightness); +#endif } void TFTDisplay::flipScreenVertically() @@ -657,6 +669,7 @@ void TFTDisplay::flipScreenVertically() bool TFTDisplay::hasTouch(void) { #ifdef RAK14014 + return true; #elif !defined(M5STACK) return tft->touch() != nullptr; #else @@ -667,6 +680,15 @@ bool TFTDisplay::hasTouch(void) bool TFTDisplay::getTouch(int16_t *x, int16_t *y) { #ifdef RAK14014 + if (_rak14014_touch_int) { + _rak14014_touch_int = false; + /* The X and Y axes have to be switched */ + *y = ft6336u.read_touch1_x(); + *x = TFT_HEIGHT - ft6336u.read_touch1_y(); + return true; + } else { + return false; + } #elif !defined(M5STACK) return tft->getTouch(x, y); #else @@ -716,7 +738,10 @@ bool TFTDisplay::connect() #elif defined(RAK14014) tft->setRotation(1); tft->setSwapBytes(true); -// tft->fillScreen(TFT_BLACK); + // tft->fillScreen(TFT_BLACK); + ft6336u.begin(); + pinMode(SCREEN_TOUCH_INT, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(SCREEN_TOUCH_INT), rak14014_tpIntHandle, FALLING); #elif defined(T_DECK) || defined(PICOMPUTER_S3) || defined(CHATTER_2) tft->setRotation(1); // T-Deck has the TFT in landscape #elif defined(T_WATCH_S3) diff --git a/src/main.h b/src/main.h index db05a4734..2ef7edb3a 100644 --- a/src/main.h +++ b/src/main.h @@ -53,6 +53,9 @@ extern Adafruit_DRV2605 drv; extern AudioThread *audioThread; #endif +// Global Screen singleton. +extern graphics::Screen *screen; + #if !defined(ARCH_PORTDUINO) && !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR #include "AccelerometerThread.h" extern AccelerometerThread *accelerometerThread; @@ -62,9 +65,6 @@ extern bool isVibrating; extern int TCPPort; // set by Portduino -// Global Screen singleton. -extern graphics::Screen *screen; - // extern Observable newPowerStatus; //TODO: move this to main-esp32.cpp somehow or a helper class // extern meshtastic::PowerStatus *powerStatus; diff --git a/src/mesh/generated/meshtastic/admin.pb.h b/src/mesh/generated/meshtastic/admin.pb.h index 2a209ad0a..d0e643dff 100644 --- a/src/mesh/generated/meshtastic/admin.pb.h +++ b/src/mesh/generated/meshtastic/admin.pb.h @@ -135,6 +135,8 @@ typedef struct _meshtastic_AdminMessage { bool enter_dfu_mode_request; /* Delete the file by the specified path from the device */ char delete_file_request[201]; + /* Set zero and offset for scale chips */ + uint32_t set_scale; /* Set the owner for this node */ meshtastic_User set_owner; /* Set channels (using the new API). @@ -238,6 +240,7 @@ extern "C" { #define meshtastic_AdminMessage_get_node_remote_hardware_pins_response_tag 20 #define meshtastic_AdminMessage_enter_dfu_mode_request_tag 21 #define meshtastic_AdminMessage_delete_file_request_tag 22 +#define meshtastic_AdminMessage_set_scale_tag 23 #define meshtastic_AdminMessage_set_owner_tag 32 #define meshtastic_AdminMessage_set_channel_tag 33 #define meshtastic_AdminMessage_set_config_tag 34 @@ -281,6 +284,7 @@ X(a, STATIC, ONEOF, BOOL, (payload_variant,get_node_remote_hardware_pin X(a, STATIC, ONEOF, MESSAGE, (payload_variant,get_node_remote_hardware_pins_response,get_node_remote_hardware_pins_response), 20) \ X(a, STATIC, ONEOF, BOOL, (payload_variant,enter_dfu_mode_request,enter_dfu_mode_request), 21) \ X(a, STATIC, ONEOF, STRING, (payload_variant,delete_file_request,delete_file_request), 22) \ +X(a, STATIC, ONEOF, UINT32, (payload_variant,set_scale,set_scale), 23) \ X(a, STATIC, ONEOF, MESSAGE, (payload_variant,set_owner,set_owner), 32) \ X(a, STATIC, ONEOF, MESSAGE, (payload_variant,set_channel,set_channel), 33) \ X(a, STATIC, ONEOF, MESSAGE, (payload_variant,set_config,set_config), 34) \ diff --git a/src/mesh/generated/meshtastic/config.pb.cpp b/src/mesh/generated/meshtastic/config.pb.cpp index f05e47573..bb82198c0 100644 --- a/src/mesh/generated/meshtastic/config.pb.cpp +++ b/src/mesh/generated/meshtastic/config.pb.cpp @@ -46,3 +46,4 @@ PB_BIND(meshtastic_Config_BluetoothConfig, meshtastic_Config_BluetoothConfig, AU + diff --git a/src/mesh/generated/meshtastic/config.pb.h b/src/mesh/generated/meshtastic/config.pb.h index 0830ed851..781538d11 100644 --- a/src/mesh/generated/meshtastic/config.pb.h +++ b/src/mesh/generated/meshtastic/config.pb.h @@ -182,6 +182,25 @@ typedef enum _meshtastic_Config_DisplayConfig_DisplayMode { meshtastic_Config_DisplayConfig_DisplayMode_COLOR = 3 } meshtastic_Config_DisplayConfig_DisplayMode; +typedef enum _meshtastic_Config_DisplayConfig_CompassOrientation { + /* The compass and the display are in the same orientation. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0 = 0, + /* Rotate the compass by 90 degrees. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90 = 1, + /* Rotate the compass by 180 degrees. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180 = 2, + /* Rotate the compass by 270 degrees. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270 = 3, + /* Don't rotate the compass, but invert the result. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0_INVERTED = 4, + /* Rotate the compass by 90 degrees and invert. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_90_INVERTED = 5, + /* Rotate the compass by 180 degrees and invert. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_180_INVERTED = 6, + /* Rotate the compass by 270 degrees and invert. */ + meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED = 7 +} meshtastic_Config_DisplayConfig_CompassOrientation; + typedef enum _meshtastic_Config_LoRaConfig_RegionCode { /* Region is not set */ meshtastic_Config_LoRaConfig_RegionCode_UNSET = 0, @@ -413,6 +432,8 @@ typedef struct _meshtastic_Config_DisplayConfig { bool heading_bold; /* Should we wake the screen up on accelerometer detected motion or tap */ bool wake_on_tap_or_motion; + /* Indicates how to rotate or invert the compass output to accurate display on the display. */ + meshtastic_Config_DisplayConfig_CompassOrientation compass_orientation; } meshtastic_Config_DisplayConfig; /* Lora Config */ @@ -547,6 +568,10 @@ extern "C" { #define _meshtastic_Config_DisplayConfig_DisplayMode_MAX meshtastic_Config_DisplayConfig_DisplayMode_COLOR #define _meshtastic_Config_DisplayConfig_DisplayMode_ARRAYSIZE ((meshtastic_Config_DisplayConfig_DisplayMode)(meshtastic_Config_DisplayConfig_DisplayMode_COLOR+1)) +#define _meshtastic_Config_DisplayConfig_CompassOrientation_MIN meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_0 +#define _meshtastic_Config_DisplayConfig_CompassOrientation_MAX meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED +#define _meshtastic_Config_DisplayConfig_CompassOrientation_ARRAYSIZE ((meshtastic_Config_DisplayConfig_CompassOrientation)(meshtastic_Config_DisplayConfig_CompassOrientation_DEGREES_270_INVERTED+1)) + #define _meshtastic_Config_LoRaConfig_RegionCode_MIN meshtastic_Config_LoRaConfig_RegionCode_UNSET #define _meshtastic_Config_LoRaConfig_RegionCode_MAX meshtastic_Config_LoRaConfig_RegionCode_SG_923 #define _meshtastic_Config_LoRaConfig_RegionCode_ARRAYSIZE ((meshtastic_Config_LoRaConfig_RegionCode)(meshtastic_Config_LoRaConfig_RegionCode_SG_923+1)) @@ -573,6 +598,7 @@ extern "C" { #define meshtastic_Config_DisplayConfig_units_ENUMTYPE meshtastic_Config_DisplayConfig_DisplayUnits #define meshtastic_Config_DisplayConfig_oled_ENUMTYPE meshtastic_Config_DisplayConfig_OledType #define meshtastic_Config_DisplayConfig_displaymode_ENUMTYPE meshtastic_Config_DisplayConfig_DisplayMode +#define meshtastic_Config_DisplayConfig_compass_orientation_ENUMTYPE meshtastic_Config_DisplayConfig_CompassOrientation #define meshtastic_Config_LoRaConfig_modem_preset_ENUMTYPE meshtastic_Config_LoRaConfig_ModemPreset #define meshtastic_Config_LoRaConfig_region_ENUMTYPE meshtastic_Config_LoRaConfig_RegionCode @@ -587,7 +613,7 @@ extern "C" { #define meshtastic_Config_PowerConfig_init_default {0, 0, 0, 0, 0, 0, 0, 0} #define meshtastic_Config_NetworkConfig_init_default {0, "", "", "", 0, _meshtastic_Config_NetworkConfig_AddressMode_MIN, false, meshtastic_Config_NetworkConfig_IpV4Config_init_default, ""} #define meshtastic_Config_NetworkConfig_IpV4Config_init_default {0, 0, 0, 0} -#define meshtastic_Config_DisplayConfig_init_default {0, _meshtastic_Config_DisplayConfig_GpsCoordinateFormat_MIN, 0, 0, 0, _meshtastic_Config_DisplayConfig_DisplayUnits_MIN, _meshtastic_Config_DisplayConfig_OledType_MIN, _meshtastic_Config_DisplayConfig_DisplayMode_MIN, 0, 0} +#define meshtastic_Config_DisplayConfig_init_default {0, _meshtastic_Config_DisplayConfig_GpsCoordinateFormat_MIN, 0, 0, 0, _meshtastic_Config_DisplayConfig_DisplayUnits_MIN, _meshtastic_Config_DisplayConfig_OledType_MIN, _meshtastic_Config_DisplayConfig_DisplayMode_MIN, 0, 0, _meshtastic_Config_DisplayConfig_CompassOrientation_MIN} #define meshtastic_Config_LoRaConfig_init_default {0, _meshtastic_Config_LoRaConfig_ModemPreset_MIN, 0, 0, 0, 0, _meshtastic_Config_LoRaConfig_RegionCode_MIN, 0, 0, 0, 0, 0, 0, 0, 0, {0, 0, 0}, 0} #define meshtastic_Config_BluetoothConfig_init_default {0, _meshtastic_Config_BluetoothConfig_PairingMode_MIN, 0} #define meshtastic_Config_init_zero {0, {meshtastic_Config_DeviceConfig_init_zero}} @@ -596,7 +622,7 @@ extern "C" { #define meshtastic_Config_PowerConfig_init_zero {0, 0, 0, 0, 0, 0, 0, 0} #define meshtastic_Config_NetworkConfig_init_zero {0, "", "", "", 0, _meshtastic_Config_NetworkConfig_AddressMode_MIN, false, meshtastic_Config_NetworkConfig_IpV4Config_init_zero, ""} #define meshtastic_Config_NetworkConfig_IpV4Config_init_zero {0, 0, 0, 0} -#define meshtastic_Config_DisplayConfig_init_zero {0, _meshtastic_Config_DisplayConfig_GpsCoordinateFormat_MIN, 0, 0, 0, _meshtastic_Config_DisplayConfig_DisplayUnits_MIN, _meshtastic_Config_DisplayConfig_OledType_MIN, _meshtastic_Config_DisplayConfig_DisplayMode_MIN, 0, 0} +#define meshtastic_Config_DisplayConfig_init_zero {0, _meshtastic_Config_DisplayConfig_GpsCoordinateFormat_MIN, 0, 0, 0, _meshtastic_Config_DisplayConfig_DisplayUnits_MIN, _meshtastic_Config_DisplayConfig_OledType_MIN, _meshtastic_Config_DisplayConfig_DisplayMode_MIN, 0, 0, _meshtastic_Config_DisplayConfig_CompassOrientation_MIN} #define meshtastic_Config_LoRaConfig_init_zero {0, _meshtastic_Config_LoRaConfig_ModemPreset_MIN, 0, 0, 0, 0, _meshtastic_Config_LoRaConfig_RegionCode_MIN, 0, 0, 0, 0, 0, 0, 0, 0, {0, 0, 0}, 0} #define meshtastic_Config_BluetoothConfig_init_zero {0, _meshtastic_Config_BluetoothConfig_PairingMode_MIN, 0} @@ -656,6 +682,7 @@ extern "C" { #define meshtastic_Config_DisplayConfig_displaymode_tag 8 #define meshtastic_Config_DisplayConfig_heading_bold_tag 9 #define meshtastic_Config_DisplayConfig_wake_on_tap_or_motion_tag 10 +#define meshtastic_Config_DisplayConfig_compass_orientation_tag 11 #define meshtastic_Config_LoRaConfig_use_preset_tag 1 #define meshtastic_Config_LoRaConfig_modem_preset_tag 2 #define meshtastic_Config_LoRaConfig_bandwidth_tag 3 @@ -778,7 +805,8 @@ X(a, STATIC, SINGULAR, UENUM, units, 6) \ X(a, STATIC, SINGULAR, UENUM, oled, 7) \ X(a, STATIC, SINGULAR, UENUM, displaymode, 8) \ X(a, STATIC, SINGULAR, BOOL, heading_bold, 9) \ -X(a, STATIC, SINGULAR, BOOL, wake_on_tap_or_motion, 10) +X(a, STATIC, SINGULAR, BOOL, wake_on_tap_or_motion, 10) \ +X(a, STATIC, SINGULAR, UENUM, compass_orientation, 11) #define meshtastic_Config_DisplayConfig_CALLBACK NULL #define meshtastic_Config_DisplayConfig_DEFAULT NULL @@ -834,7 +862,7 @@ extern const pb_msgdesc_t meshtastic_Config_BluetoothConfig_msg; #define MESHTASTIC_MESHTASTIC_CONFIG_PB_H_MAX_SIZE meshtastic_Config_size #define meshtastic_Config_BluetoothConfig_size 10 #define meshtastic_Config_DeviceConfig_size 100 -#define meshtastic_Config_DisplayConfig_size 28 +#define meshtastic_Config_DisplayConfig_size 30 #define meshtastic_Config_LoRaConfig_size 80 #define meshtastic_Config_NetworkConfig_IpV4Config_size 20 #define meshtastic_Config_NetworkConfig_size 196 diff --git a/src/mesh/generated/meshtastic/deviceonly.pb.h b/src/mesh/generated/meshtastic/deviceonly.pb.h index a5cbc42a5..0e3e28ba1 100644 --- a/src/mesh/generated/meshtastic/deviceonly.pb.h +++ b/src/mesh/generated/meshtastic/deviceonly.pb.h @@ -308,7 +308,7 @@ extern const pb_msgdesc_t meshtastic_OEMStore_msg; #define MESHTASTIC_MESHTASTIC_DEVICEONLY_PB_H_MAX_SIZE meshtastic_OEMStore_size #define meshtastic_ChannelFile_size 718 #define meshtastic_NodeInfoLite_size 166 -#define meshtastic_OEMStore_size 3368 +#define meshtastic_OEMStore_size 3370 #define meshtastic_PositionLite_size 28 #ifdef __cplusplus diff --git a/src/mesh/generated/meshtastic/localonly.pb.h b/src/mesh/generated/meshtastic/localonly.pb.h index 1b8123ef5..160202d9b 100644 --- a/src/mesh/generated/meshtastic/localonly.pb.h +++ b/src/mesh/generated/meshtastic/localonly.pb.h @@ -181,7 +181,7 @@ extern const pb_msgdesc_t meshtastic_LocalModuleConfig_msg; /* Maximum encoded size of messages (where known) */ #define MESHTASTIC_MESHTASTIC_LOCALONLY_PB_H_MAX_SIZE meshtastic_LocalModuleConfig_size -#define meshtastic_LocalConfig_size 537 +#define meshtastic_LocalConfig_size 539 #define meshtastic_LocalModuleConfig_size 685 #ifdef __cplusplus diff --git a/src/mesh/generated/meshtastic/mesh.pb.h b/src/mesh/generated/meshtastic/mesh.pb.h index ad97cb80f..0e9e6a28d 100644 --- a/src/mesh/generated/meshtastic/mesh.pb.h +++ b/src/mesh/generated/meshtastic/mesh.pb.h @@ -161,6 +161,8 @@ typedef enum _meshtastic_HardwareModel { /* RadioMaster 900 Bandit Nano, https://www.radiomasterrc.com/products/bandit-nano-expresslrs-rf-module ESP32-D0WDQ6 With SX1276/SKY66122, SSD1306 OLED and No GPS */ meshtastic_HardwareModel_RADIOMASTER_900_BANDIT_NANO = 64, + /* Heltec Capsule Sensor V3 with ESP32-S3 CPU, Portable LoRa device that can replace GNSS modules or sensors */ + meshtastic_HardwareModel_HELTEC_CAPSULE_SENSOR_V3 = 65, /* ------------------------------------------------------------------------------------------------------------------------------------------ Reserved ID For developing private Ports. These will show up in live traffic sparsely, so we can use a high number. Keep it within 8 bits. ------------------------------------------------------------------------------------------------------------------------------------------ */ diff --git a/src/mesh/generated/meshtastic/telemetry.pb.cpp b/src/mesh/generated/meshtastic/telemetry.pb.cpp index 6388e37a0..c93483a15 100644 --- a/src/mesh/generated/meshtastic/telemetry.pb.cpp +++ b/src/mesh/generated/meshtastic/telemetry.pb.cpp @@ -21,5 +21,8 @@ PB_BIND(meshtastic_AirQualityMetrics, meshtastic_AirQualityMetrics, AUTO) PB_BIND(meshtastic_Telemetry, meshtastic_Telemetry, AUTO) +PB_BIND(meshtastic_Nau7802Config, meshtastic_Nau7802Config, AUTO) + + diff --git a/src/mesh/generated/meshtastic/telemetry.pb.h b/src/mesh/generated/meshtastic/telemetry.pb.h index 02b0bdd6d..28d368754 100644 --- a/src/mesh/generated/meshtastic/telemetry.pb.h +++ b/src/mesh/generated/meshtastic/telemetry.pb.h @@ -61,7 +61,9 @@ typedef enum _meshtastic_TelemetrySensorType { /* AHT10 Integrated temperature and humidity sensor */ meshtastic_TelemetrySensorType_AHT10 = 23, /* DFRobot Lark Weather station (temperature, humidity, pressure, wind speed and direction) */ - meshtastic_TelemetrySensorType_DFROBOT_LARK = 24 + meshtastic_TelemetrySensorType_DFROBOT_LARK = 24, + /* NAU7802 Scale Chip or compatible */ + meshtastic_TelemetrySensorType_NAU7802 = 25 } meshtastic_TelemetrySensorType; /* Struct definitions */ @@ -111,6 +113,8 @@ typedef struct _meshtastic_EnvironmentMetrics { uint16_t wind_direction; /* Wind speed in m/s */ float wind_speed; + /* Weight in KG */ + float weight; } meshtastic_EnvironmentMetrics; /* Power Metrics (voltage / current / etc) */ @@ -174,6 +178,14 @@ typedef struct _meshtastic_Telemetry { } variant; } meshtastic_Telemetry; +/* NAU7802 Telemetry configuration, for saving to flash */ +typedef struct _meshtastic_Nau7802Config { + /* The offset setting for the NAU7802 */ + int32_t zeroOffset; + /* The calibration factor for the NAU7802 */ + float calibrationFactor; +} meshtastic_Nau7802Config; + #ifdef __cplusplus extern "C" { @@ -181,8 +193,9 @@ extern "C" { /* Helper constants for enums */ #define _meshtastic_TelemetrySensorType_MIN meshtastic_TelemetrySensorType_SENSOR_UNSET -#define _meshtastic_TelemetrySensorType_MAX meshtastic_TelemetrySensorType_DFROBOT_LARK -#define _meshtastic_TelemetrySensorType_ARRAYSIZE ((meshtastic_TelemetrySensorType)(meshtastic_TelemetrySensorType_DFROBOT_LARK+1)) +#define _meshtastic_TelemetrySensorType_MAX meshtastic_TelemetrySensorType_NAU7802 +#define _meshtastic_TelemetrySensorType_ARRAYSIZE ((meshtastic_TelemetrySensorType)(meshtastic_TelemetrySensorType_NAU7802+1)) + @@ -192,15 +205,17 @@ extern "C" { /* Initializer values for message structs */ #define meshtastic_DeviceMetrics_init_default {0, 0, 0, 0, 0} -#define meshtastic_EnvironmentMetrics_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define meshtastic_EnvironmentMetrics_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define meshtastic_PowerMetrics_init_default {0, 0, 0, 0, 0, 0} #define meshtastic_AirQualityMetrics_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define meshtastic_Telemetry_init_default {0, 0, {meshtastic_DeviceMetrics_init_default}} +#define meshtastic_Nau7802Config_init_default {0, 0} #define meshtastic_DeviceMetrics_init_zero {0, 0, 0, 0, 0} -#define meshtastic_EnvironmentMetrics_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define meshtastic_EnvironmentMetrics_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define meshtastic_PowerMetrics_init_zero {0, 0, 0, 0, 0, 0} #define meshtastic_AirQualityMetrics_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define meshtastic_Telemetry_init_zero {0, 0, {meshtastic_DeviceMetrics_init_zero}} +#define meshtastic_Nau7802Config_init_zero {0, 0} /* Field tags (for use in manual encoding/decoding) */ #define meshtastic_DeviceMetrics_battery_level_tag 1 @@ -222,6 +237,7 @@ extern "C" { #define meshtastic_EnvironmentMetrics_uv_lux_tag 12 #define meshtastic_EnvironmentMetrics_wind_direction_tag 13 #define meshtastic_EnvironmentMetrics_wind_speed_tag 14 +#define meshtastic_EnvironmentMetrics_weight_tag 15 #define meshtastic_PowerMetrics_ch1_voltage_tag 1 #define meshtastic_PowerMetrics_ch1_current_tag 2 #define meshtastic_PowerMetrics_ch2_voltage_tag 3 @@ -245,6 +261,8 @@ extern "C" { #define meshtastic_Telemetry_environment_metrics_tag 3 #define meshtastic_Telemetry_air_quality_metrics_tag 4 #define meshtastic_Telemetry_power_metrics_tag 5 +#define meshtastic_Nau7802Config_zeroOffset_tag 1 +#define meshtastic_Nau7802Config_calibrationFactor_tag 2 /* Struct field encoding specification for nanopb */ #define meshtastic_DeviceMetrics_FIELDLIST(X, a) \ @@ -270,7 +288,8 @@ X(a, STATIC, SINGULAR, FLOAT, white_lux, 10) \ X(a, STATIC, SINGULAR, FLOAT, ir_lux, 11) \ X(a, STATIC, SINGULAR, FLOAT, uv_lux, 12) \ X(a, STATIC, SINGULAR, UINT32, wind_direction, 13) \ -X(a, STATIC, SINGULAR, FLOAT, wind_speed, 14) +X(a, STATIC, SINGULAR, FLOAT, wind_speed, 14) \ +X(a, STATIC, SINGULAR, FLOAT, weight, 15) #define meshtastic_EnvironmentMetrics_CALLBACK NULL #define meshtastic_EnvironmentMetrics_DEFAULT NULL @@ -313,11 +332,18 @@ X(a, STATIC, ONEOF, MESSAGE, (variant,power_metrics,variant.power_metrics) #define meshtastic_Telemetry_variant_air_quality_metrics_MSGTYPE meshtastic_AirQualityMetrics #define meshtastic_Telemetry_variant_power_metrics_MSGTYPE meshtastic_PowerMetrics +#define meshtastic_Nau7802Config_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, INT32, zeroOffset, 1) \ +X(a, STATIC, SINGULAR, FLOAT, calibrationFactor, 2) +#define meshtastic_Nau7802Config_CALLBACK NULL +#define meshtastic_Nau7802Config_DEFAULT NULL + extern const pb_msgdesc_t meshtastic_DeviceMetrics_msg; extern const pb_msgdesc_t meshtastic_EnvironmentMetrics_msg; extern const pb_msgdesc_t meshtastic_PowerMetrics_msg; extern const pb_msgdesc_t meshtastic_AirQualityMetrics_msg; extern const pb_msgdesc_t meshtastic_Telemetry_msg; +extern const pb_msgdesc_t meshtastic_Nau7802Config_msg; /* Defines for backwards compatibility with code written before nanopb-0.4.0 */ #define meshtastic_DeviceMetrics_fields &meshtastic_DeviceMetrics_msg @@ -325,14 +351,16 @@ extern const pb_msgdesc_t meshtastic_Telemetry_msg; #define meshtastic_PowerMetrics_fields &meshtastic_PowerMetrics_msg #define meshtastic_AirQualityMetrics_fields &meshtastic_AirQualityMetrics_msg #define meshtastic_Telemetry_fields &meshtastic_Telemetry_msg +#define meshtastic_Nau7802Config_fields &meshtastic_Nau7802Config_msg /* Maximum encoded size of messages (where known) */ #define MESHTASTIC_MESHTASTIC_TELEMETRY_PB_H_MAX_SIZE meshtastic_Telemetry_size #define meshtastic_AirQualityMetrics_size 72 #define meshtastic_DeviceMetrics_size 27 -#define meshtastic_EnvironmentMetrics_size 68 +#define meshtastic_EnvironmentMetrics_size 73 +#define meshtastic_Nau7802Config_size 16 #define meshtastic_PowerMetrics_size 30 -#define meshtastic_Telemetry_size 79 +#define meshtastic_Telemetry_size 80 #ifdef __cplusplus } /* extern "C" */ diff --git a/src/modules/CannedMessageModule.cpp b/src/modules/CannedMessageModule.cpp index 9b993ae5a..f513e045f 100644 --- a/src/modules/CannedMessageModule.cpp +++ b/src/modules/CannedMessageModule.cpp @@ -49,7 +49,7 @@ CannedMessageModule::CannedMessageModule() LOG_INFO("CannedMessageModule is enabled\n"); // T-Watch interface currently has no way to select destination type, so default to 'node' -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NODE; #endif @@ -75,7 +75,7 @@ int CannedMessageModule::splitConfiguredMessages() String messages = cannedMessageModuleConfig.messages; -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) String separator = messages.length() ? "|" : ""; messages = "[---- Free Text ----]" + separator + messages; @@ -144,7 +144,7 @@ int CannedMessageModule::handleInputEvent(const InputEvent *event) } if (event->inputEvent == static_cast(meshtastic_ModuleConfig_CannedMessageConfig_InputEventChar_SELECT)) { -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) if (this->currentMessageIndex == 0) { this->runState = CANNED_MESSAGE_RUN_STATE_FREETEXT; @@ -170,7 +170,7 @@ int CannedMessageModule::handleInputEvent(const InputEvent *event) e.frameChanged = true; this->currentMessageIndex = -1; -#ifndef T_WATCH_S3 +#if !defined(T_WATCH_S3) && !defined(RAK14014) this->freetext = ""; // clear freetext this->cursor = 0; this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NONE; @@ -183,7 +183,7 @@ int CannedMessageModule::handleInputEvent(const InputEvent *event) (event->inputEvent == static_cast(meshtastic_ModuleConfig_CannedMessageConfig_InputEventChar_LEFT)) || (event->inputEvent == static_cast(meshtastic_ModuleConfig_CannedMessageConfig_InputEventChar_RIGHT))) { -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) if (event->inputEvent == static_cast(meshtastic_ModuleConfig_CannedMessageConfig_InputEventChar_LEFT)) { this->payload = 0xb4; } else if (event->inputEvent == static_cast(meshtastic_ModuleConfig_CannedMessageConfig_InputEventChar_RIGHT)) { @@ -283,7 +283,7 @@ int CannedMessageModule::handleInputEvent(const InputEvent *event) } } -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) if (this->runState == CANNED_MESSAGE_RUN_STATE_FREETEXT) { String keyTapped = keyForCoordinates(event->touchX, event->touchY); @@ -404,7 +404,7 @@ int32_t CannedMessageModule::runOnce() this->freetext = ""; // clear freetext this->cursor = 0; -#ifndef T_WATCH_S3 +#if !defined(T_WATCH_S3) && !defined(RAK14014) this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NONE; #endif @@ -417,7 +417,7 @@ int32_t CannedMessageModule::runOnce() this->freetext = ""; // clear freetext this->cursor = 0; -#ifndef T_WATCH_S3 +#if !defined(T_WATCH_S3) && !defined(RAK14014) this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NONE; #endif @@ -437,7 +437,7 @@ int32_t CannedMessageModule::runOnce() powerFSM.trigger(EVENT_PRESS); return INT32_MAX; } else { -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) sendText(this->dest, indexChannels[this->channel], this->messages[this->currentMessageIndex], true); #else sendText(NODENUM_BROADCAST, channels.getPrimaryIndex(), this->messages[this->currentMessageIndex], true); @@ -454,7 +454,7 @@ int32_t CannedMessageModule::runOnce() this->freetext = ""; // clear freetext this->cursor = 0; -#ifndef T_WATCH_S3 +#if !defined(T_WATCH_S3) && !defined(RAK14014) this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NONE; #endif @@ -471,7 +471,7 @@ int32_t CannedMessageModule::runOnce() this->freetext = ""; // clear freetext this->cursor = 0; -#ifndef T_WATCH_S3 +#if !defined(T_WATCH_S3) && !defined(RAK14014) this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NONE; #endif @@ -484,7 +484,7 @@ int32_t CannedMessageModule::runOnce() this->freetext = ""; // clear freetext this->cursor = 0; -#ifndef T_WATCH_S3 +#if !defined(T_WATCH_S3) && !defined(RAK14014) this->destSelect = CANNED_MESSAGE_DESTINATION_TYPE_NONE; #endif @@ -714,7 +714,7 @@ void CannedMessageModule::showTemporaryMessage(const String &message) setIntervalFromNow(2000); } -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) String CannedMessageModule::keyForCoordinates(uint x, uint y) { @@ -949,7 +949,7 @@ void CannedMessageModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *st display->drawString(10 + x, 0 + y + FONT_HEIGHT_SMALL, "Canned Message\nModule disabled."); } else if (cannedMessageModule->runState == CANNED_MESSAGE_RUN_STATE_FREETEXT) { -#ifdef T_WATCH_S3 +#if defined(T_WATCH_S3) || defined(RAK14014) drawKeyboard(display, state, 0, 0); #else diff --git a/src/modules/CannedMessageModule.h b/src/modules/CannedMessageModule.h index 43897e782..00e8c2bf9 100644 --- a/src/modules/CannedMessageModule.h +++ b/src/modules/CannedMessageModule.h @@ -98,7 +98,7 @@ class CannedMessageModule : public SinglePortModule, public Observable + +#include "modules/Telemetry/Sensor/DFRobotLarkSensor.h" +#include "modules/Telemetry/UnitConversions.h" + +#include + +DropzoneModule *dropzoneModule; + +int32_t DropzoneModule::runOnce() +{ + // Send on a 5 second delay from receiving the matching request + if (startSendConditions != 0 && (startSendConditions + 5000U) < millis()) { + service.sendToMesh(sendConditions(), RX_SRC_LOCAL); + startSendConditions = 0; + } + // Run every second to check if we need to send conditions + return 1000; +} + +ProcessMessage DropzoneModule::handleReceived(const meshtastic_MeshPacket &mp) +{ + auto &p = mp.decoded; + char matchCompare[54]; + auto incomingMessage = reinterpret_cast(p.payload.bytes); + sprintf(matchCompare, "%s conditions", owner.short_name); + if (strncasecmp(incomingMessage, matchCompare, strlen(matchCompare)) == 0) { + LOG_DEBUG("Received dropzone conditions request\n"); + startSendConditions = millis(); + } + + sprintf(matchCompare, "%s conditions", owner.long_name); + if (strncasecmp(incomingMessage, matchCompare, strlen(matchCompare)) == 0) { + LOG_DEBUG("Received dropzone conditions request\n"); + startSendConditions = millis(); + } + return ProcessMessage::CONTINUE; +} + +meshtastic_MeshPacket *DropzoneModule::sendConditions() +{ + char replyStr[200]; + /* + CLOSED @ {HH:MM:SS}z + Wind 2 kts @ 125° + 29.25 inHg 72°C + */ + uint32_t rtc_sec = getValidTime(RTCQuality::RTCQualityDevice, true); + int hour = 0, min = 0, sec = 0; + if (rtc_sec > 0) { + long hms = rtc_sec % SEC_PER_DAY; + hms = (hms + SEC_PER_DAY) % SEC_PER_DAY; + + hour = hms / SEC_PER_HOUR; + min = (hms % SEC_PER_HOUR) / SEC_PER_MIN; + sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; + } + + // Check if the dropzone is open or closed by reading the analog pin + // If pin is connected to GND (below 100 should be lower than floating voltage), + // the dropzone is open + auto dropzoneStatus = analogRead(A1) < 100 ? "OPEN" : "CLOSED"; + auto reply = allocDataPacket(); + + auto node = nodeDB->getMeshNode(nodeDB->getNodeNum()); + if (sensor.hasSensor()) { + meshtastic_Telemetry telemetry = meshtastic_Telemetry_init_zero; + sensor.getMetrics(&telemetry); + auto windSpeed = UnitConversions::MetersPerSecondToKnots(telemetry.variant.environment_metrics.wind_speed); + auto windDirection = telemetry.variant.environment_metrics.wind_direction; + auto temp = telemetry.variant.environment_metrics.temperature; + auto baro = UnitConversions::HectoPascalToInchesOfMercury(telemetry.variant.environment_metrics.barometric_pressure); + sprintf(replyStr, "%s @ %02d:%02d:%02dz\nWind %.2f kts @ %d°\nBaro %.2f inHg %.2f°C", dropzoneStatus, hour, min, sec, + windSpeed, windDirection, baro, temp); + } else { + LOG_ERROR("No sensor found\n"); + sprintf(replyStr, "%s @ %02d:%02d:%02d\nNo sensor found", dropzoneStatus, hour, min, sec); + } + LOG_DEBUG("Conditions reply: %s\n", replyStr); + reply->decoded.payload.size = strlen(replyStr); // You must specify how many bytes are in the reply + memcpy(reply->decoded.payload.bytes, replyStr, reply->decoded.payload.size); + + return reply; +} + +#endif \ No newline at end of file diff --git a/src/modules/DropzoneModule.h b/src/modules/DropzoneModule.h new file mode 100644 index 000000000..28f54ee0f --- /dev/null +++ b/src/modules/DropzoneModule.h @@ -0,0 +1,37 @@ +#pragma once +#if !MESHTASTIC_EXCLUDE_DROPZONE +#include "SinglePortModule.h" +#include "modules/Telemetry/Sensor/DFRobotLarkSensor.h" + +/** + * An example module that replies to a message with the current conditions + * and status at the dropzone when it receives a text message mentioning it's name followed by "conditions" + */ +class DropzoneModule : public SinglePortModule, private concurrency::OSThread +{ + DFRobotLarkSensor sensor; + + public: + /** Constructor + * name is for debugging output + */ + DropzoneModule() : SinglePortModule("dropzone", meshtastic_PortNum_TEXT_MESSAGE_APP), concurrency::OSThread("DropzoneModule") + { + // Set up the analog pin for reading the dropzone status + pinMode(PIN_A1, INPUT); + } + + virtual int32_t runOnce() override; + + protected: + /** Called to handle a particular incoming message + */ + virtual ProcessMessage handleReceived(const meshtastic_MeshPacket &mp) override; + + private: + meshtastic_MeshPacket *sendConditions(); + uint32_t startSendConditions = 0; +}; + +extern DropzoneModule *dropzoneModule; +#endif \ No newline at end of file diff --git a/src/modules/Modules.cpp b/src/modules/Modules.cpp index e6c44fae6..1b4bbc3b4 100644 --- a/src/modules/Modules.cpp +++ b/src/modules/Modules.cpp @@ -70,6 +70,11 @@ #include "modules/SerialModule.h" #endif #endif + +#if !MESHTASTIC_EXCLUDE_DROPZONE +#include "modules/DropzoneModule.h" +#endif + /** * Create module instances here. If you are adding a new module, you must 'new' it here (or somewhere else) */ @@ -100,6 +105,10 @@ void setupModules() #if !MESHTASTIC_EXCLUDE_ATAK atakPluginModule = new AtakPluginModule(); #endif + +#if !MESHTASTIC_EXCLUDE_DROPZONE + dropzoneModule = new DropzoneModule(); +#endif // Note: if the rest of meshtastic doesn't need to explicitly use your module, you do not need to assign the instance // to a global variable. diff --git a/src/modules/Telemetry/EnvironmentTelemetry.cpp b/src/modules/Telemetry/EnvironmentTelemetry.cpp index 46b8a1ad8..ff3202067 100644 --- a/src/modules/Telemetry/EnvironmentTelemetry.cpp +++ b/src/modules/Telemetry/EnvironmentTelemetry.cpp @@ -27,6 +27,7 @@ #include "Sensor/LPS22HBSensor.h" #include "Sensor/MCP9808Sensor.h" #include "Sensor/MLX90632Sensor.h" +#include "Sensor/NAU7802Sensor.h" #include "Sensor/OPT3001Sensor.h" #include "Sensor/RCWL9620Sensor.h" #include "Sensor/SHT31Sensor.h" @@ -51,6 +52,7 @@ RCWL9620Sensor rcwl9620Sensor; AHT10Sensor aht10Sensor; MLX90632Sensor mlx90632Sensor; DFRobotLarkSensor dfRobotLarkSensor; +NAU7802Sensor nau7802Sensor; #define FAILED_STATE_SENSOR_READ_MULTIPLIER 10 #define DISPLAY_RECEIVEID_MEASUREMENTS_ON_SCREEN true @@ -125,6 +127,8 @@ int32_t EnvironmentTelemetryModule::runOnce() result = aht10Sensor.runOnce(); if (mlx90632Sensor.hasSensor()) result = mlx90632Sensor.runOnce(); + if (nau7802Sensor.hasSensor()) + result = nau7802Sensor.runOnce(); } return result; } else { @@ -223,12 +227,18 @@ void EnvironmentTelemetryModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiSt "Volt/Cur: " + String(lastMeasurement.variant.environment_metrics.voltage, 0) + "V / " + String(lastMeasurement.variant.environment_metrics.current, 0) + "mA"); } + if (lastMeasurement.variant.environment_metrics.iaq != 0) { display->drawString(x, y += fontHeight(FONT_SMALL), "IAQ: " + String(lastMeasurement.variant.environment_metrics.iaq)); } + if (lastMeasurement.variant.environment_metrics.distance != 0) display->drawString(x, y += fontHeight(FONT_SMALL), "Water Level: " + String(lastMeasurement.variant.environment_metrics.distance, 0) + "mm"); + + if (lastMeasurement.variant.environment_metrics.weight != 0) + display->drawString(x, y += fontHeight(FONT_SMALL), + "Weight: " + String(lastMeasurement.variant.environment_metrics.weight, 0) + "kg"); } bool EnvironmentTelemetryModule::handleReceivedProtobuf(const meshtastic_MeshPacket &mp, meshtastic_Telemetry *t) @@ -245,8 +255,9 @@ bool EnvironmentTelemetryModule::handleReceivedProtobuf(const meshtastic_MeshPac LOG_INFO("(Received from %s): voltage=%f, IAQ=%d, distance=%f, lux=%f\n", sender, t->variant.environment_metrics.voltage, t->variant.environment_metrics.iaq, t->variant.environment_metrics.distance, t->variant.environment_metrics.lux); - LOG_INFO("(Received from %s): wind speed=%fm/s, direction=%d degrees\n", sender, - t->variant.environment_metrics.wind_speed, t->variant.environment_metrics.wind_direction); + LOG_INFO("(Received from %s): wind speed=%fm/s, direction=%d degrees, weight=%fkg\n", sender, + t->variant.environment_metrics.wind_speed, t->variant.environment_metrics.wind_direction, + t->variant.environment_metrics.weight); #endif // release previous packet before occupying a new spot @@ -331,6 +342,10 @@ bool EnvironmentTelemetryModule::sendTelemetry(NodeNum dest, bool phoneOnly) valid = valid && rcwl9620Sensor.getMetrics(&m); hasSensor = true; } + if (nau7802Sensor.hasSensor()) { + valid = valid && nau7802Sensor.getMetrics(&m); + hasSensor = true; + } if (aht10Sensor.hasSensor()) { if (!bmp280Sensor.hasSensor()) { valid = valid && aht10Sensor.getMetrics(&m); @@ -354,8 +369,8 @@ bool EnvironmentTelemetryModule::sendTelemetry(NodeNum dest, bool phoneOnly) LOG_INFO("(Sending): voltage=%f, IAQ=%d, distance=%f, lux=%f\n", m.variant.environment_metrics.voltage, m.variant.environment_metrics.iaq, m.variant.environment_metrics.distance, m.variant.environment_metrics.lux); - LOG_INFO("(Sending): wind speed=%fm/s, direction=%d degrees\n", m.variant.environment_metrics.wind_speed, - m.variant.environment_metrics.wind_direction); + LOG_INFO("(Sending): wind speed=%fm/s, direction=%d degrees, weight=%fkg\n", m.variant.environment_metrics.wind_speed, + m.variant.environment_metrics.wind_direction, m.variant.environment_metrics.weight); sensor_read_error_count = 0; @@ -388,4 +403,102 @@ bool EnvironmentTelemetryModule::sendTelemetry(NodeNum dest, bool phoneOnly) return valid; } +AdminMessageHandleResult EnvironmentTelemetryModule::handleAdminMessageForModule(const meshtastic_MeshPacket &mp, + meshtastic_AdminMessage *request, + meshtastic_AdminMessage *response) +{ + AdminMessageHandleResult result = AdminMessageHandleResult::NOT_HANDLED; + if (dfRobotLarkSensor.hasSensor()) { + result = dfRobotLarkSensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (sht31Sensor.hasSensor()) { + result = sht31Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (lps22hbSensor.hasSensor()) { + result = lps22hbSensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (shtc3Sensor.hasSensor()) { + result = shtc3Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (bmp085Sensor.hasSensor()) { + result = bmp085Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (bmp280Sensor.hasSensor()) { + result = bmp280Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (bme280Sensor.hasSensor()) { + result = bme280Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (bme680Sensor.hasSensor()) { + result = bme680Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (mcp9808Sensor.hasSensor()) { + result = mcp9808Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (ina219Sensor.hasSensor()) { + result = ina219Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (ina260Sensor.hasSensor()) { + result = ina260Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (veml7700Sensor.hasSensor()) { + result = veml7700Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (tsl2591Sensor.hasSensor()) { + result = tsl2591Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (opt3001Sensor.hasSensor()) { + result = opt3001Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (mlx90632Sensor.hasSensor()) { + result = mlx90632Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (rcwl9620Sensor.hasSensor()) { + result = rcwl9620Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (nau7802Sensor.hasSensor()) { + result = nau7802Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + if (aht10Sensor.hasSensor()) { + result = aht10Sensor.handleAdminMessage(mp, request, response); + if (result != AdminMessageHandleResult::NOT_HANDLED) + return result; + } + return result; +} + #endif \ No newline at end of file diff --git a/src/modules/Telemetry/EnvironmentTelemetry.h b/src/modules/Telemetry/EnvironmentTelemetry.h index cdd9491d4..ca150347e 100644 --- a/src/modules/Telemetry/EnvironmentTelemetry.h +++ b/src/modules/Telemetry/EnvironmentTelemetry.h @@ -37,6 +37,10 @@ class EnvironmentTelemetryModule : private concurrency::OSThread, public Protobu */ bool sendTelemetry(NodeNum dest = NODENUM_BROADCAST, bool wantReplies = false); + virtual AdminMessageHandleResult handleAdminMessageForModule(const meshtastic_MeshPacket &mp, + meshtastic_AdminMessage *request, + meshtastic_AdminMessage *response) override; + private: float CelsiusToFahrenheit(float c); bool firstTime = 1; diff --git a/src/modules/Telemetry/Sensor/DFRobotLarkSensor.h b/src/modules/Telemetry/Sensor/DFRobotLarkSensor.h index b26d690b1..7a988e84a 100644 --- a/src/modules/Telemetry/Sensor/DFRobotLarkSensor.h +++ b/src/modules/Telemetry/Sensor/DFRobotLarkSensor.h @@ -1,3 +1,7 @@ +#pragma once + +#ifndef _MT_DFROBOTLARKSENSOR_H +#define _MT_DFROBOTLARKSENSOR_H #include "configuration.h" #if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR @@ -21,4 +25,5 @@ class DFRobotLarkSensor : public TelemetrySensor virtual bool getMetrics(meshtastic_Telemetry *measurement) override; }; +#endif #endif \ No newline at end of file diff --git a/src/modules/Telemetry/Sensor/NAU7802Sensor.cpp b/src/modules/Telemetry/Sensor/NAU7802Sensor.cpp new file mode 100644 index 000000000..39ac4b08b --- /dev/null +++ b/src/modules/Telemetry/Sensor/NAU7802Sensor.cpp @@ -0,0 +1,143 @@ +#include "configuration.h" + +#if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR + +#include "../mesh/generated/meshtastic/telemetry.pb.h" +#include "FSCommon.h" +#include "NAU7802Sensor.h" +#include "TelemetrySensor.h" +#include +#include + +meshtastic_Nau7802Config nau7802config = meshtastic_Nau7802Config_init_zero; + +NAU7802Sensor::NAU7802Sensor() : TelemetrySensor(meshtastic_TelemetrySensorType_NAU7802, "NAU7802") {} + +int32_t NAU7802Sensor::runOnce() +{ + LOG_INFO("Init sensor: %s\n", sensorName); + if (!hasSensor()) { + return DEFAULT_SENSOR_MINIMUM_WAIT_TIME_BETWEEN_READS; + } + status = nau7802.begin(*nodeTelemetrySensorsMap[sensorType].second); + nau7802.setSampleRate(NAU7802_SPS_320); + if (!loadCalibrationData()) { + LOG_ERROR("Failed to load calibration data\n"); + } + nau7802.calibrateAFE(); + LOG_INFO("Offset: %d, Calibration factor: %.2f\n", nau7802.getZeroOffset(), nau7802.getCalibrationFactor()); + return initI2CSensor(); +} + +void NAU7802Sensor::setup() {} + +bool NAU7802Sensor::getMetrics(meshtastic_Telemetry *measurement) +{ + LOG_DEBUG("NAU7802Sensor::getMetrics\n"); + nau7802.powerUp(); + // Wait for the sensor to become ready for one second max + uint32_t start = millis(); + while (!nau7802.available()) { + delay(100); + if (millis() - start > 1000) { + nau7802.powerDown(); + return false; + } + } + // Check if we have correct calibration values after powerup + LOG_DEBUG("Offset: %d, Calibration factor: %.2f\n", nau7802.getZeroOffset(), nau7802.getCalibrationFactor()); + measurement->variant.environment_metrics.weight = nau7802.getWeight() / 1000; // sample is in kg + nau7802.powerDown(); + return true; +} + +void NAU7802Sensor::calibrate(float weight) +{ + nau7802.calculateCalibrationFactor(weight * 1000, 64); // internal sample is in grams + if (!saveCalibrationData()) { + LOG_WARN("Failed to save calibration data\n"); + } + LOG_INFO("Offset: %d, Calibration factor: %.2f\n", nau7802.getZeroOffset(), nau7802.getCalibrationFactor()); +} + +AdminMessageHandleResult NAU7802Sensor::handleAdminMessage(const meshtastic_MeshPacket &mp, meshtastic_AdminMessage *request, + meshtastic_AdminMessage *response) +{ + AdminMessageHandleResult result; + + switch (request->which_payload_variant) { + case meshtastic_AdminMessage_set_scale_tag: + if (request->set_scale == 0) { + this->tare(); + LOG_DEBUG("Client requested to tare scale\n"); + } else { + this->calibrate(request->set_scale); + LOG_DEBUG("Client requested to calibrate to %d kg\n", request->set_scale); + } + result = AdminMessageHandleResult::HANDLED; + break; + + default: + result = AdminMessageHandleResult::NOT_HANDLED; + } + + return result; +} + +void NAU7802Sensor::tare() +{ + nau7802.calculateZeroOffset(64); + if (!saveCalibrationData()) { + LOG_WARN("Failed to save calibration data\n"); + } + LOG_INFO("Offset: %d, Calibration factor: %.2f\n", nau7802.getZeroOffset(), nau7802.getCalibrationFactor()); +} + +bool NAU7802Sensor::saveCalibrationData() +{ + if (FSCom.exists(nau7802ConfigFileName) && !FSCom.remove(nau7802ConfigFileName)) { + LOG_WARN("Can't remove old state file\n"); + } + auto file = FSCom.open(nau7802ConfigFileName, FILE_O_WRITE); + nau7802config.zeroOffset = nau7802.getZeroOffset(); + nau7802config.calibrationFactor = nau7802.getCalibrationFactor(); + bool okay = false; + if (file) { + LOG_INFO("%s state write to %s.\n", sensorName, nau7802ConfigFileName); + pb_ostream_t stream = {&writecb, &file, meshtastic_Nau7802Config_size}; + + if (!pb_encode(&stream, &meshtastic_Nau7802Config_msg, &nau7802config)) { + LOG_ERROR("Error: can't encode protobuf %s\n", PB_GET_ERROR(&stream)); + } else { + okay = true; + } + file.flush(); + file.close(); + } else { + LOG_INFO("Can't write %s state (File: %s).\n", sensorName, nau7802ConfigFileName); + } + return okay; +} + +bool NAU7802Sensor::loadCalibrationData() +{ + auto file = FSCom.open(nau7802ConfigFileName, FILE_O_READ); + bool okay = false; + if (file) { + LOG_INFO("%s state read from %s.\n", sensorName, nau7802ConfigFileName); + pb_istream_t stream = {&readcb, &file, meshtastic_Nau7802Config_size}; + if (!pb_decode(&stream, &meshtastic_Nau7802Config_msg, &nau7802config)) { + LOG_ERROR("Error: can't decode protobuf %s\n", PB_GET_ERROR(&stream)); + } else { + nau7802.setZeroOffset(nau7802config.zeroOffset); + nau7802.setCalibrationFactor(nau7802config.calibrationFactor); + okay = true; + } + file.close(); + } else { + LOG_INFO("No %s state found (File: %s).\n", sensorName, nau7802ConfigFileName); + } + return okay; +} + +#endif \ No newline at end of file diff --git a/src/modules/Telemetry/Sensor/NAU7802Sensor.h b/src/modules/Telemetry/Sensor/NAU7802Sensor.h new file mode 100644 index 000000000..c53a3b31a --- /dev/null +++ b/src/modules/Telemetry/Sensor/NAU7802Sensor.h @@ -0,0 +1,31 @@ +#include "MeshModule.h" +#include "configuration.h" + +#if !MESHTASTIC_EXCLUDE_ENVIRONMENTAL_SENSOR + +#include "../mesh/generated/meshtastic/telemetry.pb.h" +#include "TelemetrySensor.h" +#include + +class NAU7802Sensor : public TelemetrySensor +{ + private: + NAU7802 nau7802; + + protected: + virtual void setup() override; + const char *nau7802ConfigFileName = "/prefs/nau7802.dat"; + bool saveCalibrationData(); + bool loadCalibrationData(); + + public: + NAU7802Sensor(); + virtual int32_t runOnce() override; + virtual bool getMetrics(meshtastic_Telemetry *measurement) override; + void tare(); + void calibrate(float weight); + AdminMessageHandleResult handleAdminMessage(const meshtastic_MeshPacket &mp, meshtastic_AdminMessage *request, + meshtastic_AdminMessage *response) override; +}; + +#endif \ No newline at end of file diff --git a/src/modules/Telemetry/Sensor/TelemetrySensor.h b/src/modules/Telemetry/Sensor/TelemetrySensor.h index 35cb7965d..da376ad31 100644 --- a/src/modules/Telemetry/Sensor/TelemetrySensor.h +++ b/src/modules/Telemetry/Sensor/TelemetrySensor.h @@ -4,6 +4,7 @@ #pragma once #include "../mesh/generated/meshtastic/telemetry.pb.h" +#include "MeshModule.h" #include "NodeDB.h" #include @@ -42,6 +43,12 @@ class TelemetrySensor virtual void setup(); public: + virtual AdminMessageHandleResult handleAdminMessage(const meshtastic_MeshPacket &mp, meshtastic_AdminMessage *request, + meshtastic_AdminMessage *response) + { + return AdminMessageHandleResult::NOT_HANDLED; + } + bool hasSensor() { return nodeTelemetrySensorsMap[sensorType].first > 0; } virtual int32_t runOnce() = 0; diff --git a/src/modules/Telemetry/UnitConversions.cpp b/src/modules/Telemetry/UnitConversions.cpp new file mode 100644 index 000000000..9f40de40f --- /dev/null +++ b/src/modules/Telemetry/UnitConversions.cpp @@ -0,0 +1,21 @@ +#include "UnitConversions.h" + +float UnitConversions::CelsiusToFahrenheit(float celcius) +{ + return (celcius * 9) / 5 + 32; +} + +float UnitConversions::MetersPerSecondToKnots(float metersPerSecond) +{ + return metersPerSecond * 1.94384; +} + +float UnitConversions::MetersPerSecondToMilesPerHour(float metersPerSecond) +{ + return metersPerSecond * 2.23694; +} + +float UnitConversions::HectoPascalToInchesOfMercury(float hectoPascal) +{ + return hectoPascal * 0.029529983071445; +} diff --git a/src/modules/Telemetry/UnitConversions.h b/src/modules/Telemetry/UnitConversions.h new file mode 100644 index 000000000..60f9b664a --- /dev/null +++ b/src/modules/Telemetry/UnitConversions.h @@ -0,0 +1,10 @@ +#pragma once + +class UnitConversions +{ + public: + static float CelsiusToFahrenheit(float celcius); + static float MetersPerSecondToKnots(float metersPerSecond); + static float MetersPerSecondToMilesPerHour(float metersPerSecond); + static float HectoPascalToInchesOfMercury(float hectoPascal); +}; diff --git a/src/mqtt/MQTT.cpp b/src/mqtt/MQTT.cpp index f3eda6d7c..9f9ac5c24 100644 --- a/src/mqtt/MQTT.cpp +++ b/src/mqtt/MQTT.cpp @@ -675,6 +675,8 @@ std::string MQTT::meshPacketToJson(meshtastic_MeshPacket *mp) msgPayload["lux"] = new JSONValue(decoded->variant.environment_metrics.lux); msgPayload["white_lux"] = new JSONValue(decoded->variant.environment_metrics.white_lux); msgPayload["iaq"] = new JSONValue((uint)decoded->variant.environment_metrics.iaq); + msgPayload["wind_speed"] = new JSONValue((uint)decoded->variant.environment_metrics.wind_speed); + msgPayload["wind_direction"] = new JSONValue((uint)decoded->variant.environment_metrics.wind_direction); } else if (decoded->which_variant == meshtastic_Telemetry_power_metrics_tag) { msgPayload["voltage_ch1"] = new JSONValue(decoded->variant.power_metrics.ch1_voltage); msgPayload["current_ch1"] = new JSONValue(decoded->variant.power_metrics.ch1_current); @@ -898,8 +900,10 @@ std::string MQTT::meshPacketToJson(meshtastic_MeshPacket *mp) jsonObj["rssi"] = new JSONValue((int)mp->rx_rssi); if (mp->rx_snr != 0) jsonObj["snr"] = new JSONValue((float)mp->rx_snr); - if (mp->hop_start != 0 && mp->hop_limit <= mp->hop_start) + if (mp->hop_start != 0 && mp->hop_limit <= mp->hop_start) { jsonObj["hops_away"] = new JSONValue((unsigned int)(mp->hop_start - mp->hop_limit)); + jsonObj["hop_start"] = new JSONValue((unsigned int)(mp->hop_start)); + } // serialize and write it to the stream JSONValue *value = new JSONValue(jsonObj); diff --git a/src/platform/esp32/architecture.h b/src/platform/esp32/architecture.h index 824c11bdd..c979d016c 100644 --- a/src/platform/esp32/architecture.h +++ b/src/platform/esp32/architecture.h @@ -147,6 +147,8 @@ #define HW_VENDOR meshtastic_HardwareModel_WIPHONE #elif defined(RADIOMASTER_900_BANDIT_NANO) #define HW_VENDOR meshtastic_HardwareModel_RADIOMASTER_900_BANDIT_NANO +#elif defined(HELTEC_CAPSULE_SENSOR_V3) +#define HW_VENDOR meshtastic_HardwareModel_HELTEC_CAPSULE_SENSOR_V3 #endif // ----------------------------------------------------------------------------- diff --git a/src/platform/nrf52/NRF52Bluetooth.cpp b/src/platform/nrf52/NRF52Bluetooth.cpp index 39898ab25..4c25f38ea 100644 --- a/src/platform/nrf52/NRF52Bluetooth.cpp +++ b/src/platform/nrf52/NRF52Bluetooth.cpp @@ -215,6 +215,18 @@ void NRF52Bluetooth::shutdown() Bluefruit.Advertising.stop(); } +void NRF52Bluetooth::startDisabled() +{ + // Setup Bluetooth + nrf52Bluetooth->setup(); + + // Shutdown bluetooth for minimum power draw + Bluefruit.Advertising.stop(); + Bluefruit.setTxPower(-40); // Minimum power + + LOG_INFO("Disabling NRF52 Bluetooth. (Workaround: tx power min, advertising stopped)\n"); +} + bool NRF52Bluetooth::isConnected() { return Bluefruit.connected(connectionHandle); diff --git a/src/platform/nrf52/NRF52Bluetooth.h b/src/platform/nrf52/NRF52Bluetooth.h index 11e18c127..450af47f9 100644 --- a/src/platform/nrf52/NRF52Bluetooth.h +++ b/src/platform/nrf52/NRF52Bluetooth.h @@ -8,6 +8,7 @@ class NRF52Bluetooth : BluetoothApi public: void setup(); void shutdown(); + void startDisabled(); void resumeAdvertising(); void clearBonds(); bool isConnected(); diff --git a/src/platform/nrf52/main-nrf52.cpp b/src/platform/nrf52/main-nrf52.cpp index 9cc52a7de..1f2c6867d 100644 --- a/src/platform/nrf52/main-nrf52.cpp +++ b/src/platform/nrf52/main-nrf52.cpp @@ -68,28 +68,47 @@ static const bool useSoftDevice = true; // Set to false for easier debugging #if !MESHTASTIC_EXCLUDE_BLUETOOTH void setBluetoothEnable(bool enable) { - if (enable && config.bluetooth.enabled) { - if (!useSoftDevice) { + // For debugging use: don't use bluetooth + if (!useSoftDevice) { + if (enable) LOG_INFO("DISABLING NRF52 BLUETOOTH WHILE DEBUGGING\n"); - } else { - if (!nrf52Bluetooth) { - LOG_DEBUG("Initializing NRF52 Bluetooth\n"); - nrf52Bluetooth = new NRF52Bluetooth(); - nrf52Bluetooth->setup(); - - // We delay brownout init until after BLE because BLE starts soft device - initBrownout(); - } else { - nrf52Bluetooth->resumeAdvertising(); - } - } - } else { - if (nrf52Bluetooth) { - nrf52Bluetooth->shutdown(); - } + return; } + + // If user disabled bluetooth: init then disable advertising & reduce power + // Workaround. Avoid issue where device hangs several days after boot.. + // Allegedly, no significant increase in power consumption + if (!config.bluetooth.enabled) { + static bool initialized = false; + if (!initialized) { + nrf52Bluetooth = new NRF52Bluetooth(); + nrf52Bluetooth->startDisabled(); + initBrownout(); + initialized = true; + } + return; + } + + if (enable) { + // If not yet set-up + if (!nrf52Bluetooth) { + LOG_DEBUG("Initializing NRF52 Bluetooth\n"); + nrf52Bluetooth = new NRF52Bluetooth(); + nrf52Bluetooth->setup(); + + // We delay brownout init until after BLE because BLE starts soft device + initBrownout(); + } + // Already setup, apparently + else + nrf52Bluetooth->resumeAdvertising(); + } + // Disable (if previously set-up) + else if (nrf52Bluetooth) + nrf52Bluetooth->shutdown(); } #else +#warning NRF52 "Bluetooth disable" workaround does not apply to builds with MESHTASTIC_EXCLUDE_BLUETOOTH void setBluetoothEnable(bool enable) {} #endif /** diff --git a/variants/heltec_capsule_sensor_v3/platformio.ini b/variants/heltec_capsule_sensor_v3/platformio.ini new file mode 100644 index 000000000..f1aef925d --- /dev/null +++ b/variants/heltec_capsule_sensor_v3/platformio.ini @@ -0,0 +1,11 @@ +[env:heltec_capsule_sensor_v3] +extends = esp32s3_base +board = heltec_wifi_lora_32_V3 +board_check = true + +build_flags = + ${esp32s3_base.build_flags} -I variants/heltec_capsule_sensor_v3 + -D HELTEC_CAPSULE_SENSOR_V3 + -D GPS_POWER_TOGGLE ; comment this line to disable triple press function on the user button to turn off gps entirely. + ;-D DEBUG_DISABLED ; uncomment this line to disable DEBUG output + diff --git a/variants/heltec_capsule_sensor_v3/variant.h b/variants/heltec_capsule_sensor_v3/variant.h new file mode 100644 index 000000000..0d5ab73cf --- /dev/null +++ b/variants/heltec_capsule_sensor_v3/variant.h @@ -0,0 +1,42 @@ +#define LED_PIN 33 +#define LED_PIN2 34 +#define EXT_PWR_DETECT 35 + +#define BUTTON_PIN 18 + +#define BATTERY_PIN 7 // A battery voltage measurement pin, voltage divider connected here to measure battery voltage +#define ADC_CHANNEL ADC1_GPIO7_CHANNEL +#define ADC_ATTENUATION ADC_ATTEN_DB_2_5 // lower dB for high resistance voltage divider +#define ADC_MULTIPLIER (4.9 * 1.045) +#define ADC_CTRL 36 // active HIGH, powers the voltage divider. Only on 1.1 +#define ADC_CTRL_ENABLED HIGH + +#undef GPS_RX_PIN +#undef GPS_TX_PIN +#define GPS_RX_PIN 5 +#define GPS_TX_PIN 4 +#define PIN_GPS_RESET 3 +#define GPS_RESET_MODE LOW +#define PIN_GPS_PPS 1 +#define PIN_GPS_EN 21 +#define GPS_EN_ACTIVE HIGH + +#define USE_SX1262 +#define LORA_DIO0 -1 // a No connect on the SX1262 module +#define LORA_RESET 12 +#define LORA_DIO1 14 // SX1262 IRQ +#define LORA_DIO2 13 // SX1262 BUSY +#define LORA_DIO3 // Not connected on PCB, but internally on the TTGO SX1262, if DIO3 is high the TXCO is enabled + +#define LORA_SCK 9 +#define LORA_MISO 11 +#define LORA_MOSI 10 +#define LORA_CS 8 + +#define SX126X_CS LORA_CS +#define SX126X_DIO1 LORA_DIO1 +#define SX126X_BUSY LORA_DIO2 +#define SX126X_RESET LORA_RESET + +#define SX126X_DIO2_AS_RF_SWITCH +#define SX126X_DIO3_TCXO_VOLTAGE 1.8 diff --git a/variants/radiomaster_900_bandit_micro/platformio.ini b/variants/radiomaster_900_bandit_micro/platformio.ini new file mode 100644 index 000000000..9e54f5859 --- /dev/null +++ b/variants/radiomaster_900_bandit_micro/platformio.ini @@ -0,0 +1,19 @@ +; +; This uses the same code and settings as the Radio Master Bandit Nano (https://www.radiomasterrc.com/products/bandit-nano-expresslrs-rf-module) +; +; Link to the unit : https://www.radiomasterrc.com/products/bandit-micro-expresslrs-rf-module +; +[env:radiomaster_900_bandit_micro] +extends = esp32_base +board = esp32doit-devkit-v1 +build_flags = + ${esp32_base.build_flags} + -DRADIOMASTER_900_BANDIT_NANO + -DVTABLES_IN_FLASH=1 + -DCONFIG_DISABLE_HAL_LOCKS=1 + -O2 + -Ivariants/radiomaster_900_bandit_nano +board_build.f_cpu = 240000000L +upload_protocol = esptool +lib_deps = + ${esp32_base.lib_deps} \ No newline at end of file diff --git a/variants/rak10701/platformio.ini b/variants/rak10701/platformio.ini index 75e29bca0..4c9bf3b20 100644 --- a/variants/rak10701/platformio.ini +++ b/variants/rak10701/platformio.ini @@ -17,6 +17,8 @@ lib_deps = https://github.com/RAKWireless/RAK13800-W5100S.git#1.0.2 rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2 bodmer/TFT_eSPI + beegee-tokyo/RAKwireless RAK12034@^1.0.0 + beegee-tokyo/RAK14014-FT6336U @ 1.0.1 debug_tool = jlink ; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm) ;upload_protocol = jlink \ No newline at end of file diff --git a/variants/rak10701/variant.h b/variants/rak10701/variant.h index 076504c16..c263796ee 100644 --- a/variants/rak10701/variant.h +++ b/variants/rak10701/variant.h @@ -307,10 +307,10 @@ SO GPIO 39/TXEN MAY NOT BE DEFINED FOR SUCCESSFUL OPERATION OF THE SX1262 - TG #define SCREEN_ROTATE #define SCREEN_TRANSITION_FRAMERATE 5 -#define HAS_TOUCHSCREEN 0 -#define SCREEN_TOUCH_INT 10 // From tp.h on the tracker open source code. -#define TOUCH_I2C_PORT 0 -#define TOUCH_SLAVE_ADDRESS 0x5D // GT911 +#define HAS_TOUCHSCREEN 1 +#define SCREEN_TOUCH_INT WB_IO6 + +#define CANNED_MESSAGE_MODULE_ENABLE 1 /*---------------------------------------------------------------------------- * Arduino objects - C++ only diff --git a/variants/rak4631/platformio.ini b/variants/rak4631/platformio.ini index f64811429..4870d4b68 100644 --- a/variants/rak4631/platformio.ini +++ b/variants/rak4631/platformio.ini @@ -16,6 +16,7 @@ lib_deps = melopero/Melopero RV3028@^1.1.0 https://github.com/RAKWireless/RAK13800-W5100S.git#1.0.2 rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2 + https://github.com/meshtastic/RAK12034-BMX160.git#4821355fb10390ba8557dc43ca29a023bcfbb9d9 debug_tool = jlink ; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm) ;upload_protocol = jlink \ No newline at end of file diff --git a/variants/rak4631_epaper/platformio.ini b/variants/rak4631_epaper/platformio.ini index 1ca9a2157..08342dcf7 100644 --- a/variants/rak4631_epaper/platformio.ini +++ b/variants/rak4631_epaper/platformio.ini @@ -13,6 +13,7 @@ lib_deps = zinggjm/GxEPD2@^1.4.9 melopero/Melopero RV3028@^1.1.0 rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2 + beegee-tokyo/RAKwireless RAK12034@^1.0.0 debug_tool = jlink ; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm) ;upload_protocol = jlink \ No newline at end of file diff --git a/variants/rak4631_epaper_onrxtx/platformio.ini b/variants/rak4631_epaper_onrxtx/platformio.ini index e0a0a5a58..f7035a1b1 100644 --- a/variants/rak4631_epaper_onrxtx/platformio.ini +++ b/variants/rak4631_epaper_onrxtx/platformio.ini @@ -15,7 +15,8 @@ lib_deps = zinggjm/GxEPD2@^1.5.1 melopero/Melopero RV3028@^1.1.0 rakwireless/RAKwireless NCP5623 RGB LED library@^1.0.2 + beegee-tokyo/RAKwireless RAK12034@^1.0.0 debug_tool = jlink ; If not set we will default to uploading over serial (first it forces bootloader entry by talking 1200bps to cdcacm) ;upload_protocol = jlink -;upload_port = /dev/ttyACM3 +;upload_port = /dev/ttyACM3 \ No newline at end of file