From bcf1fd7bcc69afd659102e2fbc0d854081589014 Mon Sep 17 00:00:00 2001 From: Pavel Slama Date: Mon, 22 Feb 2021 22:07:13 +0100 Subject: [PATCH] low power --- README.md | 3 +- UbxGpsI2C.cpp | 175 ++++++++++++++++++++++++-------------------------- UbxGpsI2C.h | 8 +++ 3 files changed, 95 insertions(+), 91 deletions(-) diff --git a/README.md b/README.md index ea2cb32..b66fcbf 100644 --- a/README.md +++ b/README.md @@ -241,7 +241,8 @@ int main() { }, "target_overrides": { "*": { - "mbed-trace.enable": true + "mbed-trace.enable": true, + "ubxgpsi2c.debug": true } } } diff --git a/UbxGpsI2C.cpp b/UbxGpsI2C.cpp index 43d5ea7..b8f3e7b 100644 --- a/UbxGpsI2C.cpp +++ b/UbxGpsI2C.cpp @@ -405,6 +405,32 @@ void UbxGpsI2C::search_data() { } } +bool UbxGpsI2C::get_cfg(char id) { + bool ok = send(UBX_CFG, id); + + if (ok) { + // register our packet before polling + oob(UBX_CFG, id, callback(this, &UbxGpsI2C::cfg_cb)); + + if (poll()) { + tr_debug("Waiting for CFG packet"); + uint32_t cfg = _flags.wait_all(UBX_FLAGS_CFG | UBX_FLAGS_SEARCH_DONE, MBED_CONF_UBXGPSI2C_TIMEOUT); + + if (cfg & UBX_FLAGS_ERROR) { + tr_error("CFG timeout"); + ok = false; + } + } + + remove_oob(UBX_CFG, id); + + } else { + ok = false; + } + + return ok; +} + void UbxGpsI2C::oob(UbxClassId class_id, char id, Callback cb) { // In case we have it already in, remove it remove_oob(class_id, id); @@ -570,44 +596,25 @@ bool UbxGpsI2C::set_output_rate(milliseconds ms, uint16_t cycles) { return false; } - bool ok = send(UBX_CFG, UBX_CFG_RATE); + bool ok = get_cfg(UBX_CFG_RATE); if (ok) { - // register our packet before polling - oob(UBX_CFG, UBX_CFG_RATE, callback(this, &UbxGpsI2C::cfg_cb)); - - if (poll()) { - tr_debug("Waiting for CFG RATE packet"); - uint32_t cfg = _flags.wait_all(UBX_FLAGS_CFG | UBX_FLAGS_SEARCH_DONE, MBED_CONF_UBXGPSI2C_TIMEOUT); - - if (!(cfg & UBX_FLAGS_ERROR)) { - tr_debug("CFG RATE packet received"); - - memcpy(&cfg_rate, _buf, sizeof(cfg_rate_t)); // _buf contains only payload + tr_debug("CFG-RATE packet received"); - cfg_rate.measRate = duration_cast(ms).count(); - cfg_rate.navRate = cycles; - cfg_rate.timeRef = 0; // UTC + memcpy(&cfg_rate, _buf, sizeof(cfg_rate_t)); // _buf contains only payload - memcpy(_buf, &cfg_rate, sizeof(cfg_rate_t)); + cfg_rate.measRate = duration_cast(ms).count(); + cfg_rate.navRate = cycles; + cfg_rate.timeRef = 0; // UTC - if (send_ack(UBX_CFG, UBX_CFG_RATE, _buf, sizeof(cfg_rate_t))) { - tr_info("New output rate: %llims", duration_cast(ms).count() * cycles); + memcpy(_buf, &cfg_rate, sizeof(cfg_rate_t)); - } else { - ok = false; - } - - } else { - tr_error("CFG RATE timeout"); - ok = false; - } + if (send_ack(UBX_CFG, UBX_CFG_RATE, _buf, sizeof(cfg_rate_t))) { + tr_info("New output rate: %llims", duration_cast(ms).count() * cycles); } else { ok = false; } - - remove_oob(UBX_CFG, UBX_CFG_RATE); } return ok; @@ -617,59 +624,40 @@ bool UbxGpsI2C::set_odometer(bool enable, UbxOdoProfile profile, uint8_t velocit odometer_t odo; tr_debug("Setting odometer"); - bool ok = send(UBX_CFG, UBX_CFG_ODO); + bool ok = get_cfg(UBX_CFG_ODO); if (ok) { - // register our packet before polling - oob(UBX_CFG, UBX_CFG_ODO, callback(this, &UbxGpsI2C::cfg_cb)); - - if (poll()) { - tr_debug("Waiting for CFG ODO packet"); - uint32_t cfg = _flags.wait_all(UBX_FLAGS_CFG | UBX_FLAGS_SEARCH_DONE, MBED_CONF_UBXGPSI2C_TIMEOUT); + tr_debug("CFG-ODO packet received"); - if (!(cfg & UBX_FLAGS_ERROR)) { - tr_debug("CFG ODO packet received"); + memcpy(&odo, _buf, sizeof(odometer_t)); // _buf contains only payload - memcpy(&odo, _buf, sizeof(odometer_t)); // _buf contains only payload + odo.odoCfg &= ~0b111; + odo.odoCfg |= (uint8_t)profile; - odo.odoCfg &= ~0b111; - odo.odoCfg |= (uint8_t)profile; + if (velocity_filter > 0) { + odo.flags |= 0b100; - if (velocity_filter > 0) { - odo.flags |= 0b100; - - } else { - odo.flags &= ~0b100; - } - - odo.velLpGain = velocity_filter; - - if (enable) { - odo.flags |= 0b1; + } else { + odo.flags &= ~0b100; + } - } else { - odo.flags &= ~0b1; - } + odo.velLpGain = velocity_filter; - memcpy(_buf, &odo, sizeof(odometer_t)); + if (enable) { + odo.flags |= 0b1; - if (send_ack(UBX_CFG, UBX_CFG_ODO, _buf, sizeof(odometer_t))) { - tr_info("Odometer %s", enable ? "enabled" : "disabled"); + } else { + odo.flags &= ~0b1; + } - } else { - ok = false; - } + memcpy(_buf, &odo, sizeof(odometer_t)); - } else { - tr_error("CFG ODO timeout"); - ok = false; - } + if (send_ack(UBX_CFG, UBX_CFG_ODO, _buf, sizeof(odometer_t))) { + tr_info("Odometer %s", enable ? "enabled" : "disabled"); } else { ok = false; } - - remove_oob(UBX_CFG, UBX_CFG_ODO); } return ok; @@ -679,44 +667,51 @@ bool UbxGpsI2C::set_power_mode(PowerModeValue mode, uint16_t period, uint16_t on cfg_pms_t cfg_pms; tr_debug("Setting power mode"); - bool ok = send(UBX_CFG, UBX_CFG_PMS); + bool ok = get_cfg(UBX_CFG_PMS); if (ok) { - // register our packet before polling - oob(UBX_CFG, UBX_CFG_PMS, callback(this, &UbxGpsI2C::cfg_cb)); + tr_debug("CFG-PMS packet received"); - if (poll()) { - tr_debug("Waiting for CFG PMS packet"); - uint32_t cfg = _flags.wait_all(UBX_FLAGS_CFG | UBX_FLAGS_SEARCH_DONE, MBED_CONF_UBXGPSI2C_TIMEOUT); + memcpy(&cfg_pms, _buf, sizeof(cfg_pms_t)); // _buf contains only payload - if (!(cfg & UBX_FLAGS_ERROR)) { - tr_debug("CFG PMS packet received"); + cfg_pms.powerSetupValue = (uint8_t)mode; + cfg_pms.period = (mode == PSV_INTERVAL) ? period : 0; + cfg_pms.onTime = (mode == PSV_INTERVAL) ? on_time : 0; - memcpy(&cfg_pms, _buf, sizeof(cfg_pms_t)); // _buf contains only payload + memcpy(_buf, &cfg_pms, sizeof(cfg_pms_t)); - cfg_pms.powerSetupValue = (uint8_t)mode; - cfg_pms.period = (mode == PSV_INTERVAL) ? period : 0; - cfg_pms.onTime = (mode == PSV_INTERVAL) ? on_time : 0; + if (send_ack(UBX_CFG, UBX_CFG_PMS, _buf, sizeof(cfg_pms_t))) { + tr_info("New power mode: %u", mode); - memcpy(_buf, &cfg_pms, sizeof(cfg_pms_t)); + } else { + ok = false; + } + } - if (send_ack(UBX_CFG, UBX_CFG_PMS, _buf, sizeof(cfg_pms_t))) { - tr_info("New power mode: %u", mode); + return ok; +} - } else { - ok = false; - } +bool UbxGpsI2C::set_low_power(bool low_power) { + cfg_rxm_t cfg_rxm; + tr_debug("Setting low power"); - } else { - tr_error("CFG PMS timeout"); - ok = false; - } + bool ok = get_cfg(UBX_CFG_RXM); + + if (ok) { + tr_debug("CFG-RXM received"); + + memcpy(&cfg_rxm, _buf, sizeof(cfg_rxm_t)); // _buf contains only payload + + cfg_rxm.lpMode = low_power; + + memcpy(_buf, &cfg_rxm, sizeof(cfg_rxm_t)); + + if (send_ack(UBX_CFG, UBX_CFG_RXM, _buf, sizeof(cfg_rxm_t))) { + tr_info("Low power: %u", low_power); } else { ok = false; } - - remove_oob(UBX_CFG, UBX_CFG_PMS); } return ok; diff --git a/UbxGpsI2C.h b/UbxGpsI2C.h index fe2114b..c9f1f2f 100644 --- a/UbxGpsI2C.h +++ b/UbxGpsI2C.h @@ -65,6 +65,7 @@ using namespace std::chrono; #define UBX_CFG_RST 0x04 #define UBX_CFG_RATE 0x08 #define UBX_CFG_CFG 0x09 +#define UBX_CFG_RXM 0x11 #define UBX_CFG_ODO 0x1E #define UBX_CFG_PMS 0x86 @@ -119,6 +120,11 @@ class UbxGpsI2C { uint8_t reserved4[2]; }; + struct cfg_rxm_t { + uint8_t reserved; + uint8_t lpMode; + }; + typedef enum { UBX_NAV = 0x01, UBX_RXM = 0x02, @@ -171,6 +177,7 @@ class UbxGpsI2C { bool set_output_rate(milliseconds ms, uint16_t cycles = 1); bool set_odometer(bool enable, UbxOdoProfile profile, uint8_t velocity_filter = 0); bool set_power_mode(PowerModeValue mode, uint16_t period = 0, uint16_t on_time = 0); + bool set_low_power(bool low_power); bool get_protocol_version(char *version); bool reset_odometer(); @@ -196,6 +203,7 @@ class UbxGpsI2C { bool get_data(); uint16_t get_sync_index(const char *buf, uint16_t buf_size, char c, uint16_t offset = 0); void search_data(); + bool get_cfg(char id); void tx_cb(int event); void rx_cb(int event);