From a3571410052be47193aeda56eafdd523f113dc1f Mon Sep 17 00:00:00 2001 From: Eric Berseth Date: Fri, 16 Oct 2020 15:36:13 -0700 Subject: [PATCH] Implement sleep framework --- lib/bmi160/src/bmi160.cpp | 40 +- lib/bmi160/src/bmi160.h | 2 +- lib/cloud-service/src/background_publish.cpp | 2 +- lib/cloud-service/src/background_publish.h | 2 +- lib/cloud-service/src/cloud_service.cpp | 10 +- lib/cloud-service/src/cloud_service.h | 2 +- lib/config-service/src/config_service.cpp | 30 +- lib/config-service/src/config_service.h | 2 + lib/location-service/src/location_service.cpp | 12 +- lib/location-service/src/location_service.h | 4 +- src/build.mk | 12 + src/gnss_led.cpp | 15 + src/gnss_led.h | 1 + src/main.cpp | 2 - src/motion_service.cpp | 14 +- src/motion_service.h | 18 +- src/temperature.cpp | 15 +- src/temperature.h | 7 +- src/tracker.cpp | 245 ++++++- src/tracker.h | 17 +- src/tracker_cellular.cpp | 4 +- src/tracker_cellular.h | 2 +- src/tracker_config.h | 24 +- src/tracker_location.cpp | 497 +++++++++++---- src/tracker_location.h | 72 ++- src/tracker_motion.cpp | 19 +- src/tracker_shipping.cpp | 5 +- src/tracker_sleep.cpp | 598 ++++++++++++++++++ src/tracker_sleep.h | 585 +++++++++++++++++ 29 files changed, 2023 insertions(+), 235 deletions(-) create mode 100644 src/build.mk create mode 100644 src/tracker_sleep.cpp create mode 100644 src/tracker_sleep.h diff --git a/lib/bmi160/src/bmi160.cpp b/lib/bmi160/src/bmi160.cpp index 3fc05a4..0df1a8c 100644 --- a/lib/bmi160/src/bmi160.cpp +++ b/lib/bmi160/src/bmi160.cpp @@ -58,7 +58,7 @@ int Bmi160::setSpiMode() { } int Bmi160::initialize() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t reg = 0; @@ -101,7 +101,7 @@ int Bmi160::initialize() { } int Bmi160::cleanup() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); // Disable interrupts, set defaults @@ -115,7 +115,7 @@ int Bmi160::cleanup() { } int Bmi160::begin(const TwoWire* interface, uint8_t address, pin_t interruptPin, size_t eventDepth) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_FALSE(initialized_, SYSTEM_ERROR_NONE); CHECK_TRUE(interface, SYSTEM_ERROR_INVALID_ARGUMENT); @@ -147,7 +147,7 @@ int Bmi160::begin(const TwoWire* interface, uint8_t address, pin_t interruptPin, } int Bmi160::begin(const SPIClass& interface, pin_t selectPin, pin_t interruptPin, size_t eventDepth) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_FALSE(initialized_, SYSTEM_ERROR_NONE); if (os_queue_create(&motionSyncQueue_, sizeof(Bmi160EventType), eventDepth, nullptr)) { @@ -173,7 +173,7 @@ int Bmi160::begin(const SPIClass& interface, pin_t selectPin, pin_t interruptPin } int Bmi160::end() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_NONE); cleanup(); @@ -189,7 +189,7 @@ int Bmi160::end() { } int Bmi160::reset() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); if (type_ == InterfaceType::BMI_SPI) { CHECK(setSpiMode()); @@ -207,7 +207,7 @@ int Bmi160::reset() { } int Bmi160::sleep() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); CHECK(writeRegister(Bmi160Register::CMD_ADDR, Bmi160Command::CMD_ACC_PMU_MODE_SUSPEND)); delay(BMI160_ACC_PMU_CMD_TIME); @@ -216,7 +216,7 @@ int Bmi160::sleep() { } int Bmi160::wakeup() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); CHECK(writeRegister(Bmi160Register::CMD_ADDR, Bmi160Command::CMD_ACC_PMU_MODE_LOW)); delay(BMI160_ACC_PMU_CMD_TIME); @@ -489,7 +489,7 @@ int Bmi160::setAccelHighGHysteresis(float& hysteresis, bool feedback) { } int Bmi160::initAccelerometer(Bmi160AccelerometerConfig& config, bool feedback) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); // Setting maximum range @@ -502,7 +502,7 @@ int Bmi160::initAccelerometer(Bmi160AccelerometerConfig& config, bool feedback) } int Bmi160::getAccelerometer(Bmi160Accelerometer& data) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t buffer[6]; @@ -518,7 +518,7 @@ int Bmi160::getAccelerometer(Bmi160Accelerometer& data) { } int Bmi160::getAccelerometerPmu(Bmi160PowerState& pmu) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t val = 0; CHECK(readRegister(Bmi160Register::PMU_STATUS_ADDR, &val)); @@ -548,7 +548,7 @@ int Bmi160::getAccelerometerPmu(Bmi160PowerState& pmu) { } int Bmi160::startMotionDetect() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t reg = 0; @@ -562,7 +562,7 @@ int Bmi160::startMotionDetect() { } int Bmi160::stopMotionDetect() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t reg = 0; @@ -576,7 +576,7 @@ int Bmi160::stopMotionDetect() { } int Bmi160::initMotion(Bmi160AccelMotionConfig& config, bool feedback) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); // Setting motion threshold @@ -602,7 +602,7 @@ int Bmi160::initMotion(Bmi160AccelMotionConfig& config, bool feedback) { } int Bmi160::initHighG(Bmi160AccelHighGConfig& config, bool feedback) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); // Setting high G threshold @@ -618,7 +618,7 @@ int Bmi160::initHighG(Bmi160AccelHighGConfig& config, bool feedback) { } int Bmi160::startHighGDetect() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t reg = 0; @@ -632,7 +632,7 @@ int Bmi160::startHighGDetect() { } int Bmi160::stopHighGDetect() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); uint8_t reg = 0; @@ -646,7 +646,7 @@ int Bmi160::stopHighGDetect() { } int Bmi160::getStatus(uint32_t& val, bool clear) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); CHECK(readRegister(Bmi160Register::INT_STATUS_0_ADDR, reinterpret_cast(&val), sizeof(val))); @@ -669,7 +669,7 @@ bool Bmi160::isHighGDetect(uint32_t val) { } int Bmi160::getChipId(uint8_t& val) { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); CHECK_TRUE(initialized_, SYSTEM_ERROR_INVALID_STATE); return readRegister(Bmi160Register::CHIPID_ADDR, &val); } @@ -758,4 +758,4 @@ int Bmi160::readRegister(uint8_t reg, uint8_t* val, int length) { return SYSTEM_ERROR_INVALID_STATE; } -std::recursive_mutex Bmi160::mutex_; +RecursiveMutex Bmi160::mutex_; diff --git a/lib/bmi160/src/bmi160.h b/lib/bmi160/src/bmi160.h index ae14628..e9eda99 100644 --- a/lib/bmi160/src/bmi160.h +++ b/lib/bmi160/src/bmi160.h @@ -219,7 +219,7 @@ class Bmi160 { float rateAccel_; uint8_t latchShadow_; os_queue_t motionSyncQueue_; - static std::recursive_mutex mutex_; + static RecursiveMutex mutex_; }; // class Bmi160 #define BMI160 Bmi160::getInstance() diff --git a/lib/cloud-service/src/background_publish.cpp b/lib/cloud-service/src/background_publish.cpp index 54f18b9..da156ca 100644 --- a/lib/cloud-service/src/background_publish.cpp +++ b/lib/cloud-service/src/background_publish.cpp @@ -112,7 +112,7 @@ void BackgroundPublish::thread_f() bool BackgroundPublish::publish(const char *name, const char *data, PublishFlags flags, publish_completed_cb_t cb, const void *context) { // protect against separate threads trying to publish at the same time - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); // check currently in idle state and ready to accept publish request if(!thread || state != BACKGROUND_PUBLISH_IDLE) diff --git a/lib/cloud-service/src/background_publish.h b/lib/cloud-service/src/background_publish.h index ab9dda6..1eb1bb5 100644 --- a/lib/cloud-service/src/background_publish.h +++ b/lib/cloud-service/src/background_publish.h @@ -73,7 +73,7 @@ class BackgroundPublish private: Thread *thread = NULL; void thread_f(); - std::recursive_mutex mutex; + RecursiveMutex mutex; volatile publish_thread_state_t state = BACKGROUND_PUBLISH_IDLE; // arguments for Particle.publish diff --git a/lib/cloud-service/src/cloud_service.cpp b/lib/cloud-service/src/cloud_service.cpp index aad89a1..50db7d7 100644 --- a/lib/cloud-service/src/cloud_service.cpp +++ b/lib/cloud-service/src/cloud_service.cpp @@ -38,7 +38,7 @@ void CloudService::init() void CloudService::tick() { auto sec = System.uptime(); - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); if(sec != last_tick_sec) { @@ -90,7 +90,7 @@ void CloudService::tick_sec() int CloudService::regCommandCallback(const char *cmd, cloud_service_cb_t cb, uint32_t req_id, uint32_t timeout_ms, const void *context) { - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); cloud_service_handler_t handler = {CloudServicePublishFlags::NONE, cb, "", req_id, timeout_ms, context, millis()}; if(!cb) @@ -210,7 +210,7 @@ int CloudService::dispatchCommand(String data) return -EINVAL; } - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); auto it = handlers.begin(); while(it != handlers.end()) { @@ -302,7 +302,7 @@ void CloudService::publish_cb( const char *event_data, const void *event_context) { - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); if(!event_context) { @@ -342,7 +342,7 @@ int CloudService::send(const char *event, { int rval = 0; size_t event_len = strlen(event); - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); if(!event_name || (!req_id && cb && (cloud_flags & CloudServicePublishFlags::FULL_ACK))) diff --git a/lib/cloud-service/src/cloud_service.h b/lib/cloud-service/src/cloud_service.h index 30739f6..f1de198 100644 --- a/lib/cloud-service/src/cloud_service.h +++ b/lib/cloud-service/src/cloud_service.h @@ -193,7 +193,7 @@ class CloudService std::list handlers; std::list deferred_handlers; - std::recursive_mutex mutex; + RecursiveMutex mutex; }; template diff --git a/lib/config-service/src/config_service.cpp b/lib/config-service/src/config_service.cpp index af892f9..0fa8d35 100644 --- a/lib/config-service/src/config_service.cpp +++ b/lib/config-service/src/config_service.cpp @@ -99,7 +99,7 @@ int ConfigObject::exit(bool write, int status) bool ConfigFloat::check(double value) { return ( - (isnan(range_min) || value >= range_min) && + (isnan(range_min) || value >= range_min) && (isnan(range_max) || value <= range_max) ); }; @@ -168,7 +168,7 @@ void ConfigService::init() CloudService::instance().regCommandCallback("set_cfg", &ConfigService::set_cfg_cb, this); CloudService::instance().regCommandCallback("get_cfg", &ConfigService::get_cfg_cb, this); CloudService::instance().regCommandCallback("reset_to_factory", &ConfigService::reset_to_factory_cb, this); - + struct stat st; if (!stat(CONFIG_SERVICE_FS_PATH, &st)) @@ -198,6 +198,18 @@ void ConfigService::tick() } } +void ConfigService::flush() +{ + if(fs_ok) + { + for(auto &it : configs) + { + config_hash(it.root, it.hash); + } + save_all(); + } +} + void ConfigService::tick_sec() { // iterate through all configs once a second to publish config updates @@ -375,12 +387,12 @@ int ConfigService::_save(config_service_desc_t &config_desc, bool force) close(fd); - // then rename over the existing config + // then rename over the existing config if(!error) { if(_rename(temp_filename, filename)) { - error = -errno; + error = -errno; } else { @@ -472,7 +484,7 @@ int ConfigService::_load(config_service_desc_t &config_desc) } int fd = open(filename, O_RDONLY); - + if(fd < 0) { return errno; @@ -583,7 +595,7 @@ int ConfigService::get_cfg_cb(CloudServiceStatus status, JSONValue *root, const } CloudService::instance().sendAck(*root, rval); - + return rval; } @@ -625,7 +637,7 @@ int ConfigService::set_cfg_cb(CloudServiceStatus status, JSONValue *root, const auto _rval = _config_process_json(_config, it->root->name(), it->root); // want to continue processing all modules and not abort if - // one module fails but still report overall failure if + // one module fails but still report overall failure if // an earlier module fails so only update the overall rval // on an actual failure code if(_rval) @@ -841,7 +853,7 @@ int config_write_json(ConfigNode *root, JSONWriter &writer) { writer.beginObject(); } - + for(int i=0; i < object_node->child_count(); i++) { auto child = object_node->child(i); @@ -931,7 +943,7 @@ void _config_hash(ConfigNode *root, murmur3_hash_t &hash) { murmur3_hash_update(hash, root->name(), strlen(root->name())); } - + for(int i=0; i < object_node->child_count(); i++) { auto child = object_node->child(i); diff --git a/lib/config-service/src/config_service.h b/lib/config-service/src/config_service.h index 92cc1ec..eb46415 100644 --- a/lib/config-service/src/config_service.h +++ b/lib/config-service/src/config_service.h @@ -77,6 +77,8 @@ class ConfigService void resetToFactory(); + void flush(); + private: ConfigService(); static ConfigService *_instance; diff --git a/lib/location-service/src/location_service.cpp b/lib/location-service/src/location_service.cpp index 3679485..e693c67 100644 --- a/lib/location-service/src/location_service.cpp +++ b/lib/location-service/src/location_service.cpp @@ -98,6 +98,8 @@ int LocationService::stop() { int LocationService::getLocation(LocationPoint& point) { WITH_LOCK(*gps_) { point.locked = (gps_->getLock()) ? 1 : 0; + point.stable = gps_->isLockStable(); + point.lockedDuration = gps_->getLockDuration(); point.epochTime = (time_t)gps_->getUTCTime(); point.timeScale = LocationTimescale::TIMESCALE_UTC; if (point.locked) { @@ -115,19 +117,19 @@ int LocationService::getLocation(LocationPoint& point) { } int LocationService::getRadiusThreshold(float& radius) { - const std::lock_guard lock(pointMutex_); + const std::lock_guard lock(pointMutex_); radius = pointThreshold_.radius; return SYSTEM_ERROR_NONE; } int LocationService::setRadiusThreshold(float radius) { - const std::lock_guard lock(pointMutex_); + const std::lock_guard lock(pointMutex_); pointThreshold_.radius = std::fabs(radius); return SYSTEM_ERROR_NONE; } int LocationService::getWayPoint(float& latitude, float& longitude) { - const std::lock_guard lock(pointMutex_); + const std::lock_guard lock(pointMutex_); CHECK_TRUE(pointThresholdConfigured_, SYSTEM_ERROR_INVALID_STATE); latitude = pointThreshold_.latitude; longitude = pointThreshold_.longitude; @@ -135,7 +137,7 @@ int LocationService::getWayPoint(float& latitude, float& longitude) { } int LocationService::setWayPoint(float latitude, float longitude) { - const std::lock_guard lock(pointMutex_); + const std::lock_guard lock(pointMutex_); pointThreshold_.latitude = latitude; pointThreshold_.longitude = longitude; pointThresholdConfigured_ = true; @@ -143,7 +145,7 @@ int LocationService::setWayPoint(float latitude, float longitude) { } int LocationService::getWayPoint(PointThreshold& point) { - const std::lock_guard lock(pointMutex_); + const std::lock_guard lock(pointMutex_); CHECK_TRUE(pointThresholdConfigured_, SYSTEM_ERROR_INVALID_STATE); point = pointThreshold_; return SYSTEM_ERROR_NONE; diff --git a/lib/location-service/src/location_service.h b/lib/location-service/src/location_service.h index 33ece55..ab444b8 100644 --- a/lib/location-service/src/location_service.h +++ b/lib/location-service/src/location_service.h @@ -40,6 +40,8 @@ enum class LocationTimescale { */ struct LocationPoint { int locked; /**< Indication of GNSS locked status */ + unsigned int lockedDuration; /**< Duration of the current GNSS lock (if applicable) */ + bool stable; /**< Indication if GNNS lock is stable (if applicable) */ time_t epochTime; /**< Epoch time from device sources */ LocationTimescale timeScale; /**< Epoch timescale */ float latitude; /**< Point latitude in degrees */ @@ -226,7 +228,7 @@ class LocationService { */ int getWayPoint(PointThreshold& point); - std::recursive_mutex pointMutex_; + RecursiveMutex pointMutex_; uint16_t selectPin_; uint16_t enablePin_; ubloxGPS* gps_; diff --git a/src/build.mk b/src/build.mk new file mode 100644 index 0000000..4f5e12c --- /dev/null +++ b/src/build.mk @@ -0,0 +1,12 @@ +INCLUDE_DIRS += $(SOURCE_PATH)/$(USRSRC) # add user sources to include path +# add C and CPP files - if USRSRC is not empty, then add a slash +CPPSRC += $(call target_files,$(USRSRC_SLASH),*.cpp) +CSRC += $(call target_files,$(USRSRC_SLASH),*.c) + +APPSOURCES=$(call target_files,$(USRSRC_SLASH),*.cpp) +APPSOURCES+=$(call target_files,$(USRSRC_SLASH),*.c) +ifeq ($(strip $(APPSOURCES)),) +$(error "No sources found in $(SOURCE_PATH)/$(USRSRC)") +endif + +CPPFLAGS+=-DMCP2515_NORMAL_WRITES diff --git a/src/gnss_led.cpp b/src/gnss_led.cpp index 786f961..dc9fe0e 100644 --- a/src/gnss_led.cpp +++ b/src/gnss_led.cpp @@ -21,11 +21,19 @@ static void GnssLedTimer(); static Timer* timer_ = nullptr; +static bool enabled = true; static LocationStatus lastStatus_ = { .powered = -1, .locked = -1 }; static int blinkCount_ = GNSS_LED_CONTROL_BLINK_PERIOD_MS / GNSS_LED_CONTROL_TIMER_PERIOD_MS; static bool blinkState_ = false; static void GnssLedTimer() { + + if (!enabled) { + digitalWrite(TRACKER_GNSS_LOCK_LED, HIGH); + lastStatus_ = { .powered = -1, .locked = -1 }; + return; + } + LocationStatus status = {0}; (void)LocationService::instance().getStatus(status); @@ -74,3 +82,10 @@ int GnssLedInit() { return SYSTEM_ERROR_NONE; } + +void GnssLedEnable(bool enable) { + enabled = enable; + if (!enabled) { + digitalWrite(TRACKER_GNSS_LOCK_LED, HIGH); + } +} diff --git a/src/gnss_led.h b/src/gnss_led.h index 92762cf..42c6377 100644 --- a/src/gnss_led.h +++ b/src/gnss_led.h @@ -22,3 +22,4 @@ constexpr system_tick_t GNSS_LED_CONTROL_TIMER_PERIOD_MS = 50; constexpr system_tick_t GNSS_LED_CONTROL_BLINK_PERIOD_MS = 250; int GnssLedInit(); +void GnssLedEnable(bool enable); diff --git a/src/main.cpp b/src/main.cpp index 8b80320..156cf13 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -35,8 +35,6 @@ SerialLogHandler logHandler(115200, LOG_LEVEL_TRACE, { void setup() { Tracker::instance().init(); - - Particle.connect(); } void loop() diff --git a/src/motion_service.cpp b/src/motion_service.cpp index 89f0463..544243e 100644 --- a/src/motion_service.cpp +++ b/src/motion_service.cpp @@ -92,11 +92,7 @@ int MotionService::start(size_t eventDepth) { int ret = SYSTEM_ERROR_NONE; // Retrieve (if first time) instance of the BMI160 IMU device -#if USE_BMI160_XFACE == BMI160_I2C - ret = BMI160.begin(BMI160_I2C_INTERFACE, BMI160_I2C_ADDRESS, BMI160_INT_PIN); -#elif USE_BMI160_XFACE == BMI160_SPI ret = BMI160.begin(BMI160_SPI_INTERFACE, BMI160_SPI_CS_PIN, BMI160_INT_PIN); -#endif // USE_BMI160_XFACE if (ret != SYSTEM_ERROR_NONE) { LOG(ERROR, "BMI160.begin() failed"); return ret; @@ -151,7 +147,7 @@ int MotionService::enableMotionDetection(MotionDetectionMode mode) { case MotionDetectionMode::NONE: { CHECK(BMI160.stopMotionDetect()); clearAwakeFlag(MOTION_AWAKE_SIGANY); - if (!isAnyoneAwake()) { + if (!isAnyAwake()) { CHECK(BMI160.sleep()); } mode_ = mode; @@ -179,7 +175,7 @@ int MotionService::enableMotionDetection(MotionDetectionMode mode) { } } - if (!isAnyoneAwake()) { + if (!isAnyAwake()) { CHECK(BMI160.wakeup()); } setAwakeFlag(MOTION_AWAKE_SIGANY); @@ -199,7 +195,7 @@ MotionDetectionMode MotionService::getMotionDetection() { } int MotionService::enableHighGDetection() { - if (!isAnyoneAwake()) { + if (!isAnyAwake()) { CHECK(BMI160.wakeup()); } setAwakeFlag(MOTION_AWAKE_HIGH_G); @@ -214,7 +210,7 @@ int MotionService::disableHighGDetection() { CHECK(BMI160.stopHighGDetect()); highGMode_ = HighGDetectionMode::DISABLE; clearAwakeFlag(MOTION_AWAKE_HIGH_G); - if (!isAnyoneAwake()) { + if (!isAnyAwake()) { CHECK(BMI160.sleep()); } @@ -308,6 +304,6 @@ void MotionService::clearAwakeFlag(uint32_t bits) { awakeFlags_ &= ~bits; } -bool MotionService::isAnyoneAwake() { +bool MotionService::isAnyAwake() { return (awakeFlags_ == MOTION_AWAKE_NONE) ? false : true; } diff --git a/src/motion_service.h b/src/motion_service.h index e22ad63..127232a 100644 --- a/src/motion_service.h +++ b/src/motion_service.h @@ -192,11 +192,19 @@ class MotionService { /** * @brief Get the event queue depth - * + * * @return size_t Queue capacity */ size_t getQueueDepth(); + /** + * @brief Indicate if any IMU module is awake + * + * @return true At least one module is keeping the IMU from sleeping + * @return false No modules are keeping the IMU from sleeping + */ + bool isAnyAwake(); + private: MotionService(); @@ -223,14 +231,6 @@ class MotionService { */ void clearAwakeFlag(uint32_t bits); - /** - * @brief Indicate if any IMU module is awake - * - * @return true At least one module is keeping the IMU from sleeping - * @return false No modules are keeping the IMU from sleeping - */ - bool isAnyoneAwake(); - os_thread_t thread_; MotionCounters counters_; os_queue_t motionEventQueue_; diff --git a/src/temperature.cpp b/src/temperature.cpp index 3128dca..1948f69 100644 --- a/src/temperature.cpp +++ b/src/temperature.cpp @@ -17,6 +17,7 @@ #include #include "thermistor.h" #include "temperature.h" +#include "tracker_sleep.h" // Configuration based on Panasonic ERTJ1VR104FM NTC thermistor @@ -78,6 +79,12 @@ enum class TempState { static Thermistor _thermistor; static TemperatureCallback _enableCharging = nullptr; static TemperatureCallback _disableCharging = nullptr; +static unsigned int chargeEvalTick = 0; + +void onWake(TrackerSleepContext context) { + // Allow evaluation immediately after wake + chargeEvalTick = 0; +} float get_temperature() { return _thermistor.getTemperature(); @@ -105,6 +112,8 @@ int temperature_init(pin_t analogPin, TemperatureCallback chargeEnable, Temperat _enableCharging = chargeEnable; _disableCharging = chargeDisable; + void onWake(TrackerSleepContext context); + return SYSTEM_ERROR_NONE; } @@ -221,14 +230,14 @@ void disableCharging(float temperature) { } void evaluate_charge_temperature(float temperature) { - static unsigned int tick = 0; static TempState chargeTempState = TempState::UNKNOWN; - if (System.uptime() - tick < ChargeTickRateSec) { + unsigned int evalLoopInterval = TrackerSleep::instance().isSleepDisabled() ? ChargeTickAwakeEvalInterval : ChargeTickSleepEvalInterval; + if (System.uptime() - chargeEvalTick < evalLoopInterval) { return; } - tick = System.uptime(); + chargeEvalTick = System.uptime(); bool isTempHigh = (temperature >= ChargeTempHighLimit); // Inclusive bool isTempBelowHighHyst = (temperature <= (ChargeTempHighLimit - ChargeTempHyst)); // Inclusive diff --git a/src/temperature.h b/src/temperature.h index 8588548..6adfd23 100644 --- a/src/temperature.h +++ b/src/temperature.h @@ -40,8 +40,11 @@ constexpr double ChargeTempLowLimit = 0.0; // degrees celsius // Hysteresis applied to high/low limits to re-enable battery charging constexpr double ChargeTempHyst = 2.0; // degrees celsius -// Rate, in seconds, to sample the temperature and evaluate battery charge enablement -constexpr unsigned int ChargeTickRateSec = 60; // seconds +// Rate, in seconds, to sample the temperature and evaluate battery charge enablement when awake +constexpr unsigned int ChargeTickAwakeEvalInterval = 60; // seconds + +// Rate, in seconds, to sample the temperature and evaluate battery charge enablement when woken +constexpr unsigned int ChargeTickSleepEvalInterval = 1; // seconds /** * @brief Get the current temperature diff --git a/src/tracker.cpp b/src/tracker.cpp index b67e9fe..549e0e8 100644 --- a/src/tracker.cpp +++ b/src/tracker.cpp @@ -18,15 +18,23 @@ #include "tracker.h" #include "tracker_cellular.h" +#include "mcp_can.h" -constexpr int TrackerLowBatteryCutoff = 8; // percent of battery charge +// Defines and constants +constexpr int CanSleepRetries = 10; // Based on a series of 10ms delays + +constexpr int TrackerLowBatteryCutoff = 2; // percent of battery charge constexpr int TrackerLowBatteryCutoffCorrection = 1; // percent of battery charge -constexpr int TrackerLowBatteryWarning = 15; // percent of battery charge +constexpr int TrackerLowBatteryWarning = 8; // percent of battery charge constexpr int TrackerLowBatteryWarningHyst = 1; // percent of battery charge -constexpr unsigned int TrackerLowBatteryEvalTime = 15 * 60; // seconds to sample for low battery condition +constexpr unsigned int TrackerLowBatteryAwakeEvalInterval = 15 * 60; // seconds to sample for low battery condition +constexpr unsigned int TrackerLowBatterySleepEvalInterval = 1; // seconds to sample for low battery condition +constexpr unsigned int TrackerLowBatterySleepWakeInterval = 15 * 60; // seconds to sample for low battery condition constexpr system_tick_t TrackerPostChargeSettleTime = 500; // milliseconds constexpr unsigned int TrackerLowBatteryStartTime = 20; // seconds to debounce low battery condition constexpr unsigned int TrackerLowBatteryDebounceTime = 5; // seconds to debounce low battery condition +constexpr unsigned int TrackerChargingAwakeEvalTime = 10; // seconds to sample the PMIC charging state +constexpr unsigned int TrackerChargingSleepEvalTime = 1; // seconds to sample the PMIC charging state void ctrl_request_custom_handler(ctrl_request* req) { @@ -52,6 +60,7 @@ Tracker *Tracker::_instance = nullptr; Tracker::Tracker() : cloudService(CloudService::instance()), configService(ConfigService::instance()), + sleep(TrackerSleep::instance()), locationService(LocationService::instance()), motionService(MotionService::instance()), rtc(AM1805_PIN_INVALID, RTC_AM1805_I2C_INSTANCE, RTC_AM1805_I2C_ADDR), @@ -69,7 +78,9 @@ Tracker::Tracker() : _delayedBatteryCheckTick(0), _pendingChargeStatus{.uptime = 0, .state = TrackerChargeState::CHARGE_INIT}, _chargeStatus(TrackerChargeState::CHARGE_INIT), - _lowBatteryEvent(0) + _lowBatteryEvent(0), + _evalChargingTick(0), + _batteryChargeEnabled(true) { _config = { @@ -82,16 +93,101 @@ int Tracker::registerConfig() static ConfigObject tracker_config("tracker", { ConfigBool("usb_cmd", &_config.UsbCommandEnable), }); - ConfigService::instance().registerModule(tracker_config); + configService.registerModule(tracker_config); return 0; } -void Tracker::enterLowBatteryShippingMode() { +void Tracker::initIo() +{ + // Initialize basic Tracker GPIO to known inactive values until they are needed later + + // ESP32 related GPIO + pinMode(ESP32_BOOT_MODE_PIN, OUTPUT); + digitalWrite(ESP32_BOOT_MODE_PIN, HIGH); + pinMode(ESP32_PWR_EN_PIN, OUTPUT); + digitalWrite(ESP32_PWR_EN_PIN, LOW); // power off device, first power off for ESP32 workaround for low power + delay(50); // ESP32 workaround for low power + digitalWrite(ESP32_PWR_EN_PIN, HIGH); // power on device, ESP32 workaround for low power + delay(50); // ESP32 workaround for low power + digitalWrite(ESP32_PWR_EN_PIN, LOW); // power off device + pinMode(ESP32_CS_PIN, OUTPUT); + digitalWrite(ESP32_CS_PIN, HIGH); + + // CAN related GPIO + pinMode(MCP_CAN_STBY_PIN, OUTPUT); + digitalWrite(MCP_CAN_STBY_PIN, LOW); + pinMode(MCP_CAN_PWR_EN_PIN, OUTPUT); + digitalWrite(MCP_CAN_PWR_EN_PIN, HIGH); // The CAN 5V power supply will be enabled for a short period + pinMode(MCP_CAN_RESETN_PIN, OUTPUT); + digitalWrite(MCP_CAN_RESETN_PIN, HIGH); + pinMode(MCP_CAN_INT_PIN, INPUT_PULLUP); + pinMode(MCP_CAN_CS_PIN, OUTPUT); + digitalWrite(MCP_CAN_CS_PIN, HIGH); + + // Reset CAN transceiver + digitalWrite(MCP_CAN_RESETN_PIN, LOW); + delay(50); + digitalWrite(MCP_CAN_RESETN_PIN, HIGH); + delay(50); + + // Initialize CAN device driver + MCP_CAN can(MCP_CAN_CS_PIN, MCP_CAN_SPI_INTERFACE); + if (can.begin(MCP_RX_ANY, CAN_1000KBPS, MCP_20MHZ) != CAN_OK) + { + Log.error("CAN init failed"); + } + Log.info("CAN status: 0x%x", can.getCANStatus()); + if (can.setMode(MODE_NORMAL)) { + Log.error("CAN mode to NORMAL failed"); + } + else { + Log.info("CAN mode to NORMAL"); + } + delay(500); + + // Set to standby + digitalWrite(MCP_CAN_STBY_PIN, HIGH); + digitalWrite(MCP_CAN_PWR_EN_PIN, LOW); // The CAN 5V power supply will now be disabled + + for (int retries = CanSleepRetries; retries >= 0; retries--) { + auto stat = can.getCANStatus() & MODE_MASK; + if (stat == MODE_SLEEP) { + Log.info("CAN mode to SLEEP"); + break; + } + // Retry setting the sleep mode + if (can.setMode(MODE_SLEEP)) { + Log.error("CAN mode not set to SLEEP"); + } + delay(10); + } +} + +void Tracker::enableWatchdog(bool enable) { +#ifndef RTC_WDT_DISABLE + if (enable) { + // watchdog at 1 minute + rtc.configure_wdt(true, 15, AM1805_WDT_REGISTER_WRB_QUARTER_HZ); + rtc.reset_wdt(); + } + else { + rtc.disable_wdt(); + } +#else + (void)enable; +#endif // RTC_WDT_DISABLE +} + +void Tracker::startLowBatteryShippingMode() { + if (sleep.isForcedShutdownPending()) { + return; + } + // Publish then shutdown - Particle.publishVitals(); - TrackerLocation::instance().triggerLocPub(Trigger::IMMEDIATE,"batt_low"); - shipping.enter(true); + sleep.forcePublishVitals(); + location.triggerLocPub(Trigger::IMMEDIATE,"batt_low"); + sleep.forceShutdown(); } void Tracker::lowBatteryHandler(system_event_t event, int data) { @@ -164,11 +260,30 @@ void Tracker::initBatteryMonitor() { // getSoC(), or reading will not have updated yet. delay(200); - pmic.enableCharging(); + if (_batteryChargeEnabled) { + pmic.enableCharging(); + } pmic.disableWatchdog(); } +bool Tracker::getChargeEnabled() { + auto chargeStatus = (PMIC().readPowerONRegister() >> 4) & 0b11; // Mask on bits 4 and 5 + return (chargeStatus != 0b00); +} + void Tracker::evaluateBatteryCharge() { + // Manage charge enablement/disablement workaround + unsigned int evalChargingInterval = sleep.isSleepDisabled() ? TrackerChargingAwakeEvalTime : TrackerChargingSleepEvalTime; + if (System.uptime() - _evalChargingTick > evalChargingInterval) { + auto chargeEnabled = getChargeEnabled(); + if (!chargeEnabled && _batteryChargeEnabled) { + enableCharging(); + } + else if (chargeEnabled && !_batteryChargeEnabled) { + disableCharging(); + } + } + // This is delayed intialization for the fuel gauge threshold since power on // events may glitch between battery states easily. if (_delayedBatteryCheck) { @@ -176,7 +291,7 @@ void Tracker::evaluateBatteryCharge() { _delayedBatteryCheck = false; FuelGauge fuelGauge; - // Set the alert level for 8% - 1%. This value will not be normalized but rather the raw + // Set the alert level for - 1%. This value will not be normalized but rather the raw // threshold value provided by the fuel gauge. // The fuel gauge will only give an alert when passing through this limit with decreasing // succesive charge amounts. It is important to check whether we are already below this limit @@ -205,17 +320,20 @@ void Tracker::evaluateBatteryCharge() { } // No further work necessary if we are still in the delayed battery check interval or not on a evaluation interval + unsigned int evalLoopInterval = sleep.isSleepDisabled() ? TrackerLowBatteryAwakeEvalInterval : TrackerLowBatterySleepEvalInterval; if (_delayedBatteryCheck || - (System.uptime() - _evalTick < TrackerLowBatteryEvalTime)) { + (System.uptime() - _evalTick < evalLoopInterval)) { return; } + _evalTick = System.uptime(); auto stateOfCharge = System.batteryCharge(); // Skip errors if (stateOfCharge < 0.0) { + Log.info("Battery charge reporting error"); return; } @@ -224,7 +342,7 @@ void Tracker::evaluateBatteryCharge() { if (_lowBatteryEvent || (stateOfCharge <= (float)TrackerLowBatteryCutoff)) { // Publish then shutdown Log.error("Battery charge of %0.1f%% is less than limit of %0.1f%%. Entering shipping mode", stateOfCharge, (float)TrackerLowBatteryCutoff); - enterLowBatteryShippingMode(); + startLowBatteryShippingMode(); } else if (!_pastWarnLimit && (stateOfCharge <= (float)TrackerLowBatteryWarning)) { _pastWarnLimit = true; @@ -241,7 +359,7 @@ void Tracker::evaluateBatteryCharge() { if (_lowBatteryEvent) { // Publish then shutdown Log.error("Battery charge of %0.1f%% is less than limit of %0.1f%%. Entering shipping mode", stateOfCharge, (float)TrackerLowBatteryCutoff); - enterLowBatteryShippingMode(); + startLowBatteryShippingMode(); } else if (_pastWarnLimit && (stateOfCharge >= (float)(TrackerLowBatteryWarning + TrackerLowBatteryWarningHyst))) { _pastWarnLimit = false; @@ -252,8 +370,49 @@ void Tracker::evaluateBatteryCharge() { } } +void Tracker::onSleepPrepare(TrackerSleepContext context) +{ + configService.flush(); + if (_model == TRACKER_MODEL_TRACKERONE) { + TrackerSleep::instance().wakeAtSeconds(System.uptime() + TrackerLowBatterySleepWakeInterval); + } +} + +void Tracker::onSleep(TrackerSleepContext context) +{ + if (_model == TRACKER_MODEL_TRACKERONE) { + GnssLedEnable(false); + } +} + +void Tracker::onWake(TrackerSleepContext context) +{ + if (_model == TRACKER_MODEL_TRACKERONE) { + // Battery charge enable/disable enforcement upon wake in case the Power Manager overrides the setting + if (_batteryChargeEnabled) { + enableCharging(); + } + else { + disableCharging(); + } + + GnssLedEnable(true); + // Ensure battery evaluation starts immediately after waking + _evalTick = 0; + } +} + +void Tracker::onSleepStateChange(TrackerSleepContext context) +{ + if (context.reason == TrackerSleepReason::STATE_TO_SHUTDOWN) { + // Consider any device shutdown here + } +} + void Tracker::init() { + int ret = 0; + last_loop_sec = System.uptime(); // mark setup as complete to skip mobile app commissioning flow @@ -264,7 +423,8 @@ void Tracker::init() dct_write_app_data(&val, DCT_SETUP_DONE_OFFSET, DCT_SETUP_DONE_SIZE); } - int ret = hal_get_device_hw_model(&_model, &_variant, nullptr); +#ifndef TRACKER_MODEL_NUMBER + ret = hal_get_device_hw_model(&_model, &_variant, nullptr); if (ret) { Log.error("Failed to read device model and variant"); @@ -273,24 +433,41 @@ void Tracker::init() { Log.info("Tracker model = %04lX, variant = %04lX", _model, _variant); } +#else + _model = TRACKER_MODEL_NUMBER; +#ifdef TRACKER_MODEL_VARIANT + _variant = TRACKER_MODEL_VARIANT; +#else + _variant = 0; +#endif // TRACKER_MODEL_VARIANT +#endif // TRACKER_MODEL_NUMBER // PIN_INVALID to disable unnecessary config of a default CS pin SPI.begin(PIN_INVALID); + // Initialize unused interfaces and pins + initIo(); + // Reset the fuel gauge state-of-charge, check if under thresholds if (_model == TRACKER_MODEL_TRACKERONE) { initBatteryMonitor(); } - CloudService::instance().init(); + cloudService.init(); + + configService.init(); - ConfigService::instance().init(); + sleep.init([this](bool enable){ this->enableWatchdog(enable); }); + sleep.registerSleepPrepare([this](TrackerSleepContext context){ this->onSleepPrepare(context); }); + sleep.registerSleep([this](TrackerSleepContext context){ this->onSleep(context); }); + sleep.registerWake([this](TrackerSleepContext context){ this->onWake(context); }); + sleep.registerStateChange([this](TrackerSleepContext context){ this->onSleepStateChange(context); }); // Register our own configuration settings registerConfig(); - ret = LocationService::instance().begin(UBLOX_SPI_INTERFACE, + ret = locationService.begin(UBLOX_SPI_INTERFACE, UBLOX_CS_PIN, UBLOX_PWR_EN_PIN, UBLOX_TX_READY_MCU_PIN, @@ -300,7 +477,7 @@ void Tracker::init() Log.error("Failed to begin location service"); } - LocationService::instance().start(); + locationService.start(); // Check for Tracker One hardware if (_model == TRACKER_MODEL_TRACKERONE) @@ -312,7 +489,7 @@ void Tracker::init() ); } - MotionService::instance().start(); + motionService.start(); location.init(); @@ -324,12 +501,7 @@ void Tracker::init() rgb.init(); rtc.begin(); -#ifdef RTC_WDT_DISABLE - rtc.disable_wdt(); -#else - // watchdog at 1 minute - rtc.configure_wdt(true, 15, AM1805_WDT_REGISTER_WRB_QUARTER_HZ); -#endif + enableWatchdog(true); location.regLocGenCallback(loc_gen_cb); } @@ -343,9 +515,9 @@ void Tracker::loop() { last_loop_sec = cur_sec; - #ifndef RTC_WDT_DISABLE - wdt_rtc->reset_wdt(); - #endif +#ifndef RTC_WDT_DISABLE + rtc.reset_wdt(); +#endif } // Evaluate low battery conditions @@ -355,9 +527,8 @@ void Tracker::loop() } // fast operations for every loop - CloudService::instance().tick(); - ConfigService::instance().tick(); - TrackerMotion::instance().loop(); + sleep.loop(); + motion.loop(); // Check for Tracker One hardware if (_model == TRACKER_MODEL_TRACKERONE) @@ -375,24 +546,30 @@ void Tracker::loop() } } + + // fast operations for every loop + cloudService.tick(); + configService.tick(); location.loop(); } int Tracker::stop() { - LocationService::instance().stop(); - MotionService::instance().stop(); + locationService.stop(); + motionService.stop(); return 0; } int Tracker::enableCharging() { PMIC pmic(true); + _batteryChargeEnabled = true; return (pmic.enableCharging()) ? SYSTEM_ERROR_NONE : SYSTEM_ERROR_IO; } int Tracker::disableCharging() { PMIC pmic(true); + _batteryChargeEnabled = false; return (pmic.disableCharging()) ? SYSTEM_ERROR_NONE : SYSTEM_ERROR_IO; } diff --git a/src/tracker.h b/src/tracker.h index afad952..df3fd8f 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -27,12 +27,14 @@ #include "motion_service.h" #include "AM1805.h" +#include "tracker_sleep.h" #include "tracker_location.h" #include "tracker_motion.h" #include "tracker_shipping.h" #include "tracker_rgb.h" #include "gnss_led.h" #include "temperature.h" +#include "mcp_can.h" struct TrackerConfig { @@ -65,7 +67,7 @@ class Tracker return *_instance; } - void enterLowBatteryShippingMode(); + void startLowBatteryShippingMode(); void init(); void loop(); int stop(); @@ -78,9 +80,12 @@ class Tracker bool isUsbCommandEnabled() { return _config.UsbCommandEnable; } + void enableWatchdog(bool enable); + // underlying services exposed to allow sharing with rest of the system CloudService &cloudService; ConfigService &configService; + TrackerSleep &sleep; LocationService &locationService; MotionService &motionService; @@ -109,7 +114,14 @@ class Tracker std::mutex _pendingLock; TrackerChargeState _chargeStatus; unsigned int _lowBatteryEvent; - + unsigned int _evalChargingTick; + bool _batteryChargeEnabled; + + void initIo(); + void onSleepPrepare(TrackerSleepContext context); + void onSleep(TrackerSleepContext context); + void onWake(TrackerSleepContext context); + void onSleepStateChange(TrackerSleepContext context); int registerConfig(); static void loc_gen_cb(JSONWriter& writer, LocationPoint &loc, const void *context); TrackerChargeState batteryDecode(battery_state_t state); @@ -118,5 +130,6 @@ class Tracker static void lowBatteryHandler(system_event_t event, int data); static void batteryStateHandler(system_event_t event, int data); void initBatteryMonitor(); + bool getChargeEnabled(); void evaluateBatteryCharge(); }; diff --git a/src/tracker_cellular.cpp b/src/tracker_cellular.cpp index 383df0d..3b771a8 100644 --- a/src/tracker_cellular.cpp +++ b/src/tracker_cellular.cpp @@ -32,7 +32,7 @@ void TrackerCellular::thread_f() { CellularSignal rssi = Cellular.RSSI(); - if(rssi.rssi < 0) + if(rssi.getStrengthValue() < 0) { auto uptime = System.uptime(); WITH_LOCK(mutex) @@ -58,7 +58,7 @@ void TrackerCellular::thread_f() int TrackerCellular::getSignal(CellularSignal &signal, unsigned int max_age) { - const std::lock_guard lg(mutex); + const std::lock_guard lg(mutex); if(!_signal_update || System.uptime() - _signal_update > max_age) { diff --git a/src/tracker_cellular.h b/src/tracker_cellular.h index ea5f43f..7fe40c8 100644 --- a/src/tracker_cellular.h +++ b/src/tracker_cellular.h @@ -51,7 +51,7 @@ class TrackerCellular CellularSignal _signal; unsigned int _signal_update; - std::recursive_mutex mutex; + RecursiveMutex mutex; Thread * thread; void thread_f(); diff --git a/src/tracker_config.h b/src/tracker_config.h index b308f58..6218a5a 100644 --- a/src/tracker_config.h +++ b/src/tracker_config.h @@ -44,21 +44,14 @@ #define TRACKER_PRODUCT_VERSION (9) #endif -#ifndef TRACKER_MODEL_NUMBER -#define TRACKER_MODEL_NUMBER (TRACKER_MODEL_EVAL) -#endif - // // Pin and interface mapping // -#define BMI160_I2C (0) -#define BMI160_SPI (1) - -#define USE_BMI160_XFACE BMI160_SPI #define BMI160_SPI_INTERFACE (SPI1) #define BMI160_SPI_CS_PIN (SEN_CS) #define BMI160_INT_PIN (SEN_INT) +#define BMI160_INT_MODE (FALLING) #define UBLOX_SPI_INTERFACE (SPI1) #define UBLOX_CS_PIN (GPS_CS) @@ -66,6 +59,19 @@ #define UBLOX_TX_READY_MCU_PIN (GPS_INT) #define UBLOX_TX_READY_GPS_PIN (14) // PIO 14 is EXTINT on GPS Module +#define ESP32_SPI_INTERFACE (SPI1) +#define ESP32_CS_PIN (WIFI_CS) +#define ESP32_BOOT_MODE_PIN (WIFI_BOOT) +#define ESP32_PWR_EN_PIN (WIFI_EN) +#define ESP32_INT_PIN (WIFI_INT) + +#define MCP_CAN_SPI_INTERFACE (SPI1) +#define MCP_CAN_CS_PIN (CAN_CS) +#define MCP_CAN_STBY_PIN (CAN_STBY) +#define MCP_CAN_PWR_EN_PIN (CAN_PWR) +#define MCP_CAN_RESETN_PIN (CAN_RST) +#define MCP_CAN_INT_PIN (CAN_INT) + #define RTC_AM1805_I2C_INSTANCE (Wire1) #define RTC_AM1805_I2C_ADDR (HAL_PLATFORM_EXTERNAL_RTC_I2C_ADDR) @@ -73,4 +79,4 @@ #define TRACKER_USER_BUTTON (D1) #define TRACKER_GNSS_LOCK_LED (D2) -#define RTC_WDT_DISABLE +//#define RTC_WDT_DISABLE diff --git a/src/tracker_location.cpp b/src/tracker_location.cpp index 9a8f695..18f26da 100644 --- a/src/tracker_location.cpp +++ b/src/tracker_location.cpp @@ -25,7 +25,9 @@ TrackerLocation *TrackerLocation::_instance = nullptr; -static constexpr system_tick_t sample_rate = 1000; // milliseconds +static constexpr system_tick_t LoopSampleRate = 1000; // milliseconds +static constexpr uint32_t EarlySleepSec = 2; // seconds +static constexpr uint32_t MiscSleepWakeSec = 3; // seconds - miscellaneous time spent by system entering and exiting sleep static int set_radius_cb(double value, const void *context) { @@ -107,7 +109,13 @@ void TrackerLocation::init() CloudService::instance().regCommandCallback("get_loc", &TrackerLocation::get_loc_cb, this); - last_location_publish_sec = System.uptime() - config_state.interval_min_seconds; + _last_location_publish_sec = System.uptime() - config_state.interval_min_seconds; + + _sleep.registerSleepPrepare([this](TrackerSleepContext context){ this->onSleepPrepare(context); }); + _sleep.registerSleep([this](TrackerSleepContext context){ this->onSleep(context); }); + _sleep.registerSleepCancel([this](TrackerSleepContext context){ this->onSleepCancel(context); }); + _sleep.registerWake([this](TrackerSleepContext context){ this->onWake(context); }); + _sleep.registerStateChange([this](TrackerSleepContext context){ this->onSleepState(context); }); } int TrackerLocation::regLocGenCallback( @@ -121,7 +129,7 @@ int TrackerLocation::regLocGenCallback( // register for callback on location publish success/fail // these callbacks are NOT persistent and are used for the next publish int TrackerLocation::regLocPubCallback( - cloud_service_send_cb_t cb, + cloud_service_send_cb_t cb, const void *context) { locPubCallbacks.append(std::bind(cb, _1, _2, _3, context)); @@ -130,10 +138,10 @@ int TrackerLocation::regLocPubCallback( int TrackerLocation::triggerLocPub(Trigger type, const char *s) { - std::lock_guard lg(mutex); + std::lock_guard lg(mutex); bool matched = false; - for(auto trigger : pending_triggers) + for(auto trigger : _pending_triggers) { if(!strcmp(trigger, s)) { @@ -141,15 +149,15 @@ int TrackerLocation::triggerLocPub(Trigger type, const char *s) break; } } - + if(!matched) { - pending_triggers.append(s); + _pending_triggers.append(s); } if(type == Trigger::IMMEDIATE) { - pending_immediate = true; + _pending_immediate = true; } return 0; @@ -173,13 +181,14 @@ int TrackerLocation::location_publish_cb(CloudServiceStatus status, JSONValue *r // this could either be on the Particle Cloud ack (default) OR the // end-to-end ACK Log.info("location cb publish %lu success!", *(uint32_t *) context); + _first_publish = false; } else if(status == CloudServiceStatus::FAILURE) { // right now FAILURE only comes out of a Particle Cloud issue // once Particle Cloud passes if waiting on end-to-end it will // only ever timeout - + // save on failure for retry if(req_event && !location_publish_retry_str) { @@ -227,7 +236,7 @@ void TrackerLocation::location_publish() WITH_ACK, CloudServicePublishFlags::FULL_ACK, &TrackerLocation::location_publish_cb, this, - CLOUD_DEFAULT_TIMEOUT_MS, &last_location_publish_sec); + CLOUD_DEFAULT_TIMEOUT_MS, &_last_location_publish_sec); } else { @@ -235,7 +244,7 @@ void TrackerLocation::location_publish() rval = cloud_service.send(WITH_ACK, CloudServicePublishFlags::FULL_ACK, &TrackerLocation::location_publish_cb, this, - CLOUD_DEFAULT_TIMEOUT_MS, &last_location_publish_sec); + CLOUD_DEFAULT_TIMEOUT_MS, &_last_location_publish_sec); } if(rval == -EBUSY) @@ -276,146 +285,420 @@ void TrackerLocation::location_publish() cloud_service.unlock(); } -void TrackerLocation::loop() -{ - static system_tick_t last_sample = 0; - static int last_loc_locked_pub = 0; - static bool first_lock_triggered = false; +void TrackerLocation::enableNetworkGnss() { + LocationService::instance().start(); + _sleep.forceFullWakeCycle(); +} - LocationPoint cur_loc; +void TrackerLocation::disableGnss() { + LocationService::instance().stop(); +} - // - // Detect events and add to pending list to publish - // +bool TrackerLocation::isSleepEnabled() { + return !_sleep.isSleepDisabled(); +} - // This loop should only sample as fast as necessary - if (millis() - last_sample < sample_rate) - { - return; +EvaluationResults TrackerLocation::evaluatePublish() { + // This will allow a trigger publish on boot + if (_first_publish) { + Log.trace("%s first", __FUNCTION__); + return EvaluationResults {PublishReason::TRIGGERS, true}; } - last_sample = millis(); - if(LocationService::instance().getLocation(cur_loc) != SYSTEM_ERROR_NONE) - { - return; + if (_pending_immediate) { + // request for immediate publish overrides the default min/max interval checking + Log.trace("%s pending_immediate", __FUNCTION__); + return EvaluationResults {PublishReason::IMMEDIATE, true}; } - float radius; - bool outside; + auto now = System.uptime(); + uint32_t interval = now - _last_location_publish_sec; + uint32_t maxInterval = now - _monotonic_publish_sec; - if(cur_loc.locked && - LocationService::instance().getRadiusThreshold(radius) == SYSTEM_ERROR_NONE && - radius && - LocationService::instance().isOutsideRadius(outside, cur_loc) == SYSTEM_ERROR_NONE && - outside) - { - triggerLocPub(Trigger::NORMAL,"radius"); + bool networkNeeded = false; + uint32_t max = (uint32_t)config_state.interval_max_seconds; + auto maxNetwork = max; + if (maxNetwork > (uint32_t)_nextEarlyWake) { + maxNetwork -= (uint32_t)_nextEarlyWake; } - if(cur_loc.locked != last_loc_locked_pub) - { - if (config_state.lock_trigger || (!first_lock_triggered && cur_loc.locked != 0) ) + if (config_state.interval_max_seconds) { + if (maxInterval >= maxNetwork) { + // max interval adjusted for early wake + Log.trace("%s maxNetwork", __FUNCTION__); + networkNeeded = true; + } + + if (maxInterval >= max) { + // max interval and past the max interval so have to publish + Log.trace("%s max", __FUNCTION__); + return EvaluationResults {PublishReason::TIME, true}; + } + } + + uint32_t min = (uint32_t)config_state.interval_min_seconds; + auto minNetwork = min; + if (minNetwork > (uint32_t)_nextEarlyWake) { + minNetwork -= (uint32_t)_nextEarlyWake; + } + + if (_pending_triggers.size()) { + if (!config_state.interval_min_seconds || + (interval >= minNetwork)) { + // min interval adjusted for early wake + Log.trace("%s minNetwork", __FUNCTION__); + networkNeeded = true; + } + + if (!config_state.interval_min_seconds || + (interval >= min)) { + // no min interval or past the min interval so can publish + Log.trace("%s min", __FUNCTION__); + return EvaluationResults {PublishReason::TRIGGERS, true}; + } + } + + return EvaluationResults {PublishReason::NONE, networkNeeded}; +} + +// The purpose of thhe sleep prepare callback is to allow each task to calculate +// the next time it needs to wake and process inputs, publish, and what not. +void TrackerLocation::onSleepPrepare(TrackerSleepContext context) { + // The first thing to figure out is the needed interval, min or max + unsigned int wake = _last_location_publish_sec; + int32_t interval = 0; + if (_pending_triggers.size()) { + interval = config_state.interval_min_seconds; + wake += interval; + } + else { + interval = config_state.interval_max_seconds; + wake += interval; + } + + // Next calculate the early wake offset so that we can wake in the minimum amount of time before + // the next publish in order to minimize time spent in fully powered operation + auto t_conn = (uint32_t)_sleep.getConfigConnectingTime(); + if (_sleep.isFullWakeCycle()) { + uint32_t newEarlyWakeSec = 0; + uint32_t lastWakeSec = (uint32_t)(context.lastWakeMs + 500) / 1000; // Round ms to s + if (lastWakeSec >= MiscSleepWakeSec) { + lastWakeSec -= MiscSleepWakeSec; + } + uint32_t wakeToLockDurationSec = 0; + int32_t publishVariance = 0; + + if (_firstLockSec == 0) { + wakeToLockDurationSec = t_conn; + } + else { + wakeToLockDurationSec = _firstLockSec - lastWakeSec; + } + + publishVariance = (int32_t)_last_location_publish_sec - (int32_t)_monotonic_publish_sec; + + newEarlyWakeSec = wakeToLockDurationSec + publishVariance + 1; + if (newEarlyWakeSec > t_conn) { + newEarlyWakeSec = t_conn; + } + _nextEarlyWake = _earlyWake = newEarlyWakeSec; + } + else { // Not in full wake (modem on) + _nextEarlyWake = (_earlyWake == 0) ? t_conn : _earlyWake; + } + + // If the interval and early adjustments puts the wake time in the past then + // spoil the next sleep attempt and stay awake. + if (wake > _nextEarlyWake) + wake -= _nextEarlyWake; + + TrackerSleepError wakeRet = _sleep.wakeAtSeconds(wake); + + if (wakeRet == TrackerSleepError::TIME_IN_PAST) { + wake = 0; // Force cancelled sleep + _sleep.wakeAtSeconds(wake); + } + + Log.trace("TrackerLocation: last=%lu, interval=%ld, wake=%u", _last_location_publish_sec, interval, wake); +} + +// The purpose of this callback is to alert us that sleep has been cancelled by another task or improper wake settings. +void TrackerLocation::onSleepCancel(TrackerSleepContext context) { + +} + +// This callback will alert us that the system is just about to go to sleep. This is past of the point +// of no return to cancel the pending sleep cycle. +void TrackerLocation::onSleep(TrackerSleepContext context) { + disableGnss(); +} + +// This callback will be called immediately after wake from sleep and allows us to figure out if the network interface +// is needed and enable it if so. +void TrackerLocation::onWake(TrackerSleepContext context) { + // Allow capturing of the first lock instance + _firstLockSec = 0; + + auto result = evaluatePublish(); + + if (result.networkNeeded) { + enableNetworkGnss(); + Log.trace("%s needs to start the network", __FUNCTION__); + } + else { + // Put in our vote to shutdown early + _sleep.extendExecutionFromNow(EarlySleepSec, true); + } + + // Ensure the loop runs immediately + _loopSampleTick = 0; +} + +void TrackerLocation::onSleepState(TrackerSleepContext context) { + switch (context.reason) { + case TrackerSleepReason::STATE_TO_CONNECTING: { + Log.trace("%s starting GNSS", __FUNCTION__); + LocationService::instance().start(); + break; + } + + case TrackerSleepReason::STATE_TO_SHUTDOWN: { + Log.trace("%s stopping GNSS for shutdown", __FUNCTION__); + disableGnss(); + break; + } + } +} + +GnssState TrackerLocation::loopLocation(LocationPoint& cur_loc) { + static GnssState lastGnssState = GnssState::OFF; + GnssState currentGnssState = GnssState::ON_LOCKED_STABLE; + + LocationStatus locStatus; + LocationService::instance().getStatus(locStatus); + + do { + if (!locStatus.powered) { + currentGnssState = GnssState::OFF; + break; + } + + if (LocationService::instance().getLocation(cur_loc) != SYSTEM_ERROR_NONE) { - first_lock_triggered = (first_lock_triggered || cur_loc.locked != 0); + currentGnssState = GnssState::ERROR; + break; + } + + if (!cur_loc.locked) { + currentGnssState = GnssState::ON_UNLOCKED; + break; + } + + if (!cur_loc.stable) { + currentGnssState = GnssState::ON_LOCKED_UNSTABLE; + break; + } + + float radius; + LocationService::instance().getRadiusThreshold(radius); + if (radius) { + bool outside; + LocationService::instance().isOutsideRadius(outside, cur_loc); + if (outside) { + triggerLocPub(Trigger::NORMAL,"radius"); + } + } + } while (false); + + // Detect GNSS locked changes + if ((currentGnssState == GnssState::ON_LOCKED_STABLE) && + (currentGnssState != lastGnssState)) { + + // Capture the time that the first lock out of sleep happened + if (_firstLockSec == 0) { + _firstLockSec = System.uptime(); + } + + // Only publish with "lock" trigger when not sleeping and when enabled to do so + if (_sleep.isSleepDisabled() && config_state.lock_trigger) { triggerLocPub(Trigger::NORMAL,"lock"); } } - // - // Perform interval evaluation - // - bool pub_requested = false; - if(pending_immediate) + lastGnssState = currentGnssState; + + return currentGnssState; +} + +void TrackerLocation::buildPublish(LocationPoint& cur_loc) { + if(cur_loc.locked) { - // request for immediate publish overrides the default min/max interval checking - pub_requested = true; - pending_immediate = false; + LocationService::instance().setWayPoint(cur_loc.latitude, cur_loc.longitude); } - else if(config_state.interval_max_seconds && System.uptime() - last_location_publish_sec >= (uint32_t) config_state.interval_max_seconds) + + CloudService &cloud_service = CloudService::instance(); + cloud_service.beginCommand("loc"); + cloud_service.writer().name("loc").beginObject(); + if (cur_loc.locked) { - // max interval and past the max interval so have to publish - triggerLocPub(Trigger::NORMAL,"time"); - pub_requested = true; + cloud_service.writer().name("lck").value(1); + cloud_service.writer().name("time").value((unsigned int) cur_loc.epochTime); + cloud_service.writer().name("lat").value(cur_loc.latitude, 8); + cloud_service.writer().name("lon").value(cur_loc.longitude, 8); + if(!config_state.min_publish) + { + cloud_service.writer().name("alt").value(cur_loc.altitude, 3); + cloud_service.writer().name("hd").value(cur_loc.heading, 2); + cloud_service.writer().name("h_acc").value(cur_loc.horizontalAccuracy, 3); + cloud_service.writer().name("v_acc").value(cur_loc.verticalAccuracy, 3); + } } - else if(!config_state.interval_min_seconds || System.uptime() - last_location_publish_sec >= (uint32_t) config_state.interval_min_seconds) + else { - // no min interval or past the min interval so can publish - // check radius and imu triggers - if (pending_triggers.size()) - { - pub_requested = true; + cloud_service.writer().name("lck").value(0); + } + for(auto cb : locGenCallbacks) + { + cb(cloud_service.writer(), cur_loc); + } + cloud_service.writer().endObject(); + if(!_pending_triggers.isEmpty()) + { + std::lock_guard lg(mutex); + cloud_service.writer().name("trig").beginArray(); + for (auto trigger : _pending_triggers) { + cloud_service.writer().value(trigger); } + _pending_triggers.clear(); + cloud_service.writer().endArray(); } + Log.info("%.*s", cloud_service.writer().dataSize(), cloud_service.writer().buffer()); +} - // - // Perform publish of location data if requested - // +void TrackerLocation::loop() { + // The rest of this loop should only sample as fast as necessary + if (millis() - _loopSampleTick < LoopSampleRate) + { + return; + } + _loopSampleTick = millis(); - // first any retry attempts of last loc - if(location_publish_retry_str && Particle.connected()) + // First take care of any retry attempts of last loc + if (location_publish_retry_str && Particle.connected()) { + Log.info("retry failed publish"); location_publish(); } + // Gather current location information and status + LocationPoint cur_loc; + auto locationStatus = loopLocation(cur_loc); + + // Perform interval evaluation + auto publishReason = evaluatePublish(); + + // This evaluation may have performed earlier and determined that no network was needed. Check again + // because this loop may overlap with required network operations. + if (!_sleep.isFullWakeCycle() && publishReason.networkNeeded) { + enableNetworkGnss(); + } + + bool publishNow = false; + + // : NONE TIME TRIG IMM + // ---------------------------------------- + // GnssState::OFF NA PUB PUB PUB + // GnssState::ON_UNLOCKED NA WAIT WAIT PUB + // GnssState::ON_LOCKED_UNSTABLE NA WAIT WAIT PUB + // GnssState::ON_LOCKED_STABLE NA PUB PUB PUB + + switch (publishReason.reason) { + case PublishReason::NONE: { + // If there is nothing to do then get out + return; + } + + case PublishReason::TIME: { + switch (locationStatus) { + case GnssState::ON_LOCKED_STABLE: { + Log.trace("publishing from max interval"); + triggerLocPub(Trigger::NORMAL,"time"); + publishNow = true; + break; + } + + case GnssState::OFF: + // fall through + case GnssState::ON_UNLOCKED: + // fall through + case GnssState::ON_LOCKED_UNSTABLE: { + Log.trace("waiting for stable GNSS lock for max interval"); + break; + } + } + break; + } + + case PublishReason::TRIGGERS: { + switch (locationStatus) { + case GnssState::ON_LOCKED_STABLE: { + Log.trace("publishing from triggers"); + publishNow = true; + _newMonotonic = true; + break; + } + + case GnssState::OFF: + // fall through + case GnssState::ON_UNLOCKED: + // fall through + case GnssState::ON_LOCKED_UNSTABLE: { + Log.trace("waiting for stable GNSS lock for triggers"); + break; + } + } + break; + } + + case PublishReason::IMMEDIATE: { + Log.trace("publishing from immediate"); + _pending_immediate = false; + publishNow = true; + _newMonotonic = true; + break; + } + } + + // + // Perform publish of location data if requested + // + // then of any new publish - if(pub_requested) + if(publishNow) { if(location_publish_retry_str) { + Log.info("freeing unsuccessful retry"); // retried attempt not completed in time for new publish // drop and issue callbacks issue_location_publish_callbacks(CloudServiceStatus::TIMEOUT, NULL, location_publish_retry_str); free(location_publish_retry_str); location_publish_retry_str = nullptr; } - - last_location_publish_sec = System.uptime(); - last_loc_locked_pub = cur_loc.locked; - if(cur_loc.locked) + Log.info("publishing now..."); + buildPublish(cur_loc); + pendingLocPubCallbacks = locPubCallbacks; + locPubCallbacks.clear(); + _last_location_publish_sec = System.uptime(); + if (_first_publish || _newMonotonic) { - LocationService::instance().setWayPoint(cur_loc.latitude, cur_loc.longitude); - } - - CloudService &cloud_service = CloudService::instance(); - cloud_service.beginCommand("loc"); - cloud_service.writer().name("loc").beginObject(); - if (cur_loc.locked) - { - cloud_service.writer().name("lck").value(1); - cloud_service.writer().name("time").value((unsigned int) cur_loc.epochTime); - cloud_service.writer().name("lat").value(cur_loc.latitude, 8); - cloud_service.writer().name("lon").value(cur_loc.longitude, 8); - if(!config_state.min_publish) - { - cloud_service.writer().name("alt").value(cur_loc.altitude, 3); - cloud_service.writer().name("hd").value(cur_loc.heading, 2); - cloud_service.writer().name("h_acc").value(cur_loc.horizontalAccuracy, 3); - cloud_service.writer().name("v_acc").value(cur_loc.verticalAccuracy, 3); - } + _monotonic_publish_sec = _last_location_publish_sec; } else { - cloud_service.writer().name("lck").value(0); + _monotonic_publish_sec += (uint32_t)config_state.interval_max_seconds; } - for(auto cb : locGenCallbacks) - { - cb(cloud_service.writer(), cur_loc); - } - cloud_service.writer().endObject(); - if(!pending_triggers.isEmpty()) - { - std::lock_guard lg(mutex); - cloud_service.writer().name("trig").beginArray(); - for (auto trigger : pending_triggers) { - cloud_service.writer().value(trigger); - } - pending_triggers.clear(); - cloud_service.writer().endArray(); - } - Log.info("%.*s", cloud_service.writer().dataSize(), cloud_service.writer().buffer()); - pendingLocPubCallbacks = locPubCallbacks; - locPubCallbacks.clear(); location_publish(); } } diff --git a/src/tracker_location.h b/src/tracker_location.h index 2a61539..0cd1d3f 100644 --- a/src/tracker_location.h +++ b/src/tracker_location.h @@ -20,12 +20,21 @@ #include "cloud_service.h" #include "location_service.h" #include "motion_service.h" +#include "tracker_sleep.h" #define TRACKER_LOCATION_INTERVAL_MIN_DEFAULT_SEC (900) #define TRACKER_LOCATION_INTERVAL_MAX_DEFAULT_SEC (3600) #define TRACKER_LOCATION_MIN_PUBLISH_DEFAULT (false) #define TRACKER_LOCATION_LOCK_TRIGGER (true) +// wait at most this many seconds for a locked GPS location to become stable +// before publishing regardless +#define TRACKER_LOCATION_STABLE_WAIT_MAX (30) + +// wait at most this many seconds for initial lock on boot before publishing +// regardless +#define TRACKER_LOCATION_INITIAL_LOCK_MAX (90) + struct tracker_location_config_t { int32_t interval_min_seconds; // 0 = no min int32_t interval_max_seconds; // 0 = no max @@ -38,6 +47,26 @@ enum class Trigger { IMMEDIATE = 1, }; +enum class GnssState { + OFF, + ERROR, + ON_UNLOCKED, + ON_LOCKED_UNSTABLE, + ON_LOCKED_STABLE, +}; + +enum class PublishReason { + NONE, + TIME, + TRIGGERS, + IMMEDIATE, +}; + +struct EvaluationResults { + PublishReason reason; + bool networkNeeded; +}; + class TrackerLocation { public: @@ -74,7 +103,7 @@ class TrackerLocation // register for callback on location publish success/fail // these callbacks are NOT persistent and are used for the next publish int regLocPubCallback( - cloud_service_send_cb_t cb, + cloud_service_send_cb_t cb, const void *context=nullptr); template @@ -92,7 +121,16 @@ class TrackerLocation private: TrackerLocation() : - location_publish_retry_str(nullptr) + _sleep(TrackerSleep::instance()), + _loopSampleTick(0), + _pending_immediate(false), + _first_publish(true), + _earlyWake(0), + _nextEarlyWake(0), + location_publish_retry_str(nullptr), + _monotonic_publish_sec(0), + _newMonotonic(true), + _firstLockSec(0) { config_state = { .interval_min_seconds = TRACKER_LOCATION_INTERVAL_MIN_DEFAULT_SEC, @@ -102,11 +140,16 @@ class TrackerLocation }; } static TrackerLocation *_instance; + TrackerSleep& _sleep; - std::recursive_mutex mutex; + RecursiveMutex mutex; - Vector pending_triggers; - bool pending_immediate; + Vector _pending_triggers; + system_tick_t _loopSampleTick = 0; + bool _pending_immediate; + bool _first_publish; + unsigned int _earlyWake; + unsigned int _nextEarlyWake; char *location_publish_retry_str; @@ -121,7 +164,22 @@ class TrackerLocation void location_publish(); - uint32_t last_location_publish_sec; + bool isSleepEnabled(); + void enableNetworkGnss(); + void disableGnss(); + void onSleepPrepare(TrackerSleepContext context); + void onSleep(TrackerSleepContext context); + void onSleepCancel(TrackerSleepContext context); + void onWake(TrackerSleepContext context); + void onSleepState(TrackerSleepContext context); + EvaluationResults evaluatePublish(); + void buildPublish(LocationPoint& cur_loc); + GnssState loopLocation(LocationPoint& cur_loc); + + uint32_t _last_location_publish_sec; + uint32_t _monotonic_publish_sec; + bool _newMonotonic; + uint32_t _firstLockSec; tracker_location_config_t config_state, config_state_shadow; @@ -148,4 +206,4 @@ int TrackerLocation::regLocPubCallback( const void *context) { return regLocPubCallback(std::bind(cb, instance, _1, _2, _3), context); -} \ No newline at end of file +} diff --git a/src/tracker_motion.cpp b/src/tracker_motion.cpp index f5f5f28..5097bf1 100644 --- a/src/tracker_motion.cpp +++ b/src/tracker_motion.cpp @@ -16,6 +16,7 @@ #include "tracker_motion.h" #include "tracker_location.h" +#include "tracker_sleep.h" #include "config_service.h" #include "motion_service.h" @@ -30,9 +31,17 @@ static int get_motion_enabled_cb(int32_t &value, const void *context) static int set_motion_enabled_cb(int32_t value, const void *context) { - // TODO: What do we do if this fails to the underlying IMU? - // Should we keep retrying at this layer somehow? Return a failure? - static_cast((void *)context)->enableMotionDetection((MotionDetectionMode) value); + MotionService *motion_service = static_cast((void *)context); + + motion_service->enableMotionDetection((MotionDetectionMode) value); + if ((MotionDetectionMode)value == MotionDetectionMode::NONE) { + if (!motion_service->isAnyAwake()) { + TrackerSleep::instance().ignore((pin_t)BMI160_INT_PIN); + } + } + else { + TrackerSleep::instance().wakeFor((pin_t)BMI160_INT_PIN, BMI160_INT_MODE); + } return 0; } @@ -49,10 +58,14 @@ static int set_high_g_enabled_cb(int32_t value, const void *context) if(value == (int32_t) HighGDetectionMode::DISABLE) { motion_service->disableHighGDetection(); + if (!motion_service->isAnyAwake()) { + TrackerSleep::instance().ignore((pin_t)BMI160_INT_PIN); + } } else if(value == (int32_t) HighGDetectionMode::ENABLE) { motion_service->enableHighGDetection(); + TrackerSleep::instance().wakeFor((pin_t)BMI160_INT_PIN, BMI160_INT_MODE); } else { diff --git a/src/tracker_shipping.cpp b/src/tracker_shipping.cpp index afcf4c0..5f6fe9b 100644 --- a/src/tracker_shipping.cpp +++ b/src/tracker_shipping.cpp @@ -77,7 +77,10 @@ void TrackerShipping::shutdown() // sleep forever waiting for power to be removed // leave network on for quicker drain of residual power // once main power is removed - System.sleep(SLEEP_MODE_DEEP, 0, SLEEP_NETWORK_STANDBY); + SystemSleepConfiguration config; + config.mode(SystemSleepMode::HIBERNATE) + .gpio(PMIC_INT, FALLING); + System.sleep(config); // shouldn't hit these lines as never coming back from sleep but out of an // abundance of paranoia force a reset so we don't get stuck in some weird diff --git a/src/tracker_sleep.cpp b/src/tracker_sleep.cpp new file mode 100644 index 0000000..c9dfc54 --- /dev/null +++ b/src/tracker_sleep.cpp @@ -0,0 +1,598 @@ +/* + * Copyright (c) 2020 Particle Industries, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "tracker_sleep.h" +#include "config_service.h" +#include "tracker_location.h" +#include "tracker.h" + +// Private constants +constexpr system_tick_t TrackerSleepMinSleepDuration = 1000; // milliseconds + +TrackerSleep *TrackerSleep::_instance = nullptr; + +void TrackerSleep::handleOta(system_event_t event, int param) { + TrackerSleep::instance().pauseSleep(); +} + +int TrackerSleep::init(SleepWatchdogCallback watchdog) { + static ConfigObject sleepDesc + ( + "sleep", + { + ConfigStringEnum( + "mode", + { + {"disable", (int32_t) TrackerSleepMode::Disable}, + {"enable", (int32_t) TrackerSleepMode::Enable}, + }, + &_config_state.mode + ), + ConfigInt("exe_min", &_config_state.execute_min_seconds, TrackerSleepDefaultExeMinTime, TrackerSleepDefaultMaxTime), + ConfigInt("conn_max", &_config_state.connecting_max_seconds, TrackerSleepDefaultConnMaxTime, TrackerSleepDefaultMaxTime) + } + ); + + _watchdog = watchdog; + + ConfigService::instance().registerModule(sleepDesc); + + // Associate OTA handler to pause sleep + System.on(firmware_update+firmware_update_pending, handleOta); + + // Register callback to be alerted when there is a publish + TrackerLocation::instance().regLocGenCallback([this](JSONWriter& writer, LocationPoint &loc, const void *context){annoucePublish();}); + + return SYSTEM_ERROR_NONE; +} + +TrackerSleepError TrackerSleep::updateNextWake(uint64_t milliseconds) { + // A input value of 0 means that the requestor wants to cancel the current sleep cycle, pass through + // the sleep state and re-enter the execution phase + if (milliseconds == 0) { + _nextWakeMs = 0; + return TrackerSleepError::NONE; + } + + // This function performs a basic priority scheduler calculation based on the next scheduled wake + // time versus the requested value from the caller. Any action for sleep evaluation is performed in + // the future so the only comparison to present time would be the requested value. The next wake + // time is compared to the future present time later on. + uint64_t now = System.millis(); + + // Nothing from the past makes sense + if (milliseconds <= now) { + return TrackerSleepError::TIME_IN_PAST; + } + // Anything evaluated past this point assumes the requested wake time is in the future + + // We want to capture the very first wake request after waking or a subsequent time that is + // sooner than one already established. + if (_nextWakeMs == 0) { + _nextWakeMs = milliseconds; + } + else { + if (milliseconds > _nextWakeMs) { + return TrackerSleepError::TIME_SKIPPED; + } + else { + _nextWakeMs = milliseconds; + } + } + + return TrackerSleepError::NONE; +} + +TrackerSleepError TrackerSleep::wakeAtSeconds(unsigned int uptimeSeconds) { + return updateNextWake((uint64_t)uptimeSeconds * 1000); +} + +TrackerSleepError TrackerSleep::wakeAtMilliseconds(system_tick_t milliseconds) { + return updateNextWake((uint64_t)milliseconds); +} + +TrackerSleepError TrackerSleep::wakeAtMilliseconds(uint64_t milliseconds) { + return updateNextWake(milliseconds); +} + +TrackerSleepError TrackerSleep::wakeAt(std::chrono::milliseconds ms) { + return updateNextWake((uint64_t)ms.count()); +} + +int TrackerSleep::wakeFor(pin_t pin, InterruptMode mode) { + // Search through existing wake pins and update mode if already existing + for (auto item : _onPin) { + if (item.first == pin) { + item.second = mode; + return SYSTEM_ERROR_NONE; + } + } + + _onPin.append(std::make_pair(pin, mode)); + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::wakeFor(SystemSleepFlag flag) { + _onFlag.append(flag); + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::wakeFor(network_interface_t netif) { + if (netif != NETWORK_INTERFACE_CELLULAR) { + return SYSTEM_ERROR_NOT_SUPPORTED; + } + + _onNetwork = true; + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::wakeForBle() { + _onBle = true; + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::ignore(pin_t pin) { + int index = 0; + + // Search through list and remove (all) instances of this pin + for (auto item : _onPin) { + if (item.first == pin) { + _onPin.removeAt(index); + } + index++; + } + + return (index == _onPin.size()) ? SYSTEM_ERROR_NOT_FOUND : SYSTEM_ERROR_NONE; +} + +int TrackerSleep::ignore(SystemSleepFlag flag) { + if (_onFlag.removeAll(flag) == 0) { + return SYSTEM_ERROR_NOT_FOUND; + } + + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::ignore(network_interface_t netif) { + if (netif != NETWORK_INTERFACE_CELLULAR) { + return SYSTEM_ERROR_NOT_SUPPORTED; + } + + _onNetwork = false; + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::ignoreBle() { + _onBle = false; + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::registerSleepPrepare(SleepCallback callback) { + _onSleepPrepare.append(callback); + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::registerSleepCancel(SleepCallback callback) { + _onSleepCancel.append(callback); + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::registerSleep(SleepCallback callback) { + _onSleep.append(callback); + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::registerWake(SleepCallback callback) { + _onWake.append(callback); + return SYSTEM_ERROR_NONE; +} + +int TrackerSleep::registerStateChange(SleepCallback callback) { + _onStateTransition.append(callback); + return SYSTEM_ERROR_NONE; +} + +void TrackerSleep::startModem() { + Log.info("Starting modem"); + _lastModemOnMs = System.millis(); + Particle.connect(); + _inFullWakeup = true; +} + +void TrackerSleep::stopModem() { + Log.info("Stoping modem"); + // Explicitly disconnect from the cloud with graceful offline status message + Particle.disconnect(CloudDisconnectOptions().graceful(true).timeout(TrackerSleepGracefulTimeout)); + waitUntil(Particle.disconnected); + Cellular.disconnect(); + waitUntilNot(Cellular.ready); + _inFullWakeup = false; +} + +TrackerSleepResult TrackerSleep::sleep() { + TrackerSleepResult retval; + + SystemSleepConfiguration config; + + // Prepare to call all of the registered sleep prep callbacks with the same message + TrackerSleepContext sleepContext = { + .reason = TrackerSleepReason::PREPARE_SLEEP, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + // Full wakeup is requested only after this point + _fullWakeupOverride = false; + + for (auto callback : _onSleepPrepare) { + callback(sleepContext); + } + + // We need to calculate the sleep duration based on the absolute uptime in milliseconds and how much time we need + // to wake beforehand to power on the cellular modem and GNSS module. + uint64_t now = System.millis(); + system_tick_t duration = (system_tick_t)(_nextWakeMs - now); + + // Don't sleep if too short of duration + bool cancelSleep = false; + if (_nextWakeMs == 0) { + Log.trace("cancelled sleep because of missing wake time"); + cancelSleep = true; + } + if (_nextWakeMs < now) { + Log.trace("cancelled sleep at %lu milliseconds because it is in the past", (uint32_t)_nextWakeMs); + cancelSleep = true; + } + + if (cancelSleep) { + // It is not worth sleeping + TrackerSleepContext sleepCancelContext = { + .reason = TrackerSleepReason::CANCEL_SLEEP, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onSleepCancel) { + callback(sleepCancelContext); + } + + // The next wake time is now invalid and should be treated uninitialized + _nextWakeMs = 0; + retval.error = TrackerSleepError::CANCELLED; + return retval; + } + + config.mode(SystemSleepMode::ULTRA_LOW_POWER) + .gpio(PMIC_INT, FALLING); // Always detect power events + + // Accumulate all of the pin sources for wake + for (auto pin : _onPin) { + config.gpio(pin.first, pin.second); + } + + if (_onNetwork) { + config.network(NETWORK_INTERFACE_CELLULAR); + } + + if (_onBle) { + config.ble(); + } + + // Disable watchdog + if (_watchdog) { + _watchdog(false); + } + + stopModem(); + + TrackerSleepContext sleepNowContext = { + .reason = TrackerSleepReason::SLEEP, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onSleep) { + callback(sleepNowContext); + } + + // Perform the actual System sleep now + // Capture time that sleep was entered + _lastSleepMs = System.millis(); + + // Re-evaluate the duration because handlers and preparation may have taken away time + duration = (system_tick_t)(_nextWakeMs - _lastSleepMs); + if (_lastSleepMs >= _nextWakeMs) { + duration = TrackerSleepMinSleepDuration; // Sleep for at least 1 second + } + config.duration(duration); + + _lastRequestedWakeMs = _lastSleepMs + duration; + Log.info("sleeping until %lu milliseconds", (uint32_t)_lastRequestedWakeMs); + + retval.result = System.sleep(config); + // Capture the wake time to help calculate the next sleep cycle + _lastWakeMs = System.millis(); + + _executeDurationSec = (uint32_t)_config_state.execute_min_seconds; + + // Enable watchdog + if (_watchdog) { + _watchdog(true); + } + + // Our loop count restarts to indicate that we are executing out of sleep + _loopCount = 0; + _nextWakeMs = 0; + _inFullWakeup = false; + + // Call all registered callbacks for wake and provide a common context + TrackerSleepContext wakeContext = { + .reason = TrackerSleepReason::WAKE, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onWake) { + callback(wakeContext); + } + + retval.error = TrackerSleepError::NONE; + return retval; +} + +void TrackerSleep::stateToConnecting() { + _fullWakeupOverride = false; + _executionState = TrackerExecutionState::CONNECTING; + _lastConnectingSec = System.uptime(); + _publishFlag = false; + + startModem(); + + TrackerSleepContext stateContext = { + .reason = TrackerSleepReason::STATE_TO_CONNECTING, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onStateTransition) { + callback(stateContext); + } +} + +void TrackerSleep::stateToExecute() { + _executionState = TrackerExecutionState::EXECUTION; + _lastExecuteSec = System.uptime(); + + TrackerSleepContext stateContext = { + .reason = TrackerSleepReason::STATE_TO_EXECUTION, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onStateTransition) { + callback(stateContext); + } +} + +void TrackerSleep::stateToSleep() { + _executionState = TrackerExecutionState::SLEEP; + + TrackerSleepContext stateContext = { + .reason = TrackerSleepReason::STATE_TO_SLEEP, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = _nextWakeMs, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onStateTransition) { + callback(stateContext); + } +} + +void TrackerSleep::stateToShutdown() { + _executionState = TrackerExecutionState::SHUTDOWN; + + TrackerSleepContext stateContext = { + .reason = TrackerSleepReason::STATE_TO_SHUTDOWN, + .loop = _loopCount, + .lastSleepMs = _lastSleepMs, + .lastWakeMs = _lastWakeMs, + .nextWakeMs = 0, + .modemOnMs = _lastModemOnMs, + }; + + for (auto callback : _onStateTransition) { + callback(stateContext); + } + + _lastShutdownMs = millis(); +} + +int TrackerSleep::loop() { + + // Perform state operations and transitions + switch (_executionState) { + /* ---------------------------------------------------------------------------------------------------------------- + * BOOT state + * This state is only entered upon power on and can only transition to the CONNECTING state + *----------------------------------------------------------------------------------------------------------------- + */ + case TrackerExecutionState::BOOT: { + _lastWakeMs = System.millis(); + _loopCount = 0; + _executeDurationSec = (uint32_t)_config_state.execute_min_seconds; + stateToConnecting(); + break; + } + + + /* ---------------------------------------------------------------------------------------------------------------- + * CONNECTING state + * This state is entered from all other states and can only transition to the EXECUTION state. + * + * The purpose of this state is to wait for a Particle connection and publish from the location service. If + * that doesn't happen then transition immediately to the EXECUTE state based on a configurable timeout. Waiting + * for a valid connection state during poor cellular reception would otherwise cause the system to wait + * indefinitely and run down battery charge. + *----------------------------------------------------------------------------------------------------------------- + */ + case TrackerExecutionState::CONNECTING: { + if (_pendingPublishVitals && Particle.connected()) { + _pendingPublishVitals = false; + Particle.publishVitals(); + } + if (_publishFlag && Particle.connected()) { + _publishFlag = false; + Log.trace("published and transitioning to EXECUTE"); + stateToExecute(); + } + else if (System.uptime() - _lastConnectingSec >= (uint32_t)_config_state.connecting_max_seconds) { + TrackerLocation::instance().triggerLocPub(Trigger::IMMEDIATE, "imm"); + Log.trace("publishing timed out and transitioning to EXECUTE"); + stateToExecute(); + } + + break; + } + + + /* ---------------------------------------------------------------------------------------------------------------- + * EXECUTION state + * This state is entered from all other states and can only transition to the EXECUTION state. + * + * The purpose of this state is to wait for a Particle connection and publish from the location service. If + * that doesn't happen then transition immediately to the EXECUTE state based on a configurable timeout. Waiting + * for a valid connection state during poor cellular reception would otherwise cause the system to wait + * indefinitely and run down battery charge. + *----------------------------------------------------------------------------------------------------------------- + */ + case TrackerExecutionState::EXECUTION: { + // Execution depends on whether we are sleep enabled + if (!isSleepDisabled()) { + if (!_inFullWakeup && (_pendingPublishVitals || _pendingShutdown)) { + _fullWakeupOverride = true; + } + + // Check immediately if a full wake was requested and enter connecting state + if (!_inFullWakeup && _fullWakeupOverride) { + Log.trace("full wakeup requested, connecting"); + stateToConnecting(); + break; + } + + if (_pendingShutdown) { + Log.trace("EXECUTE time expired and transitioning to SHUTDOWN"); + stateToShutdown(); + } + + if (!_holdSleep && + (System.uptime() - _lastExecuteSec >= _executeDurationSec)) { + + Log.trace("EXECUTE time expired and transitioning to SLEEP"); + stateToSleep(); + } + } + else { + // Nothing specific to do + _lastExecuteSec = System.uptime(); + + if (_pendingPublishVitals && Particle.connected()) { + _pendingPublishVitals = false; + Particle.publishVitals(); + } + + if (_pendingShutdown) { + stateToShutdown(); + } + } + + break; + } + + /* ---------------------------------------------------------------------------------------------------------------- + * SLEEP state + * This state is only entered from the EXECUTE state and can only transition to EXECUTION and CONNECTING states. + * + * The purpose of this state is to enter sleep and decide what to do after waking. + *----------------------------------------------------------------------------------------------------------------- + */ + case TrackerExecutionState::SLEEP: { + // Perform actual sleep here + auto result = sleep(); + + // There was a problem going to sleep so transition back to EXECUTE and re-evaluate + if (result.error == TrackerSleepError::CANCELLED) { + Log.trace("cancelled and executing"); + stateToExecute(); + } + else if (_fullWakeupOverride) { + Log.trace("woke and connecting"); + stateToConnecting(); + } + else { + Log.trace("woke and executing without connection"); + stateToExecute(); + } + break; + } + + /* ---------------------------------------------------------------------------------------------------------------- + * SHUTDOWN state + * This state is only entered from the EXECUTE state and can only transition to SHUTDOWN states. + * + * The purpose of this state is to start shipping mode and wait for it to happen. + *----------------------------------------------------------------------------------------------------------------- + */ + case TrackerExecutionState::SHUTDOWN: { + if (_pendingPublishVitals && Particle.connected()) { + _pendingPublishVitals = false; + Particle.publishVitals(); + } + if ((_publishFlag && Particle.connected()) || + (millis() - _lastShutdownMs >= TrackerSleepShutdownTimeout)) { + // Stop everything + stopModem(); + TrackerShipping::instance().enter(true); + while (true) {} + } + break; + } + } + + _loopCount++; + + return SYSTEM_ERROR_NONE; +} diff --git a/src/tracker_sleep.h b/src/tracker_sleep.h new file mode 100644 index 0000000..c2c7299 --- /dev/null +++ b/src/tracker_sleep.h @@ -0,0 +1,585 @@ +/* + * Copyright (c) 2020 Particle Industries, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include "Particle.h" +#include "tracker_config.h" + + +/** + * @brief Definition of sleep configurations. + * Sleep can be disabled or enabled in one or more different modes. + * + */ +enum class TrackerSleepMode { + Disable, /**< Sleep is disabled */ + Enable, /**< Sleep is enabled */ +}; + +// Default configurations for Sleep +constexpr TrackerSleepMode TrackerSleepDefaultMode = TrackerSleepMode::Disable; +constexpr int32_t TrackerSleepDefaultExeMinTime = 10; // seconds +constexpr int32_t TrackerSleepDefaultConnMaxTime = 90; // seconds +constexpr int32_t TrackerSleepDefaultMaxTime = 86400; // seconds +constexpr system_tick_t TrackerSleepGracefulTimeout = 5 * 1000; // milliseconds +constexpr system_tick_t TrackerSleepShutdownTimeout = 4 * 1000; // milliseconds + +struct tracker_sleep_config_t { + TrackerSleepMode mode; + int32_t execute_min_seconds; + int32_t connecting_max_seconds; +}; + +/** + * @brief Return types for time related sleep function calls. + * + */ +enum class TrackerSleepError { + NONE, /**< No errors */ + TIME_IN_PAST, /**< Time given is in the past */ + TIME_SKIPPED, /**< Time given was evaluated and not needed */ + CANCELLED, /**< Operation was cancelled */ +}; + +/** + * @brief Reason for callback context. + * + */ +enum class TrackerSleepReason { + PREPARE_SLEEP, /**< The system is preparing to sleep */ + CANCEL_SLEEP, /**< The system canceled sleep */ + SLEEP, /**< The system is going to sleep */ + WAKE, /**< The system woke from sleep */ + STATE_TO_CONNECTING, /**< Sleep transition to CONNECTING */ + STATE_TO_EXECUTION, /**< Sleep transition to EXECUTION */ + STATE_TO_SLEEP, /**< Sleep transition to SLEEP */ + STATE_TO_SHUTDOWN, /**< Sleep transition to SHUTDOWN */ +}; + +/** + * @brief Return structure for Tracker sleep functions. + * + */ +struct TrackerSleepResult { + SystemSleepResult result; /**< System sleep result */ + TrackerSleepError error; /**< Tracker sleep internal error result */ +}; + +/** + * @brief Structure to contain information related to sleep and wake. + * + */ +struct TrackerSleepContext { + TrackerSleepReason reason; /**< Enumerated reason for the call */ + size_t loop; /**< Loop number call made */ + uint64_t lastSleepMs; /**< The last time, in milliseconds, the system went to sleep */ + uint64_t lastWakeMs; /**< The last time, in milliseconds, the system woke from sleep */ + uint64_t nextWakeMs; /**< The next time, in milliseconds, the system will wake from sleep */ + uint64_t modemOnMs; /**< The time, in milliseconds, when the modem was turned on */ +}; + +/** + * @brief Type definition of sleep watchdog callbacks. + * + */ +using SleepWatchdogCallback = std::function; + +/** + * @brief Type definition of sleep callback signature. + * + */ +using SleepCallback = std::function; + +/** + * @brief Execution states for sleep + * + */ +enum class TrackerExecutionState { + BOOT, + CONNECTING, + EXECUTION, + SLEEP, + SHUTDOWN, +}; + +/** + * @brief TrackerSleep class to configure and manage sleep. + * + */ +class TrackerSleep { +public: + /** + * @brief Singleton class instance access for TrackerSleep. + * + * @return TrackerSleep& + */ + static TrackerSleep& instance() { + if (!_instance) { + _instance = new TrackerSleep(); + } + return *_instance; + } + + /** + * @brief Initialize TrackerSleep. + * + * @retval SYSTEM_ERROR_NONE + */ + int init(SleepWatchdogCallback watchdog = nullptr); + + /** + * @brief Get the config mode for sleep + * + * @return TrackerSleepMode Enumeration for sleep configuration mode + */ + TrackerSleepMode getConfigMode() { + return _config_state.mode; + } + + /** + * @brief Indicate that sleep is disabled + * + * @return true Sleep is disabled + * @return false Sleep is enabled (in some form) + */ + bool isSleepDisabled() { + return (_config_state.mode == TrackerSleepMode::Disable); + } + + /** + * @brief Get the config execution time limit + * + * @return int32_t Execution time limit in seconds + */ + int32_t getConfigExecuteTime() { + return _config_state.execute_min_seconds; + } + + /** + * @brief Get the config connecting limit time + * + * @return int32_t Connecting time limit in seconds + */ + int32_t getConfigConnectingTime() { + return _config_state.connecting_max_seconds; + } + + /** + * @brief Schedules system wake at specific time in relation to System.uptime(). + * + * @param uptimeSeconds Absolute time, in seconds. Another, sooner pending wake + * time may take precidence. + * @retval TrackerSleepError::NONE Time was scheduled + * @retval TrackerSleepError::TIME_IN_PAST Given time happened in the past + * @retval TrackerSleepError::TIME_SKIPPED Given time happens later than a sooner wake request + */ + TrackerSleepError wakeAtSeconds(unsigned int uptimeSeconds); + + /** + * @brief Schedules system wake at specific time in relation to millis(). + * + * @param milliseconds Absolute time, in milliseconds. Another, sooner pending wake + * time may take precidence. + * @retval TrackerSleepError::NONE Time was scheduled + * @retval TrackerSleepError::TIME_IN_PAST Given time happened in the past + * @retval TrackerSleepError::TIME_SKIPPED Given time happens later than a sooner wake request + */ + TrackerSleepError wakeAtMilliseconds(system_tick_t milliseconds); + + /** + * @brief Schedules system wake at specific time in relation to System.millis(). + * + * @param milliseconds Absolute time, in milliseconds. Another, sooner pending wake + * time may take precidence. + * @retval TrackerSleepError::NONE Time was scheduled + * @retval TrackerSleepError::TIME_IN_PAST Given time happened in the past + * @retval TrackerSleepError::TIME_SKIPPED Given time happens later than a sooner wake request + */ + TrackerSleepError wakeAtMilliseconds(uint64_t milliseconds); + + /** + * @brief Schedules system wake at specific time in relation to System.millis(). + * + * @param milliseconds Absolute time, in std::chrono. Another, sooner pending wake + * time may take precidence. + * @retval TrackerSleepError::NONE Time was scheduled + * @retval TrackerSleepError::TIME_IN_PAST Given time happened in the past + * @retval TrackerSleepError::TIME_SKIPPED Given time happens later than a sooner wake request + */ + TrackerSleepError wakeAt(std::chrono::milliseconds ms); + + /** + * @brief Enables system wake for a pin change. + * + * @param pin System pin to associate to wake. + * @param mode System pin mode to associate to wake. + * @retval SYSTEM_ERROR_NONE + */ + int wakeFor(pin_t pin, InterruptMode mode); + + /** + * @brief Enables system wake for a SystemSleepFlag change. + * + * @param flag SystemSleepFlag event to associate to wake. + * @retval SYSTEM_ERROR_NONE + */ + int wakeFor(SystemSleepFlag flag); + + /** + * @brief Enables system wake for a System network interface change. + * + * @param netif Network interface event to associate to wake. + * @retval SYSTEM_ERROR_NONE + */ + int wakeFor(network_interface_t netif); + + /** + * @brief Enables system wake for a System BLE change. + * + * @retval SYSTEM_ERROR_NONE + */ + int wakeForBle(); + + /** + * @brief Disables system wake for a pin change. + * + * @param pin System pin to disassociate to wake. + * @retval SYSTEM_ERROR_NONE + */ + int ignore(pin_t pin); + + /** + * @brief Disables system wake for a SystemSleepFlag change. + * + * @param flag SystemSleepFlag event to disassociate to wake. + * @retval SYSTEM_ERROR_NONE + */ + int ignore(SystemSleepFlag flag); + + /** + * @brief Disables system wake for a System network interface change. + * + * @param netif Network interface event to disassociate to wake. + * @retval SYSTEM_ERROR_NONE + */ + int ignore(network_interface_t netif); + + /** + * @brief Disables system wake for a System BLE change. + * + * @retval SYSTEM_ERROR_NONE + */ + int ignoreBle(); + + /** + * @brief Prevent the system from going to sleep. + * + */ + void pauseSleep(){ + _holdSleep = true; + } + + /** + * @brief Allow the system to go to sleep. + * + */ + void resumeSleep() { + _holdSleep = false; + } + + /** + * @brief Extend execution time accumulatively before going back to sleep. + * + * @param seconds Number of seconds to extend the execution phase. + * @return uint32_t The new extended execution time. + */ + uint32_t extendExecution(uint32_t seconds) { + return _executeDurationSec += seconds; + } + + /** + * @brief Extend execution time from present time before going back to sleep. If the + * requested time period is less than the current execution time then override. + * + * @param seconds Number of seconds to extend the execution phase. + * @param force Force the new extended execution time even if it occurs sooner. + * @return uint32_t The new extended execution time. + */ + uint32_t extendExecutionFromNow(uint32_t seconds, bool force = false) { + auto now = System.uptime(); + auto expectedTime = _lastExecuteSec + _executeDurationSec; + auto newTime = now + seconds; + if (force || (newTime > expectedTime)) { + _lastExecuteSec = now; + _executeDurationSec = seconds; + Log.trace("extending execution"); + } + return _executeDurationSec; + } + + /** + * @brief Register a callback to be called while preparing for sleep. + * + * @param callback Function to call on sleep preparation + * @retval SYSTEM_ERROR_NONE + */ + int registerSleepPrepare(SleepCallback callback); + + /** + * @brief Register a callback to be called immediately after cancelling sleep. + * + * @param callback Function to call on sleep cancellation + * @retval SYSTEM_ERROR_NONE + */ + int registerSleepCancel(SleepCallback callback); + + /** + * @brief Register a callback to be called immediately prior to going to sleep. + * + * @param callback Function to call on sleep + * @retval SYSTEM_ERROR_NONE + */ + int registerSleep(SleepCallback callback); + + /** + * @brief Register a callback to be called immediately after returning sleep. + * + * @param callback Function to call on sleep completion + * @retval SYSTEM_ERROR_NONE + */ + int registerWake(SleepCallback callback); + + /** + * @brief Register a callback to be called immediately after sleep state change. + * + * @param callback Function to call on sleep state change + * @retval SYSTEM_ERROR_NONE + */ + int registerStateChange(SleepCallback callback); + + /** + * @brief Indicate that the current connecting/execution phase has cellular modem and GNSS powered. + * + * @return true Cellular modem and GNSS are powered during this execution cycle + * @return false Cellular modem and GNSS are not powered during this execution cycle + */ + bool isFullWakeCycle() { + return ( + isSleepDisabled() || + _inFullWakeup + ); + } + + /** + * @brief Instruct the TrackerSleep class to power on the cellular modem and GNSS in short wake cycles. + * + * @retval SYSTEM_ERROR_NONE + */ + int forceFullWakeCycle() { + if (!_inFullWakeup) { + _fullWakeupOverride = true; + } + return SYSTEM_ERROR_NONE; + } + + /** + * @brief Instruct TrackerSleep to power down the system after the current execution cycle. + * + */ + void forceShutdown() { + _fullWakeupOverride = true; + _pendingShutdown = true; + } + + /** + * @brief Indicate whether TrackerSleep is preparing for a pending shutdown request. + * + * @return true Shutdown is pending. + * @return false Shutdown is not pending. + */ + bool isForcedShutdownPending() { + return _pendingShutdown; + } + + /** + * @brief Instruct TrackerSleep to publish a vitals message before going to sleep. + * + */ + void forcePublishVitals() { + _pendingPublishVitals = true; + } + + /** + * @brief Main execution loop for the TrackerSleep class. This must be executed within every system loop. + * + * @retval SYSTEM_ERROR_NONE + */ + int loop(); + +private: + /** + * @brief Construct a new TrackerSleep singleton object + * + */ + TrackerSleep() : + _onNetwork(false), + _onBle(false), + _wakeupReason(SystemSleepWakeupReason::UNKNOWN), + _fullWakeupOverride(false), + _inFullWakeup(true), + _holdSleep(false), + _pendingPublishVitals(false), + _pendingShutdown(false), + _executionState(TrackerExecutionState::BOOT), + _lastConnectingSec(0), + _lastExecuteSec(0), + _executeDurationSec(0), + _nextWakeMs(0), + _lastWakeMs(0), + _lastRequestedWakeMs(0), + _lastSleepMs(0), + _lastModemOnMs(0), + _lastNetworkConnectMs(0), + _lastCloudConnectMs(0), + _loopCount(0), + _publishFlag(false) + + { + + _config_state = { + .mode = TrackerSleepDefaultMode, + .execute_min_seconds = TrackerSleepDefaultExeMinTime, + .connecting_max_seconds = TrackerSleepDefaultConnMaxTime, + }; + } + + /** + * @brief Instruct TrackerSleep class that a publish occured. + * + */ + void annoucePublish() { + _publishFlag = true; + extendExecutionFromNow(_config_state.execute_min_seconds); + } + + /** + * @brief System callback to handle OTA events that will disable sleep. + * + * @param event + * @param param + */ + static void handleOta(system_event_t event, int param); + + /** + * @brief Schedules system wake at specific time in relation to System.millis(). + * + * @param milliseconds Absolute time, in milliseconds. Another, sooner pending wake + * time may take precidence. + * @retval TrackerSleepError::NONE Time was scheduled + * @retval TrackerSleepError::TIME_IN_PAST Given time happened in the past + * @retval TrackerSleepError::TIME_SKIPPED Given time happens later than a sooner wake request + */ + TrackerSleepError updateNextWake(uint64_t milliseconds); + + /** + * @brief Prepare for sleep and wakeup. + * + * @return TrackerSleepResult + */ + TrackerSleepResult sleep(); + + /** + * @brief Transition to CONNECTING state + * + */ + void stateToConnecting(); + + /** + * @brief Transition to EXECUTE state + * + */ + void stateToExecute(); + + /** + * @brief Transition to SLEEP state + * + */ + void stateToSleep(); + + /** + * @brief Transition to SHUTDOWN state + * + */ + void stateToShutdown(); + + /** + * @brief Power the cellular modem on + * + */ + void startModem(); + + /** + * @brief Power the cellular modem off + * + */ + void stopModem(); + + // Singleton instance + static TrackerSleep* _instance; + + // Callback to enable/disable watchdog + SleepWatchdogCallback _watchdog; + + // Callback containers for sleep and wake + Vector _onSleepPrepare; + Vector _onSleepCancel; + Vector _onSleep; + Vector _onWake; + Vector _onStateTransition; + + // Sleep conditions + Vector> _onPin; + Vector _onFlag; + bool _onNetwork; + bool _onBle; + + // Cloud configuration for TrackerSleep + tracker_sleep_config_t _config_state; + + // Stored wakeup reason + SystemSleepWakeupReason _wakeupReason; + + bool _fullWakeupOverride; + bool _inFullWakeup; + bool _holdSleep; + bool _pendingPublishVitals; + bool _pendingShutdown; + TrackerExecutionState _executionState; + uint32_t _lastConnectingSec; + uint32_t _lastExecuteSec; + uint32_t _executeDurationSec; + system_tick_t _lastShutdownMs; + uint64_t _nextWakeMs; + uint64_t _lastWakeMs; + uint64_t _lastRequestedWakeMs; + uint64_t _lastSleepMs; + uint64_t _lastModemOnMs; + uint64_t _lastNetworkConnectMs; + uint64_t _lastCloudConnectMs; + size_t _loopCount; + bool _publishFlag; +};