diff --git a/altimu10.cpp b/altimu10.cpp new file mode 100644 index 0000000..43d4d82 --- /dev/null +++ b/altimu10.cpp @@ -0,0 +1,291 @@ +#include "vector.h" +#include "altimu10.h" +#include "exceptions.h" +#include +#include +#include +#include +#include + +altimu10::comm_config altimu10::auto_detect(const std::string & i2c_bus_name) +{ + i2c_bus bus(i2c_bus_name.c_str()); + altimu10::comm_config config; + + // Detect LSM6 devices. + { + auto addrs = { lsm6::SA0_LOW_ADDR, lsm6::SA0_HIGH_ADDR }; + for (uint8_t addr : addrs) + { + int result = bus.try_write_byte_and_read_byte(addr, lsm6::WHO_AM_I); + if (result == lsm6::LSM6DS33) + { + config.lsm6.use_sensor = true; + config.lsm6.device = (lsm6::device_type)result; + config.lsm6.i2c_bus_name = i2c_bus_name; + config.lsm6.i2c_address = (lsm6::i2c_addr)addr; + break; + } + } + } + + // Detect LIS3MDL devices. + { + auto addrs = { lis3mdl::SA1_LOW_ADDR, lis3mdl::SA1_HIGH_ADDR }; + for (uint8_t addr : addrs) + { + int result = bus.try_write_byte_and_read_byte(addr, lis3mdl::WHO_AM_I); + if (result == lis3mdl::LIS3MDL) + { + config.lis3mdl.use_sensor = true; + config.lis3mdl.device = (lis3mdl::device_type)result; + config.lis3mdl.i2c_bus_name = i2c_bus_name; + config.lis3mdl.i2c_address = (lis3mdl::i2c_addr)addr; + break; + } + } + } + + + // Detect LPS25H devices. + { + auto & c = config.lps25h; + if (lps25h::LPS25H == bus.try_write_byte_and_read_byte( + lps25h::SA0_HIGH_ADDR, lps25h::WHO_AM_I)) + { + c.use_sensor = true; + c.device = lps25h::LPS25H; + c.i2c_bus_name = i2c_bus_name; + c.i2c_address = lps25h::SA0_HIGH_ADDR; + } + + } + + return config; +} + +sensor_set altimu10::config_sensor_set(const comm_config & config) +{ + sensor_set set; + + if (config.lsm6.use_sensor) + { + set.acc = true; + set.gyro = true; + } + + if (config.lis3mdl.use_sensor) + { + set.mag = true; + } + + if (config.lps25h.use_sensor) + { + set.temp = true; + set.press = true; + } + + return set; +} + +altimu10::comm_config altimu10::disable_redundant_sensors( + const comm_config & in, const sensor_set & needed) +{ + comm_config config = in; + + return config; +} + +void altimu10::handle::open(const comm_config & config) +{ + this->config = config; + + if (config.lsm6.use_sensor) + { + lsm6.open(config.lsm6); + } + + if (config.lis3mdl.use_sensor) + { + lis3mdl.open(config.lis3mdl); + } + + if (config.lps25h.use_sensor) + { + lps25h.open(config.lps25h); + } +} + +void altimu10::handle::enable() +{ + if (config.lsm6.use_sensor) + { + lsm6.enable(); + } + + if (config.lis3mdl.use_sensor) + { + lis3mdl.enable(); + } + + if (config.lps25h.use_sensor) + { + lps25h.enable(); + } +} + +void altimu10::handle::load_calibration() +{ + wordexp_t expansion_result; + wordexp("~/.minimu9-ahrs-cal", &expansion_result, 0); + + std::ifstream file(expansion_result.we_wordv[0]); + if (file.fail()) + { + throw posix_error("Failed to open calibration file ~/.ahrs-cal"); + } + + file >> mag_min(0) >> mag_max(0) + >> mag_min(1) >> mag_max(1) + >> mag_min(2) >> mag_max(2); + if (file.fail() || file.bad()) + { + throw std::runtime_error("Failed to parse calibration file ~/.ahrs-cal"); + } +} + +void altimu10::handle::read_mag_raw() +{ + if (config.lis3mdl.use_sensor) + { + lis3mdl.read(); + for (int i = 0; i < 3; i++) { m[i] = lis3mdl.m[i]; } + } + else + { + throw std::runtime_error("No magnetometer to read."); + } +} + +void altimu10::handle::read_acc_raw() +{ + if (config.lsm6.use_sensor) + { + lsm6.read_acc(); + for (int i = 0; i < 3; i++) { a[i] = lsm6.a[i]; } + } + else + { + throw std::runtime_error("No accelerometer to read."); + } +} + +void altimu10::handle::read_gyro_raw() +{ + if (config.lsm6.use_sensor) + { + lsm6.read_gyro(); + for (int i = 0; i < 3; i++) { g[i] = lsm6.g[i]; } + } + else + { + throw std::runtime_error("No gyro to read."); + } +} + +void altimu10::handle::read_press_raw() +{ + if (config.lps25h.use_sensor) + { + lps25h.read_press(); + p = lps25h.p; + refp = lps25h.refp; + } + else + { + throw std::runtime_error("No barometer to read."); + } +} + +void altimu10::handle::read_temp_raw() +{ + if (config.lps25h.use_sensor) + { + lps25h.read_temp(); + t = lps25h.t; + } + else + { + throw std::runtime_error("No termometer found."); + } +} + +float altimu10::handle::get_acc_scale() const +{ + // Info about linear acceleration sensitivity from datasheets: + // LSM303DLM: at FS = 8 g, 3.9 mg/digit (12-bit reading) + // LSM303DLHC: at FS = 8 g, 4 mg/digit (12-bit reading probably an approximation) + // LSM303DLH: at FS = 8 g, 3.9 mg/digit (12-bit reading) + // LSM303D: at FS = 8 g, 0.244 mg/LSB (16-bit reading) + // LSM6DS33: at FS = 8 g, 0.244 mg/LSB (16-bit reading) + return 0.000244; +} + +float altimu10::handle::get_gyro_scale() const +{ + // Info about sensitivity from datasheets: + // L3G4200D, FS = 2000 dps: 70 mdps/digit + // L3GD20, FS = 2000 dps: 70 mdps/digit + // L3GD20H, FS = 2000 dps: 70 mdps/digit + // LSM6DS33, FS = 2000 dps: 70 mdps/digit + return 0.07 * 3.14159265 / 180; +} + +void altimu10::handle::measure_offsets() +{ + // LSM303 accelerometer: 8 g sensitivity. 3.8 mg/digit; 1 g = 256. + gyro_offset = vector::Zero(); + const int sampleCount = 32; + for(int i = 0; i < sampleCount; i++) + { + read_gyro_raw(); + gyro_offset += vector_from_ints(&g); + usleep(20 * 1000); + } + gyro_offset /= sampleCount; +} + +vector altimu10::handle::read_mag() +{ + read_mag_raw(); + + vector v; + v(0) = (float)(m[0] - mag_min(0)) / (mag_max(0) - mag_min(0)) * 2 - 1; + v(1) = (float)(m[1] - mag_min(1)) / (mag_max(1) - mag_min(1)) * 2 - 1; + v(2) = (float)(m[2] - mag_min(2)) / (mag_max(2) - mag_min(2)) * 2 - 1; + return v; +} + +vector altimu10::handle::read_acc() +{ + read_acc_raw(); + return vector_from_ints(&a) * get_acc_scale(); +} + +vector altimu10::handle::read_gyro() +{ + read_gyro_raw(); + return (vector_from_ints(&g) - gyro_offset) * get_gyro_scale(); +} + +float altimu10::handle::read_temp() +{ + read_temp_raw(); + return (((float)t/480) + 42.5); +} + +float altimu10::handle::read_press() +{ + read_press_raw(); + return ((float)(p+refp)/4096.f); +} diff --git a/altimu10.h b/altimu10.h new file mode 100644 index 0000000..448537f --- /dev/null +++ b/altimu10.h @@ -0,0 +1,52 @@ +#pragma once + +#include "imu.h" +#include "lsm6.h" +#include "lis3mdl.h" +#include "lps25h.h" +#include "sensor_set.h" + +namespace altimu10 +{ + // Represents the sensors of the AltIMU-10v5 and how to communicate with them. + struct comm_config { + lis3mdl::comm_config lis3mdl; + lsm6::comm_config lsm6; + lps25h::comm_config lps25h; + }; + + comm_config auto_detect(const std::string & i2c_bus_name); + + sensor_set config_sensor_set(const comm_config &); + + comm_config disable_redundant_sensors(const comm_config &, const sensor_set &); + + class handle : public imu { + public: + void open(const comm_config &); + + comm_config config; + lsm6::handle lsm6; + lis3mdl::handle lis3mdl; + lps25h::handle lps25h; + + virtual void read_acc_raw(); + virtual void read_mag_raw(); + virtual void read_gyro_raw(); + virtual void read_temp_raw(); + virtual void read_press_raw(); + + virtual float get_acc_scale() const; + virtual float get_gyro_scale() const; + + virtual vector read_acc(); + virtual vector read_mag(); + virtual vector read_gyro(); + virtual float read_temp(); + virtual float read_press(); + + virtual void enable(); + virtual void load_calibration(); + virtual void measure_offsets(); + }; +} diff --git a/imu.h b/imu.h index a3325fb..081dff0 100644 --- a/imu.h +++ b/imu.h @@ -10,11 +10,15 @@ class imu read_mag_raw(); read_acc_raw(); read_gyro_raw(); + read_temp_raw(); + read_press_raw(); } virtual void read_acc_raw() = 0; virtual void read_mag_raw() = 0; virtual void read_gyro_raw() = 0; + virtual void read_temp_raw() = 0; + virtual void read_press_raw() = 0; virtual float get_acc_scale() const = 0; virtual float get_gyro_scale() const = 0; @@ -22,6 +26,10 @@ class imu int32_t m[3]; int32_t a[3]; int32_t g[3]; + int16_t t; + int32_t p; + int32_t refp; + // TODO: remove stuff below this point @@ -29,7 +37,11 @@ class imu virtual vector read_mag() = 0; // In body coords, scaled to -1..1 range virtual vector read_acc() = 0; // In body coords, with units = g virtual vector read_gyro() = 0; // In body coords, with units = rad/sec - void read(){ read_mag(); read_acc(); read_gyro(); } + + virtual float read_temp() = 0; + virtual float read_press() = 0; + + void read(){ read_mag(); read_acc(); read_gyro(); read_temp(); read_press(); } virtual void measure_offsets() = 0; virtual void enable() = 0; diff --git a/lps25h.cpp b/lps25h.cpp new file mode 100644 index 0000000..eb930ef --- /dev/null +++ b/lps25h.cpp @@ -0,0 +1,67 @@ +#include "lps25h.h" +#include + +void lps25h::handle::open(const comm_config & config) +{ + if (!config.use_sensor) + { + throw std::runtime_error("LPS25H configuration is null."); + } + + this->config = config; + i2c.open(config.i2c_bus_name); +} + +void lps25h::handle::enable() +{ + if (config.device == LPS25H) + { + //// LPS25H altimeter + + // Trun devices off + write_reg(CTRL_REG1, 0x00); + + // Turn devices on + write_reg(CTRL_REG1, 0b10110100); + // Configure, set defaults: + write_reg(CTRL_REG2, 0x00); + + uint8_t xlow; + uint8_t low; + uint8_t high; + i2c.write_byte_and_read(config.i2c_address, REF_P_XL, &xlow, 1); + i2c.write_byte_and_read(config.i2c_address, REF_P_L, &low, 1); + i2c.write_byte_and_read(config.i2c_address, REF_P_H, &high, 1); + refp = (int32_t)(xlow | low << 8 | high << 16); + } + else + { + throw std::runtime_error("Cannot enable unknown device."); + } +} + +void lps25h::handle::write_reg(reg_addr addr, uint8_t value) +{ + i2c.write_two_bytes(config.i2c_address, addr, value); +} + +void lps25h::handle::read_temp() +{ + uint8_t low; + uint8_t high; + i2c.write_byte_and_read(config.i2c_address, TEMP_OUT_L, &low, 1); + i2c.write_byte_and_read(config.i2c_address, TEMP_OUT_H, &high, 1); + t = (int16_t)(low | high << 8); +} + +void lps25h::handle::read_press() +{ + uint8_t xlow; + uint8_t low; + uint8_t high; + i2c.write_byte_and_read(config.i2c_address, PRESS_POUT_XL, &xlow, 1); + i2c.write_byte_and_read(config.i2c_address, PRESS_OUT_L, &low, 1); + i2c.write_byte_and_read(config.i2c_address, PRESS_OUT_H, &high, 1); + p = (int32_t)(xlow | low << 8 | high << 16); +} + diff --git a/lps25h.h b/lps25h.h new file mode 100644 index 0000000..9cc09e2 --- /dev/null +++ b/lps25h.h @@ -0,0 +1,83 @@ +#pragma once + +#include +#include + +namespace lps25h +{ + enum device_type + { + LPS25H = 0xBD, + }; + + enum i2c_addr + { + SA0_LOW_ADDR = 0x5C, + SA0_HIGH_ADDR = 0x5D, + }; + + enum reg_addr + { + REF_P_XL = 0x08, + REF_P_L = 0x09, + REF_P_H = 0x0A, + + WHO_AM_I = 0x0F, + + RES_CONF = 0x10, + + CTRL_REG1 = 0x20, + CTRL_REG2 = 0x21, + CTRL_REG3 = 0x22, + CTRL_REG4 = 0x23, + + INT_CFG = 0x24, + INT_SOURCE = 0x25, + + STATUS_REG = 0x27, + + PRESS_POUT_XL = 0x28, + PRESS_OUT_L = 0x29, + PRESS_OUT_H = 0x2A, + + TEMP_OUT_L = 0x2B, + TEMP_OUT_H = 0x2C, + + FIFO_CTRL = 0x2E, + FIFO_STATUS = 0x2F, + + THS_P_L = 0x30, + THS_P_H = 0x31, + + RPDS_L = 0x39, + RPDS_H = 0x3A, + }; + + struct comm_config { + bool use_sensor = false; + device_type device; + std::string i2c_bus_name; + i2c_addr i2c_address; + }; + + class handle + { + public: + void open(const comm_config &); + + void enable(); + + void write_reg(reg_addr addr, uint8_t value); + + void read_temp(); + void read_press(); + + int16_t t; + int32_t p; + int32_t refp; + + protected: + i2c_bus i2c; + comm_config config; + }; +}; diff --git a/minimu9-ahrs.cpp b/minimu9-ahrs.cpp index 885d78a..843036f 100644 --- a/minimu9-ahrs.cpp +++ b/minimu9-ahrs.cpp @@ -4,7 +4,7 @@ #include "vector.h" #include "version.h" #include "prog_options.h" -#include "minimu9.h" +#include "altimu10.h" #include "exceptions.h" #include "pacer.h" #include @@ -164,9 +164,9 @@ void ahrs(imu & imu, fuse_function * fuse, rotation_output_function * output) // to ground coordinates when it its changed to a matrix. quaternion rotation = quaternion::Identity(); - // Set up a timer that expires every 20 ms. + // Set up a timer that expires every 1 s. pacer loop_pacer; - loop_pacer.set_period_ns(20000000); + loop_pacer.set_period_s(1); auto start = std::chrono::steady_clock::now(); while(1) @@ -180,11 +180,12 @@ void ahrs(imu & imu, fuse_function * fuse, rotation_output_function * output) vector angular_velocity = imu.read_gyro(); vector acceleration = imu.read_acc(); vector magnetic_field = imu.read_mag(); - + float temperature = imu.read_temp(); + float pressure = imu.read_press(); fuse(rotation, dt, angular_velocity, acceleration, magnetic_field); output(rotation); - std::cout << " " << acceleration << " " << magnetic_field << std::endl; + std::cout << " " << acceleration << " " << magnetic_field << " " << temperature << " " << pressure << std::endl; loop_pacer.pace(); } @@ -211,9 +212,9 @@ int main_with_exceptions(int argc, char **argv) sensor_set set; set.mag = set.acc = set.gyro = true; - minimu9::comm_config config = minimu9::auto_detect(options.i2c_bus_name); + altimu10::comm_config config = altimu10::auto_detect(options.i2c_bus_name); - sensor_set missing = set - minimu9::config_sensor_set(config); + sensor_set missing = set - altimu10::config_sensor_set(config); if (missing) { if (missing.mag) @@ -232,9 +233,7 @@ int main_with_exceptions(int argc, char **argv) return 1; } - config = minimu9::disable_redundant_sensors(config, set); - - minimu9::handle imu; + altimu10::handle imu; imu.open(config); rotation_output_function * output; diff --git a/pacer.h b/pacer.h index aa76cf7..87ac208 100644 --- a/pacer.h +++ b/pacer.h @@ -37,7 +37,28 @@ class pacer struct itimerspec spec = { 0 }; spec.it_value.tv_nsec = 1; - spec.it_interval.tv_nsec = 20000000; + spec.it_interval.tv_nsec = nanoseconds; + int result = timerfd_settime(fd, 0, &spec, NULL); + if (result == -1) + { + throw posix_error("Failed to set timerfd interval"); + } + } + +void set_period_s(uint32_t seconds) + { + if (fd == -1) + { + fd = timerfd_create(CLOCK_MONOTONIC, 0); + if (fd == -1) + { + throw posix_error("Failed to create timerfd"); + } + } + + struct itimerspec spec = { 0 }; + spec.it_value.tv_sec = 1; + spec.it_interval.tv_sec = seconds; int result = timerfd_settime(fd, 0, &spec, NULL); if (result == -1) { diff --git a/prog_options.cpp b/prog_options.cpp index 6907e3c..9bbdf17 100644 --- a/prog_options.cpp +++ b/prog_options.cpp @@ -22,7 +22,7 @@ static opts::options_description sensor_options_desc(prog_options & options) opts::options_description desc("Sensor options"); desc.add_options() ("i2c-bus,b", - opts::value(&options.i2c_bus_name)->default_value("/dev/i2c-0"), + opts::value(&options.i2c_bus_name)->default_value("/dev/i2c-1"), "I2C bus the IMU is connected to") ; return desc; diff --git a/sensor_set.h b/sensor_set.h index c5e0d0b..7a681f2 100644 --- a/sensor_set.h +++ b/sensor_set.h @@ -5,6 +5,8 @@ struct sensor_set bool mag = false; bool acc = false; bool gyro = false; + bool temp = false; + bool press = false; operator bool() { @@ -22,5 +24,7 @@ inline sensor_set operator-( r.mag = c1.mag && !c2.mag; r.acc = c1.acc && !c2.acc; r.gyro = c1.gyro && !c2.gyro; + r.temp = c1.temp && !c2.temp; + r.press = c1.press && !c2.press; return r; }