diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9955eedb85..7ea3b55eda 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -157,7 +157,7 @@ private: unsigned _measure_ticks; ringbuffer::RingBuffer *_reports; - mag_scale _scale; + struct mag_calibration_s _scale; float _range_scale; float _range_ga; bool _collect_phase; @@ -737,14 +737,14 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSSCALE: /* set new scale factors */ - memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale)); /* check calibration, but not actually return an error */ (void)check_calibration(); return 0; case MAGIOCGSCALE: /* copy out scale factors */ - memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); + memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale)); return 0; case MAGIOCCALIBRATE: diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h index e7d8c34b97..d86f6eb74b 100644 --- a/src/drivers/mpu9250/mag.h +++ b/src/drivers/mpu9250/mag.h @@ -79,7 +79,7 @@ private: int _mag_class_instance; bool _mag_reading_data; ringbuffer::RingBuffer *_mag_reports; - struct mag_scale _mag_scale; + struct mag_calibration_s _mag_scale; float _mag_range_scale; perf_counter_t _mag_reads; float _mag_asa_x; diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index e28e49b629..a8abd0d2d1 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -194,250 +194,6 @@ */ #define MPU9250_TIMER_REDUCTION 200 -class MPU9250_gyro; - -class MPU9250 : public device::SPI -{ -public: - MPU9250(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation); - virtual ~MPU9250(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - void print_registers(); - - // deliberately cause a sensor error - void test_error(); - -protected: - virtual int probe(); - - friend class MPU9250_gyro; - - virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); - -private: - MPU9250_gyro *_gyro; - uint8_t _whoami; /** whoami result */ - - struct hrt_call _call; - unsigned _call_interval; - - ringbuffer::RingBuffer *_accel_reports; - - struct accel_calibration_s _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - int _accel_orb_class_instance; - int _accel_class_instance; - - ringbuffer::RingBuffer *_gyro_reports; - - struct gyro_calibration_s _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; - - unsigned _dlpf_freq; - - unsigned _sample_rate; - perf_counter_t _accel_reads; - perf_counter_t _gyro_reads; - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _bad_registers; - perf_counter_t _good_transfers; - perf_counter_t _reset_retries; - perf_counter_t _duplicates; - perf_counter_t _controller_latency_perf; - - uint8_t _register_wait; - uint64_t _reset_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - - Integrator _accel_int; - Integrator _gyro_int; - - enum Rotation _rotation; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define MPU9250_NUM_CHECKED_REGISTERS 11 - static const uint8_t _checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; - - // last temperature reading for print_info() - float _last_temperature; - - // keep last accel reading for duplicate detection - uint16_t _last_accel[3]; - bool _got_duplicate; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - void measure(); - - /** - * Read a register from the MPU9250 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); - uint16_t read_reg16(unsigned reg); - - /** - * Write a register in the MPU9250 - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the MPU9250 - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the MPU9250, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - - /** - * Set the MPU9250 measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_accel_range(unsigned max_g); - - /** - * Swap a 16-bit value read from the MPU9250 to native byte order. - */ - uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } - - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_bus == EXTERNAL_BUS); } - - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - - /** - * Accel self test - * - * @return 0 on success, 1 on failure - */ - int accel_self_test(); - - /** - * Gyro self test - * - * @return 0 on success, 1 on failure - */ - int gyro_self_test(); - - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); - - /* - set sample rate (approximate) - 1kHz to 5Hz - */ - void _set_sample_rate(unsigned desired_sample_rate_hz); - - /* - check that key registers still have the right value - */ - void check_registers(void); - - /* do not allow to copy this class due to pointer data members */ - MPU9250(const MPU9250 &); - MPU9250 operator=(const MPU9250 &); - -#pragma pack(push, 1) - /** - * Report conversation within the MPU9250, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - }; -#pragma pack(pop) -}; /* list of registers that will be checked in check_registers(). Note diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h index ecbf894db0..abae2b0310 100644 --- a/src/drivers/mpu9250/mpu9250.h +++ b/src/drivers/mpu9250/mpu9250.h @@ -58,7 +58,7 @@ private: ringbuffer::RingBuffer *_accel_reports; - struct accel_scale _accel_scale; + struct accel_calibration_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; @@ -67,7 +67,7 @@ private: ringbuffer::RingBuffer *_gyro_reports; - struct gyro_scale _gyro_scale; + struct gyro_calibration_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s;