mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 05:23:34 +08:00
drivers: some fixes for the calibration refactor
This commit is contained in:
Julian Oes
@@ -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:
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user