diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp index c4e8a66c9e..49bb4bb855 100755 --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -154,9 +154,11 @@ MPU9250_mag::~MPU9250_mag() if (_mag_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); } + if (_mag_reports != nullptr) { delete _mag_reports; } + perf_free(_mag_reads); } @@ -174,6 +176,7 @@ MPU9250_mag::init() } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + if (_mag_reports == nullptr) { goto out; } @@ -187,7 +190,7 @@ MPU9250_mag::init() _mag_reports->get(&mrp); _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag_orb_class_instance, ORB_PRIO_LOW); // &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_mag_topic == nullptr) { @@ -208,15 +211,18 @@ MPU9250_mag::measure(struct ak8963_regs data) _parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | sizeof(struct ak8963_regs)); _mag_reading_data = true; return; + } else { _parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | 1); _mag_reading_data = false; } + } else { if (true == _mag_reading_data) { _parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | 1); _mag_reading_data = false; } + return; } @@ -294,6 +300,7 @@ MPU9250_mag::read(struct file *filp, char *buffer, size_t buflen) if (!_mag_reports->get(mrp)) { break; } + transferred++; mrp++; } @@ -345,6 +352,7 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) if (MPU9250_AK8963_SAMPLE_RATE != arg) { return -EINVAL; } + return OK; } } @@ -378,6 +386,7 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) return MPU9250_AK8963_SAMPLE_RATE; case MAGIOCSSAMPLERATE: + /* * We don't currently support any means of changing * the sampling rate of the mag @@ -385,6 +394,7 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) if (MPU9250_AK8963_SAMPLE_RATE != arg) { return -EINVAL; } + return OK; case MAGIOCSSCALE: @@ -407,11 +417,13 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg) return self_test(); #ifdef MAGIOCSHWLOWPASS + case MAGIOCSHWLOWPASS: return -EINVAL; #endif #ifdef MAGIOCGHWLOWPASS + case MAGIOCGHWLOWPASS: return -EINVAL; #endif @@ -433,12 +445,15 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) uint8_t addr; _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers + if (out) { _parent->write_reg(MPUREG_I2C_SLV0_D0, *out); addr = AK8963_I2C_ADDR; + } else { addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG; } + _parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr); _parent->write_reg(MPUREG_I2C_SLV0_REG, reg); _parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLVO_EN); @@ -470,10 +485,12 @@ MPU9250_mag::ak8963_check_id(void) for (int i = 0; i < 5; i++) { uint8_t deviceid = 0; passthrough_read(AK8963REG_WIA, &deviceid, 0x01); + if (AK8963_DEVICE_ID == deviceid) { return true; } } + return false; } /* @@ -503,13 +520,16 @@ MPU9250_mag::ak8963_read_adjustments(void) usleep(50); passthrough_read(AK8963REG_ASAX, response, 3); passthrough_write(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); + for (int i = 0; i < 3; i++) { if (0 != response[i] && 0xff != response[i]) { ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f; + } else { return false; } } + _mag_asa_x = ak8963_ASA[0]; _mag_asa_y = ak8963_ASA[1]; _mag_asa_z = ak8963_ASA[2]; diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h index e735c05af9..e7d8c34b97 100644 --- a/src/drivers/mpu9250/mag.h +++ b/src/drivers/mpu9250/mag.h @@ -34,13 +34,13 @@ class MPU9250; #pragma pack(push, 1) - struct ak8963_regs { - uint8_t st1; - int16_t x; - int16_t y; - int16_t z; - uint8_t st2; - }; +struct ak8963_regs { + uint8_t st1; + int16_t x; + int16_t y; + int16_t z; + uint8_t st2; +}; #pragma pack(pop) /** diff --git a/src/drivers/mpu9250/main.cpp b/src/drivers/mpu9250/main.cpp index efab0cedfb..d178175cf3 100644 --- a/src/drivers/mpu9250/main.cpp +++ b/src/drivers/mpu9250/main.cpp @@ -117,7 +117,7 @@ start(bool external_bus, enum Rotation rotation) const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG; if (*g_dev_ptr != nullptr) - /* if already started, the still command succeeded */ + /* if already started, the still command succeeded */ { errx(0, "already started"); } @@ -202,8 +202,9 @@ test(bool external_bus) /* get the driver */ int fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'm start')", path_accel); + } /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); @@ -344,7 +345,7 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 3d8fbad58b..738ebd0874 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -213,7 +213,8 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU }; -MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device, enum Rotation rotation) : +MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device, + enum Rotation rotation) : SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED), _gyro(new MPU9250_gyro(this, path_gyro)), _mag(new MPU9250_mag(this, path_mag)), @@ -340,6 +341,7 @@ MPU9250::init() } ret = probe(); + if (ret != OK) { DEVICE_DEBUG("MPU9250 probe failed"); return ret; @@ -1235,7 +1237,7 @@ MPU9250::check_registers(void) _checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS; } -bool MPU9250::check_null_data(uint32_t* data, uint8_t size) +bool MPU9250::check_null_data(uint32_t *data, uint8_t size) { while (size--) { if (*data++) { @@ -1243,6 +1245,7 @@ bool MPU9250::check_null_data(uint32_t* data, uint8_t size) return false; } } + // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); @@ -1253,7 +1256,7 @@ bool MPU9250::check_null_data(uint32_t* data, uint8_t size) return true; } -bool MPU9250::check_duplicate(uint8_t* accel_data) +bool MPU9250::check_duplicate(uint8_t *accel_data) { /* see if this is duplicate accelerometer data. Note that we @@ -1268,10 +1271,12 @@ bool MPU9250::check_duplicate(uint8_t* accel_data) perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; + } else { memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data)); _got_duplicate = false; } + return _got_duplicate; } @@ -1329,7 +1334,7 @@ MPU9250::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); - if (check_null_data((uint32_t*)&report, sizeof(report) / 4)) { + if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) { return; } diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h index e3e27bb955..ecbf894db0 100644 --- a/src/drivers/mpu9250/mpu9250.h +++ b/src/drivers/mpu9250/mpu9250.h @@ -20,7 +20,8 @@ class MPU9250_gyro; class MPU9250 : public device::SPI { public: - MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device, enum Rotation rotation); + MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device, + enum Rotation rotation); virtual ~MPU9250(); virtual int init(); @@ -110,8 +111,8 @@ private: // last temperature reading for print_info() float _last_temperature; - bool check_null_data(uint32_t* data, uint8_t size); - bool check_duplicate(uint8_t* accel_data); + bool check_null_data(uint32_t *data, uint8_t size); + bool check_duplicate(uint8_t *accel_data); // keep last accel reading for duplicate detection uint8_t _last_accel_data[6]; bool _got_duplicate;