Skip to content

Commit

Permalink
Merge branch 'master' into mtk_gps_factory_reset_with_pcas10_3
Browse files Browse the repository at this point in the history
  • Loading branch information
thebentern authored Mar 15, 2024
2 parents df7b4b9 + 0de36fb commit ef57369
Show file tree
Hide file tree
Showing 25 changed files with 253 additions and 147 deletions.
4 changes: 2 additions & 2 deletions arch/rp2040/rp2040.ini
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
; Common settings for rp2040 Processor based targets
[rp2040_base]
platform = https://github.com/maxgerhardt/platform-raspberrypi.git#612de5399d68b359053f1307ed223d400aea975c
platform = https://github.com/maxgerhardt/platform-raspberrypi.git#60d6ae81fcc73c34b1493ca9e261695e471bc0c2
extends = arduino_base
platform_packages = framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#3.6.2
platform_packages = framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git#3.7.2

board_build.core = earlephilhower
board_build.filesystem_size = 0.5m
Expand Down
5 changes: 3 additions & 2 deletions platformio.ini
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,7 @@ build_flags = -Wno-missing-field-initializers
-DRADIOLIB_EXCLUDE_PAGER
-DRADIOLIB_EXCLUDE_FSK4
-DRADIOLIB_EXCLUDE_APRS
-DRADIOLIB_EXCLUDE_LORAWAN

monitor_speed = 115200

Expand All @@ -77,7 +78,7 @@ lib_deps =
https://github.com/meshtastic/esp8266-oled-ssd1306.git#ee628ee6c9588d4c56c9e3da35f0fc9448ad54a8 ; 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#2044b2c51e91ab4cd8cc93b15e40658cd808dd06
https://github.com/meshtastic/TinyGPSPlus.git#f9f4fef2183514aa52be91d714c1455dd6f26e45
https://github.com/meshtastic/ArduinoThread.git#72921ac222eed6f526ba1682023cee290d9aa1b3
nanopb/Nanopb@^0.4.7
erriez/ErriezCRC32@^1.0.1
Expand Down Expand Up @@ -130,4 +131,4 @@ lib_deps =
adafruit/Adafruit PM25 AQI Sensor@^1.0.6
adafruit/Adafruit MPU6050@^2.2.4
adafruit/Adafruit LIS3DH@^1.2.4
https://github.com/lewisxhe/BMA423_Library@^0.0.1
https://github.com/lewisxhe/SensorLib#27fd0f721e20cd09e1f81383f0ba58a54fe84a17
2 changes: 1 addition & 1 deletion protobufs
68 changes: 23 additions & 45 deletions src/AccelerometerThread.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,16 @@
#include <Adafruit_LIS3DH.h>
#include <Adafruit_MPU6050.h>
#include <Arduino.h>
#include <SensorBMA423.hpp>
#include <Wire.h>
#include <bma.h>

BMA423 bmaSensor;
SensorBMA423 bmaSensor;
bool BMA_IRQ = false;

#define ACCELEROMETER_CHECK_INTERVAL_MS 100
#define ACCELEROMETER_CLICK_THRESHOLD 40

uint16_t readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len)
int readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len)
{
Wire.beginTransmission(address);
Wire.write(reg);
Expand All @@ -29,7 +29,7 @@ uint16_t readRegister(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len)
return 0; // Pass
}

uint16_t writeRegister(uint8_t address, uint8_t reg, uint8_t *data, uint16_t len)
int writeRegister(uint8_t address, uint8_t reg, uint8_t *data, uint8_t len)
{
Wire.beginTransmission(address);
Wire.write(reg);
Expand Down Expand Up @@ -72,24 +72,14 @@ class AccelerometerThread : public concurrency::OSThread
lis.setRange(LIS3DH_RANGE_2_G);
// Adjust threshold, higher numbers are less sensitive
lis.setClick(config.device.double_tap_as_button_press ? 2 : 1, ACCELEROMETER_CLICK_THRESHOLD);
} else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && bmaSensor.begin(readRegister, writeRegister, delay)) {
} else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 &&
bmaSensor.begin(accelerometer_found.address, &readRegister, &writeRegister)) {
LOG_DEBUG("BMA423 initializing\n");
Acfg cfg;
cfg.odr = BMA4_OUTPUT_DATA_RATE_100HZ;
cfg.range = BMA4_ACCEL_RANGE_2G;
cfg.bandwidth = BMA4_ACCEL_NORMAL_AVG4;
cfg.perf_mode = BMA4_CONTINUOUS_MODE;
bmaSensor.setAccelConfig(cfg);
bmaSensor.enableAccel();

struct bma4_int_pin_config pin_config;
pin_config.edge_ctrl = BMA4_LEVEL_TRIGGER;
pin_config.lvl = BMA4_ACTIVE_HIGH;
pin_config.od = BMA4_PUSH_PULL;
pin_config.output_en = BMA4_OUTPUT_ENABLE;
pin_config.input_en = BMA4_INPUT_DISABLE;
// The correct trigger interrupt needs to be configured as needed
bmaSensor.setINTPinConfig(pin_config, BMA4_INTR1_MAP);
bmaSensor.configAccelerometer(bmaSensor.RANGE_2G, bmaSensor.ODR_100HZ, bmaSensor.BW_NORMAL_AVG4,
bmaSensor.PERF_CONTINUOUS_MODE);
bmaSensor.enableAccelerometer();
bmaSensor.configInterrupt(BMA4_LEVEL_TRIGGER, BMA4_ACTIVE_HIGH, BMA4_PUSH_PULL, BMA4_OUTPUT_ENABLE,
BMA4_INPUT_DISABLE);

#ifdef BMA423_INT
pinMode(BMA4XX_INT, INPUT);
Expand All @@ -102,34 +92,22 @@ class AccelerometerThread : public concurrency::OSThread
RISING); // Select the interrupt mode according to the actual circuit
#endif

struct bma423_axes_remap remap_data;
#ifdef T_WATCH_S3
remap_data.x_axis = 1;
remap_data.x_axis_sign = 0;
remap_data.y_axis = 0;
remap_data.y_axis_sign = 0;
remap_data.z_axis = 2;
remap_data.z_axis_sign = 1;
// Need to raise the wrist function, need to set the correct axis
bmaSensor.setReampAxes(bmaSensor.REMAP_TOP_LAYER_RIGHT_CORNER);
#else
remap_data.x_axis = 0;
remap_data.x_axis_sign = 1;
remap_data.y_axis = 1;
remap_data.y_axis_sign = 0;
remap_data.z_axis = 2;
remap_data.z_axis_sign = 1;
bmaSensor.setReampAxes(bmaSensor.REMAP_BOTTOM_LAYER_BOTTOM_LEFT_CORNER);
#endif
// Need to raise the wrist function, need to set the correct axis
bmaSensor.setRemapAxes(&remap_data);
// sensor.enableFeature(BMA423_STEP_CNTR, true);
bmaSensor.enableFeature(BMA423_TILT, true);
bmaSensor.enableFeature(BMA423_WAKEUP, true);
// sensor.resetStepCounter();
// bmaSensor.enableFeature(bmaSensor.FEATURE_STEP_CNTR, true);
bmaSensor.enableFeature(bmaSensor.FEATURE_TILT, true);
bmaSensor.enableFeature(bmaSensor.FEATURE_WAKEUP, true);
// bmaSensor.resetPedometer();

// Turn on feature interrupt
bmaSensor.enableStepCountInterrupt();
bmaSensor.enableTiltInterrupt();
bmaSensor.enablePedometerIRQ();
bmaSensor.enableTiltIRQ();
// It corresponds to isDoubleClick interrupt
bmaSensor.enableWakeupInterrupt();
bmaSensor.enableWakeupIRQ();
}
}

Expand All @@ -150,8 +128,8 @@ class AccelerometerThread : public concurrency::OSThread
buttonPress();
return 500;
}
} else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && bmaSensor.getINT()) {
if (bmaSensor.isTilt() || bmaSensor.isDoubleClick()) {
} else if (acceleremoter_type == ScanI2C::DeviceType::BMA423 && bmaSensor.readIrqStatus() != DEV_WIRE_NONE) {
if (bmaSensor.isTilt() || bmaSensor.isDoubleTap()) {
wakeScreen();
return 500;
}
Expand Down
8 changes: 8 additions & 0 deletions src/SerialConsole.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,11 @@
#include "PowerFSM.h"
#include "configuration.h"

#ifdef RP2040_SLOW_CLOCK
#define Port Serial2
#else
#define Port Serial
#endif
// Defaulting to the formerly removed phone_timeout_secs value of 15 minutes
#define SERIAL_CONNECTION_TIMEOUT (15 * 60) * 1000UL

Expand Down Expand Up @@ -31,6 +35,10 @@ SerialConsole::SerialConsole() : StreamAPI(&Port), RedirectablePrint(&Port), con
canWrite = false; // We don't send packets to our port until it has talked to us first
// setDestination(&noopPrint); for testing, try turning off 'all' debug output and see what leaks

#ifdef RP2040_SLOW_CLOCK
Port.setTX(SERIAL2_TX);
Port.setRX(SERIAL2_RX);
#endif
Port.begin(SERIAL_BAUD);
#if defined(ARCH_NRF52) || defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3) || defined(ARCH_RP2040)
time_t timeout = millis();
Expand Down
51 changes: 48 additions & 3 deletions src/gps/GPS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,26 @@ bool GPS::setup()
// Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
_serial_gps->write("$PCAS11,3*1E\r\n");
delay(250);
} else if (gnssModel == GNSS_MODEL_MTK_L76B) {
// Waveshare Pico-GPS hat uses the L76B with 9600 baud
// Initialize the L76B Chip, use GPS + GLONASS
// See note in L76_Series_GNSS_Protocol_Specification, chapter 3.29
_serial_gps->write("$PMTK353,1,1,0,0,0*2B\r\n");
// Above command will reset the GPS and takes longer before it will accept new commands
delay(1000);
// only ask for RMC and GGA (GNRMC and GNGGA)
// See note in L76_Series_GNSS_Protocol_Specification, chapter 2.1
_serial_gps->write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
delay(250);
// Enable SBAS
_serial_gps->write("$PMTK301,2*2E\r\n");
delay(250);
// Enable PPS for 2D/3D fix only
_serial_gps->write("$PMTK285,3,100*3F\r\n");
delay(250);
// Switch to Fitness Mode, for running and walking purpose with low speed (<5 m/s)
_serial_gps->write("$PMTK886,1*29\r\n");
delay(250);
} else if (gnssModel == GNSS_MODEL_UC6580) {
// The Unicore UC6580 can use a lot of sat systems, enable it to
// use GPS L1 & L5 + BDS B1I & B2a + GLONASS L1 + GALILEO E1 & E5a + SBAS
Expand Down Expand Up @@ -625,17 +645,27 @@ void GPS::setGPSPower(bool on, bool standbyOnly, uint32_t sleepTime)
return;
}
#endif
#ifdef PIN_GPS_STANDBY // Specifically the standby pin for L76K and clones
#ifdef PIN_GPS_STANDBY // Specifically the standby pin for L76B, L76K and clones
if (on) {
LOG_INFO("Waking GPS\n");
pinMode(PIN_GPS_STANDBY, OUTPUT);
// Some PCB's use an inverse logic due to a transistor driver
// Example for this is the Pico-Waveshare Lora+GPS HAT
#ifdef PIN_GPS_STANDBY_INVERTED
digitalWrite(PIN_GPS_STANDBY, 0);
#else
digitalWrite(PIN_GPS_STANDBY, 1);
#endif
return;
} else {
LOG_INFO("GPS entering sleep\n");
// notifyGPSSleep.notifyObservers(NULL);
pinMode(PIN_GPS_STANDBY, OUTPUT);
#ifdef PIN_GPS_STANDBY_INVERTED
digitalWrite(PIN_GPS_STANDBY, 1);
#else
digitalWrite(PIN_GPS_STANDBY, 0);
#endif
return;
}
#endif
Expand Down Expand Up @@ -916,7 +946,7 @@ GnssModel_t GPS::probe(int serialSpeed)
uint8_t buffer[768] = {0};
delay(100);

// Close all NMEA sentences , Only valid for MTK platform
// Close all NMEA sentences , Only valid for L76K MTK platform
_serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
delay(20);

Expand All @@ -928,6 +958,18 @@ GnssModel_t GPS::probe(int serialSpeed)
return GNSS_MODEL_MTK;
}

// Close all NMEA sentences, valid for L76B MTK platform (Waveshare Pico GPS)
_serial_gps->write("$PMTK514,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2E\r\n");
delay(20);

// Get version information
clearBuffer();
_serial_gps->write("$PMTK605*31\r\n");
if (getACK("Quectel-L76B", 500) == GNSS_RESPONSE_OK) {
LOG_INFO("L76B GNSS init succeeded, using L76B GNSS Module\n");
return GNSS_MODEL_MTK_L76B;
}

uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00};
UBXChecksum(cfg_rate, sizeof(cfg_rate));
clearBuffer();
Expand Down Expand Up @@ -1109,7 +1151,6 @@ GPS *GPS::createGps()
LOG_DEBUG("Using GPIO%d for GPS RX\n", new_gps->rx_gpio);
LOG_DEBUG("Using GPIO%d for GPS TX\n", new_gps->tx_gpio);
_serial_gps->begin(GPS_BAUDRATE, SERIAL_8N1, new_gps->rx_gpio, new_gps->tx_gpio);

#else
_serial_gps->begin(GPS_BAUDRATE);
#endif
Expand Down Expand Up @@ -1174,6 +1215,10 @@ bool GPS::factoryReset()
_serial_gps->write("$PCAS10,3*1F\r\n");
delay(100);
} else {
// fire this for good measure, if we have an L76B - won't harm other devices.
_serial_gps->write("$PMTK104*37\r\n");
// No PMTK_ACK for this command.
delay(100);
// send the UBLOX Factory Reset Command regardless of detect state, something is very wrong, just assume it's UBLOX.
// Factory Reset
byte _message_reset[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0xFF, 0xFB, 0x00, 0x00, 0x00,
Expand Down
12 changes: 5 additions & 7 deletions src/gps/GPS.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,7 @@ struct uBloxGnssModelInfo {
char extension[10][30];
};

typedef enum {
GNSS_MODEL_MTK,
GNSS_MODEL_UBLOX,
GNSS_MODEL_UC6580,
GNSS_MODEL_UNKNOWN,
} GnssModel_t;
typedef enum { GNSS_MODEL_MTK, GNSS_MODEL_UBLOX, GNSS_MODEL_UC6580, GNSS_MODEL_UNKNOWN, GNSS_MODEL_MTK_L76B } GnssModel_t;

typedef enum {
GNSS_RESPONSE_NONE,
Expand Down Expand Up @@ -92,8 +87,11 @@ class GPS : private concurrency::OSThread

public:
/** If !NULL we will use this serial port to construct our GPS */
#if defined(RPI_PICO_WAVESHARE)
static SerialUART *_serial_gps;
#else
static HardwareSerial *_serial_gps;

#endif
static uint8_t _message_PMREQ[];
static uint8_t _message_PMREQ_10[];
static const uint8_t _message_CFG_RXM_PSM[];
Expand Down
4 changes: 2 additions & 2 deletions src/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -223,7 +223,7 @@ void setup()
#if defined(TTGO_T_ECHO) && defined(PIN_POWER_EN)
pinMode(PIN_POWER_EN, OUTPUT);
digitalWrite(PIN_POWER_EN, HIGH);
digitalWrite(PIN_POWER_EN1, INPUT);
// digitalWrite(PIN_POWER_EN1, INPUT);
#endif

#if defined(LORA_TCXO_GPIO)
Expand Down Expand Up @@ -965,4 +965,4 @@ void loop()
mainDelay.delay(delayMsec);
}
// if (didWake) LOG_DEBUG("wake!\n");
}
}
16 changes: 16 additions & 0 deletions src/mesh/Channels.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@

#include <assert.h>

#include "mqtt/MQTT.h"

/// 16 bytes of random PSK for our _public_ default channel that all devices power up on (AES128)
static const uint8_t defaultpsk[] = {0xd4, 0xf1, 0xbb, 0x3a, 0x20, 0x29, 0x07, 0x59,
0xf0, 0xbc, 0xff, 0xab, 0xcf, 0x4e, 0x69, 0x01};
Expand Down Expand Up @@ -193,6 +195,10 @@ void Channels::onConfigChanged()
if (ch.role == meshtastic_Channel_Role_PRIMARY)
primaryIndex = i;
}
if (channels.anyMqttEnabled() && mqtt && !mqtt->isEnabled()) {
LOG_DEBUG("MQTT is enabled on at least one channel, so set MQTT thread to run immediately\n");
mqtt->start();
}
}

meshtastic_Channel &Channels::getByIndex(ChannelIndex chIndex)
Expand Down Expand Up @@ -237,6 +243,16 @@ void Channels::setChannel(const meshtastic_Channel &c)
old = c; // slam in the new settings/role
}

bool Channels::anyMqttEnabled()
{
for (int i = 0; i < getNumChannels(); i++)
if (channelFile.channels[i].role != meshtastic_Channel_Role_DISABLED && channelFile.channels[i].has_settings &&
(channelFile.channels[i].settings.downlink_enabled || channelFile.channels[i].settings.uplink_enabled))
return true;

return false;
}

const char *Channels::getName(size_t chIndex)
{
// Convert the short "" representation for Default into a usable string
Expand Down
3 changes: 3 additions & 0 deletions src/mesh/Channels.h
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,9 @@ class Channels
// Returns true if we can be reached via a channel with the default settings given a region and modem preset
bool hasDefaultChannel();

// Returns true if any of our channels have enabled MQTT uplink or downlink
bool anyMqttEnabled();

private:
/** Given a channel index, change to use the crypto key specified by that index
*
Expand Down
13 changes: 2 additions & 11 deletions src/mesh/NodeDB.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,16 +101,7 @@ bool NodeDB::resetRadioConfig(bool factory_reset)
// devicestate.no_save = true;
if (devicestate.no_save) {
LOG_DEBUG("***** DEVELOPMENT MODE - DO NOT RELEASE *****\n");

// Sleep quite frequently to stress test the BLE comms, broadcast position every 6 mins
config.display.screen_on_secs = 10;
config.power.wait_bluetooth_secs = 10;
config.position.position_broadcast_secs = 6 * 60;
config.power.ls_secs = 60;
config.lora.region = meshtastic_Config_LoRaConfig_RegionCode_TW;

// Enter super deep sleep soon and stay there not very long
// radioConfig.preferences.sds_secs = 60;
// Put your development config changes here
}

// Update the global myRegion
Expand Down Expand Up @@ -199,7 +190,7 @@ void NodeDB::installDefaultConfig()
config.position.broadcast_smart_minimum_distance = 100;
config.position.broadcast_smart_minimum_interval_secs = 30;
if (config.device.role != meshtastic_Config_DeviceConfig_Role_ROUTER)
config.device.node_info_broadcast_secs = 3 * 60 * 60;
config.device.node_info_broadcast_secs = default_node_info_broadcast_secs;
config.device.serial_enabled = true;
resetRadioConfig();
strncpy(config.network.ntp_server, "0.pool.ntp.org", 32);
Expand Down
Loading

0 comments on commit ef57369

Please sign in to comment.