Skip to content

Commit

Permalink
fixes + new fn
Browse files Browse the repository at this point in the history
  • Loading branch information
@pilotak
pilotak committed Mar 2, 2021
1 parent d5e0d01 commit 71936eb
Show file tree
Hide file tree
Showing 3 changed files with 225 additions and 9 deletions.
101 changes: 97 additions & 4 deletions UbxGpsI2C.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/*
MIT License
Copyright (c) 2020 Pavel Slama
Copyright (c) 2021 Pavel Slama
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
Expand Down Expand Up @@ -412,13 +412,13 @@ void UbxGpsI2C::search_data() {
uint16_t new_bytes = bytes_available();

if (new_bytes == 0 || new_bytes == USHRT_MAX) {
tr_debug("Search done");
_flags.set(UBX_FLAGS_SEARCH_DONE);

} else {
tr_debug("There are new bytes to read");
_bytes_available = new_bytes;
get_data();
return;
}
}

Expand All @@ -436,6 +436,8 @@ bool UbxGpsI2C::get_cfg(char id) {

if (poll()) {
tr_debug("Waiting for CFG packet");
_flags.clear(UBX_FLAGS_CFG | UBX_FLAGS_SEARCH_DONE);

uint32_t cfg = _flags.wait_all(UBX_FLAGS_CFG | UBX_FLAGS_SEARCH_DONE, MBED_CONF_UBXGPSI2C_TIMEOUT);

if (cfg & UBX_FLAGS_ERROR) {
Expand Down Expand Up @@ -533,6 +535,7 @@ void UbxGpsI2C::cfg_cb() {
}

void UbxGpsI2C::mon_cb() {
// it is crucial to extract it in cb, because there could be another class waiting
for (auto i = UBX_HEADER_LEN + 40; i < _packet_len; i = i + 30) {
if (data[i] == 'P' && data[i + 1] == 'R') {
_buf[0] = (data[i + 8] - '0') * 10 + (data[i + 9] - '0');
Expand All @@ -550,6 +553,8 @@ bool UbxGpsI2C::init(I2C *i2c_obj) {
_i2c = i2c_obj;
}

MBED_ASSERT(_i2c);

tr_info("Setting up ublox GPS");

_buf[0] = 0; // DDC port
Expand All @@ -569,7 +574,8 @@ bool UbxGpsI2C::init(I2C *i2c_obj) {

memcpy(&cfg_prt, _buf, sizeof(cfg_prt_t)); // _buf contains only payload

cfg_prt.outProtoMask &= ~0b110; // output only UBX
cfg_prt.outProtoMask = 1; // output only UBX
cfg_prt.flags |= 0b10; // enable extendedTxTimeout

memcpy(_buf, &cfg_prt, sizeof(cfg_prt_t));

Expand Down Expand Up @@ -713,7 +719,7 @@ bool UbxGpsI2C::set_power_mode(PowerModeValue mode, uint16_t period, uint16_t on
return ok;
}

bool UbxGpsI2C::set_low_power(bool low_power) {
bool UbxGpsI2C::set_psm(bool low_power) {
cfg_rxm_t cfg_rxm;
tr_debug("Setting low power");

Expand Down Expand Up @@ -750,6 +756,8 @@ bool UbxGpsI2C::get_protocol_version(char *version) {

if (poll()) {
tr_debug("Waiting for MON VER packet");
_flags.clear(UBX_FLAGS_MON | UBX_FLAGS_SEARCH_DONE);

uint32_t cfg = _flags.wait_all(UBX_FLAGS_MON | UBX_FLAGS_SEARCH_DONE, MBED_CONF_UBXGPSI2C_TIMEOUT);

if (!(cfg & UBX_FLAGS_ERROR)) {
Expand All @@ -774,6 +782,19 @@ bool UbxGpsI2C::get_protocol_version(char *version) {
return ok;
}

bool UbxGpsI2C::reset(ResetMode mode, uint16_t bbr_mask) {
cfg_rst_t cfg_rst;
tr_info("Resetting");

cfg_rst.navBbrMask = bbr_mask;
cfg_rst.resetMode = mode;
cfg_rst.reserved1 = 0;

memcpy(_buf, &cfg_rst, sizeof(cfg_rst_t));

return send(UBX_CFG, UBX_CFG_RST, _buf, sizeof(cfg_rst_t));
}

bool UbxGpsI2C::reset_odometer() {
tr_debug("Resetting odometer");

Expand All @@ -785,3 +806,75 @@ bool UbxGpsI2C::reset_odometer() {

return false;
}

bool UbxGpsI2C::permanent_configuration(PermanentConfig type, uint32_t mask, uint8_t device_mask) {
cfg_cfg_t cfg_cfg;
tr_debug("Setting permanent configuration");

cfg_cfg.clearMask = 0;
cfg_cfg.saveMask = 0;
cfg_cfg.loadMask = 0;

switch (type) {
case Clear:
cfg_cfg.clearMask = mask;
break;

case Save:
cfg_cfg.saveMask = mask;
break;

case Load:
cfg_cfg.loadMask = mask;
break;
}

cfg_cfg.deviceMask = device_mask;

memcpy(_buf, &cfg_cfg, sizeof(cfg_cfg_t));

if (send_ack(UBX_CFG, UBX_CFG_CFG, _buf, sizeof(cfg_cfg_t))) {
tr_info("Permanent configuration OK");

return true;
}

return false;
}

bool UbxGpsI2C::set_dynamic_model(DynamicModel model) {
cfg_nav5_t cfg_nav5;

bool ok = get_cfg(UBX_CFG_NAV5);

if (ok) {
tr_debug("CFG-NAV5 received");

memcpy(&cfg_nav5, _buf, sizeof(cfg_nav5_t)); // _buf contains only payload

cfg_nav5.dynModel = model;

memcpy(_buf, &cfg_nav5, sizeof(cfg_nav5_t));

if (send_ack(UBX_CFG, UBX_CFG_NAV5, _buf, sizeof(cfg_nav5_t))) {
tr_info("Dynamic model set to: %u", model);

} else {
ok = false;
}
}

return ok;
}

bool UbxGpsI2C::wakeup() {
int32_t ack = -1;

_buf[0] = 0xFF;

_i2c->lock();
ack = _i2c->write(_i2c_addr, _buf, 1);
_i2c->unlock();

return (ack == 0);
}
129 changes: 126 additions & 3 deletions UbxGpsI2C.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
/*
MIT License
Copyright (c) 2020 Pavel Slama
Copyright (c) 2021 Pavel Slama
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
Expand Down Expand Up @@ -66,7 +66,10 @@ using namespace std::chrono;
#define UBX_CFG_RATE 0x08
#define UBX_CFG_CFG 0x09
#define UBX_CFG_RXM 0x11
#define UBX_CFG_SBAS 0x16
#define UBX_CFG_ODO 0x1E
#define UBX_CFG_NAV5 0x24
#define UBX_CFG_PM2 0x3B
#define UBX_CFG_PMS 0x86

#define UBX_ACK_NAK 0x00
Expand All @@ -81,7 +84,30 @@ using namespace std::chrono;
class UbxGpsI2C {
public:
struct cfg_prt_t {
uint8_t port;
uint8_t portID;
uint8_t reserved1;
uint16_t txReady;
uint32_t mode;
uint8_t reserved2[4];
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint8_t reserved3[2];
};

struct cfg_prt_usb_t {
uint8_t portID;
uint8_t reserved1;
uint16_t txReady;
uint8_t reserved2[8];
uint16_t inProtoMask;
uint16_t outProtoMask;
uint8_t reserved3[2];
uint8_t reserved4[2];
};

struct cfg_prt_uart_t {
uint8_t portID;
uint8_t reserved1;
uint16_t txReady;
uint32_t mode;
Expand Down Expand Up @@ -125,6 +151,63 @@ class UbxGpsI2C {
uint8_t lpMode;
};

struct cfg_cfg_t {
uint32_t clearMask;
uint32_t saveMask;
uint32_t loadMask;
uint8_t deviceMask;
};

struct cfg_nav5_t {
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgnssTimeout;
uint8_t cnoThreshNumSVs;
uint8_t cnoThresh;
uint8_t reserved1[2];
uint16_t staticHoldMaxDist;
uint8_t utcStandard;
uint8_t reserved2[5];
};

struct cfg_sbas_t {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
};

struct cfg_pm2_t {
uint8_t version;
uint8_t reserved1;
uint8_t maxStartupStateDur; // s
uint8_t reserved2;
uint32_t flags;
uint32_t updatePeriod; // ms
uint32_t searchPeriod; // ms
uint32_t gridOffset; // ms
uint16_t onTime; // s
uint16_t minAcqTime; // s
uint8_t reserved3[20];
};

struct cfg_rst_t {
uint16_t navBbrMask;
uint8_t resetMode;
uint8_t reserved1;
};

typedef enum {
UBX_NAV = 0x01,
UBX_RXM = 0x02,
Expand Down Expand Up @@ -160,6 +243,42 @@ class UbxGpsI2C {
PSV_INVALID = 0xFF,
} PowerModeValue;

typedef enum {
Clear,
Save,
Load
} PermanentConfig;

typedef enum {
DevBBR = 0b1, // Battery backed RAM
DevFlash = 0b10, // Flash
DevEEPROM = 0b100, // EEPROM
DevSpiFlash = 0b10000, // SPI Flash
DevAll = 0b10111
} DeviceMask;

typedef enum {
Portable = 0,
Stationary = 2,
Pedestrian,
Automotive,
Sea,
Airborne1g,
Airborne2g,
Airborne4g,
WristWornWatch,
Bike
} DynamicModel;

typedef enum {
HardwareReset = 0, // immediately
SoftwareReset,
GnssReset,
AfterShutdown = 4,
ControlledGnssStop = 8,
ControlledGnssStart,
} ResetMode;

UbxGpsI2C(EventQueue *queue, int8_t address = UBX_DEFAULT_ADDRESS);
UbxGpsI2C(PinName sda, PinName scl, EventQueue *queue, int8_t address = UBX_DEFAULT_ADDRESS,
uint32_t frequency = 400000);
Expand All @@ -177,9 +296,13 @@ 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 set_psm(bool low_power);
bool get_protocol_version(char *version);
bool reset(ResetMode mode, uint16_t bbr_mask);
bool reset_odometer();
bool permanent_configuration(PermanentConfig type, uint32_t mask, uint8_t device_mask);
bool set_dynamic_model(DynamicModel model);
bool wakeup();

char data[MBED_CONF_UBXGPSI2C_DATA_SIZE] = {0};

Expand Down
4 changes: 2 additions & 2 deletions mbed_lib.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@
"required": true
},
"buffer-size": {
"help": "Max size of TX buffer for I2C transfers",
"value": 32,
"help": "Max size of TX buffer for I2C transfers and CFG packet",
"value": 64,
"required": true
},
"repeat-count": {
Expand Down

0 comments on commit 71936eb

Please sign in to comment.