Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ist8308: small style and consistency cleanup #15613

Merged
merged 1 commit into from Aug 25, 2020
Merged

ist8308: small style and consistency cleanup #15613

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
140 changes: 77 additions & 63 deletions src/drivers/magnetometer/isentek/ist8308/IST8308.cpp
View file
Open in desktop
Expand Up @@ -40,7 +40,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
return (msb << 8u) | lsb;
}

IST8308::IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int bus_frequency) :
IST8308::IST8308(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) :
I2C(DRV_MAG_DEVTYPE_IST8308, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_px4_mag(get_device_id(), rotation)
Expand All @@ -50,7 +50,7 @@ IST8308::IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, in

IST8308::~IST8308()
{
perf_free(_transfer_perf);
perf_free(_reset_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
}
Expand Down Expand Up @@ -78,17 +78,18 @@ bool IST8308::Reset()
void IST8308::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_transfer_perf);

perf_print_counter(_reset_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
}

int IST8308::probe()
{
const uint8_t whoami = RegisterRead(Register::WAI);
const uint8_t WAI = RegisterRead(Register::WAI);

if (whoami != Device_ID) {
DEVICE_DEBUG("unexpected WAI 0x%02x", whoami);
if (WAI != Device_ID) {
DEVICE_DEBUG("unexpected WAI 0x%02x", WAI);
return PX4_ERROR;
}

Expand All @@ -97,13 +98,17 @@ int IST8308::probe()

void IST8308::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();

switch (_state) {
case STATE::RESET:
// CNTL3: Software Reset
RegisterSetAndClearBits(Register::CNTL3, CNTL3_BIT::SRST, 0);
_reset_timestamp = hrt_absolute_time();
RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST);
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(50_ms); // Power On Reset: max:50ms
perf_count(_reset_perf);
ScheduleDelayed(50_ms); // Power On Reset: max 50ms
break;

case STATE::WAIT_FOR_RESET:
Expand All @@ -114,18 +119,18 @@ void IST8308::RunImpl()

// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleNow();
ScheduleDelayed(10_ms);

} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_ERR("Reset failed, retrying");
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleNow();
ScheduleDelayed(100_ms);

} else {
PX4_DEBUG("Reset not complete, check again in 50 ms");
ScheduleDelayed(50_ms);
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
}
}

Expand All @@ -138,9 +143,16 @@ void IST8308::RunImpl()
ScheduleOnInterval(20_ms, 20_ms);

} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 50 ms
ScheduleDelayed(50_ms);
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;

} else {
PX4_DEBUG("Configure failed, retrying");
}

ScheduleDelayed(100_ms);
}

break;
Expand All @@ -156,41 +168,52 @@ void IST8308::RunImpl()
uint8_t DATAZH;
} buffer{};

perf_begin(_transfer_perf);
bool success = false;
uint8_t cmd = static_cast<uint8_t>(Register::STAT);

bool failure = false;
const hrt_abstime timestamp_sample = hrt_absolute_time();
const uint8_t cmd = static_cast<uint8_t>(Register::STAT);
if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {

if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
perf_count(_bad_transfer_perf);
failure = true;
}
if (buffer.STAT && STAT_BIT::DRDY) {
int16_t x = combine(buffer.DATAXH, buffer.DATAXL);
int16_t y = combine(buffer.DATAYH, buffer.DATAYL);
int16_t z = combine(buffer.DATAZH, buffer.DATAZL);

// sensor's frame is +x forward, +y right, +z up
z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z

_px4_mag.update(now, x, y, z);

perf_end(_transfer_perf);
success = true;

if (!failure && (buffer.STAT && STAT_BIT::DRDY)) {
float x = combine(buffer.DATAXH, buffer.DATAXL);
float y = combine(buffer.DATAYH, buffer.DATAYL);
float z = combine(buffer.DATAZH, buffer.DATAZL);
if (_failure_count > 0) {
_failure_count--;
}
}

} else {
perf_count(_bad_transfer_perf);
}

// sensor's frame is +x forward, +y right, +z up
z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
if (!success) {
_failure_count++;

_px4_mag.update(timestamp_sample, x, y, z);
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
return;
}
}

if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check registers incrementally
if (RegisterCheck(_register_cfg[_checked_register], true)) {
_last_config_check_timestamp = timestamp_sample;
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
_checked_register = (_checked_register + 1) % size_register_cfg;

} else {
// register check failed, force reconfigure
PX4_DEBUG("Health check failed, reconfiguring");
_state = STATE::CONFIGURE;
ScheduleNow();
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
}
}
Expand All @@ -201,10 +224,16 @@ void IST8308::RunImpl()

bool IST8308::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}

// now check that all are configured
bool success = true;

for (const auto &reg : _register_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
Expand All @@ -215,7 +244,7 @@ bool IST8308::Configure()
return success;
}

bool IST8308::RegisterCheck(const register_config_t &reg_cfg, bool notify)
bool IST8308::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;

Expand All @@ -231,15 +260,6 @@ bool IST8308::RegisterCheck(const register_config_t &reg_cfg, bool notify)
success = false;
}

if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);

if (notify) {
perf_count(_bad_register_perf);
_px4_mag.increase_error_count();
}
}

return success;
}

Expand All @@ -260,15 +280,9 @@ void IST8308::RegisterWrite(Register reg, uint8_t value)
void IST8308::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
uint8_t val = (orig_val & ~clearbits) | setbits;

if (setbits) {
val |= setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}

if (clearbits) {
val &= ~clearbits;
}

RegisterWrite(reg, val);
}
27 changes: 10 additions & 17 deletions src/drivers/magnetometer/isentek/ist8308/IST8308.hpp
View file
Open in desktop
Expand Up @@ -46,33 +46,26 @@
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>

using namespace iSentek_IST8308;

class IST8308 : public device::I2C, public I2CSPIDriver<IST8308>
{
public:
IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int bus_frequency);
IST8308(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE);
~IST8308() override;

static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();

void print_status();
void RunImpl();

int init() override;

void Start();
bool Reset();

void RunImpl();
void print_status() override;

private:
void custom_method(const BusCLIArguments &cli) override;

// Sensor Configuration
struct register_config_t {
Register reg;
Expand All @@ -82,34 +75,34 @@ class IST8308 : public device::I2C, public I2CSPIDriver<IST8308>

int probe() override;

bool Reset();

bool Configure();

bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
bool RegisterCheck(const register_config_t &reg_cfg);

uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);

PX4Magnetometer _px4_mag;

perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};

hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};

uint8_t _checked_register{0};
int _failure_count{0};

enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
READ,
};

STATE _state{STATE::RESET};
} _state{STATE::RESET};

uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{5};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
Expand Down
30 changes: 10 additions & 20 deletions src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp
View file
Open in desktop
Expand Up @@ -36,39 +36,33 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>

void
IST8308::print_usage()
{
PRINT_MODULE_USAGE_NAME("ist8308", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_COMMAND("reset");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

I2CSPIDriverBase *IST8308::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
IST8308 *instance = new IST8308(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency);
IST8308 *instance = new IST8308(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation);

if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}

if (OK != instance->init()) {
if (instance->init() != PX4_OK) {
delete instance;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}

return instance;
}

void IST8308::custom_method(const BusCLIArguments &cli)
void IST8308::print_usage()
{
Reset();
PRINT_MODULE_USAGE_NAME("ist8308", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

extern "C" int ist8308_main(int argc, char *argv[])
Expand Down Expand Up @@ -107,10 +101,6 @@ extern "C" int ist8308_main(int argc, char *argv[])
return ThisDriver::module_status(iterator);
}

if (!strcmp(verb, "reset")) {
return ThisDriver::module_custom_method(cli, iterator);
}

ThisDriver::print_usage();
return -1;
}