Skip to content

Commit

Permalink
low power
Browse files Browse the repository at this point in the history
  • Loading branch information
@pilotak
pilotak committed Feb 22, 2021
1 parent 9fbdf10 commit bcf1fd7
Show file tree
Hide file tree
Showing 3 changed files with 95 additions and 91 deletions.
3 changes: 2 additions & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,7 +241,8 @@ int main() {
},
"target_overrides": {
"*": {
"mbed-trace.enable": true
"mbed-trace.enable": true,
"ubxgpsi2c.debug": true
}
}
}
Expand Down
175 changes: 85 additions & 90 deletions UbxGpsI2C.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<void()> cb) {
// In case we have it already in, remove it
remove_oob(class_id, id);
Expand Down Expand Up @@ -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<milliseconds>(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<milliseconds>(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<milliseconds>(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<milliseconds>(ms).count() * cycles);

} else {
ok = false;
}

remove_oob(UBX_CFG, UBX_CFG_RATE);
}

return ok;
Expand All @@ -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;
Expand All @@ -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;
Expand Down
8 changes: 8 additions & 0 deletions UbxGpsI2C.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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();

Expand All @@ -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);
Expand Down

0 comments on commit bcf1fd7

Please sign in to comment.