forked from Archive/PX4-Autopilot
Added some functions for changing rates etc (WIP)
This commit is contained in:
parent
4703a68979
commit
eb3d6f228c
|
@ -108,8 +108,10 @@ static const int ERROR = -1;
|
||||||
#define ADDR_CTRL_REG3 0x22
|
#define ADDR_CTRL_REG3 0x22
|
||||||
#define ADDR_CTRL_REG4 0x23
|
#define ADDR_CTRL_REG4 0x23
|
||||||
#define ADDR_CTRL_REG5 0x24
|
#define ADDR_CTRL_REG5 0x24
|
||||||
|
#define ADDR_CTRL_REG6 0x25
|
||||||
#define ADDR_CTRL_REG7 0x26
|
#define ADDR_CTRL_REG7 0x26
|
||||||
|
|
||||||
|
#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||||
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
|
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
|
||||||
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||||
#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
|
#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||||
|
@ -127,11 +129,13 @@ static const int ERROR = -1;
|
||||||
#define REG1_Y_ENABLE_A (1<<1)
|
#define REG1_Y_ENABLE_A (1<<1)
|
||||||
#define REG1_X_ENABLE_A (1<<0)
|
#define REG1_X_ENABLE_A (1<<0)
|
||||||
|
|
||||||
|
#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6))
|
||||||
#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
|
#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
|
||||||
#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
|
#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
|
||||||
#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
|
#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
|
||||||
#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
|
#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
|
||||||
|
|
||||||
|
#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3))
|
||||||
#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
|
#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
|
||||||
#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
|
#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
|
||||||
#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
|
#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
|
||||||
|
@ -143,6 +147,7 @@ static const int ERROR = -1;
|
||||||
#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
|
#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
|
||||||
#define REG5_RES_LOW_M ((0<<6) | (0<<5))
|
#define REG5_RES_LOW_M ((0<<6) | (0<<5))
|
||||||
|
|
||||||
|
#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2))
|
||||||
#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
|
#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
|
||||||
#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
|
#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
|
||||||
#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
|
#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
|
||||||
|
@ -151,6 +156,7 @@ static const int ERROR = -1;
|
||||||
#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
|
#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
|
||||||
#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
|
#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
|
||||||
|
|
||||||
|
#define REG6_FULL_SCALE_BITS_M ((1<<7) | (1<<6))
|
||||||
#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6))
|
#define REG6_FULL_SCALE_2GA_M ((0<<7) | (0<<6))
|
||||||
#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6))
|
#define REG6_FULL_SCALE_4GA_M ((0<<7) | (1<<6))
|
||||||
#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6))
|
#define REG6_FULL_SCALE_8GA_M ((1<<7) | (0<<6))
|
||||||
|
@ -174,72 +180,66 @@ public:
|
||||||
LSM303D(int bus, const char* path, spi_dev_e device);
|
LSM303D(int bus, const char* path, spi_dev_e device);
|
||||||
virtual ~LSM303D();
|
virtual ~LSM303D();
|
||||||
|
|
||||||
virtual int init();
|
virtual int init();
|
||||||
|
|
||||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Diagnostics - print some basic information about the driver.
|
* Diagnostics - print some basic information about the driver.
|
||||||
*/
|
*/
|
||||||
void print_info();
|
void print_info();
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
virtual int probe();
|
virtual int probe();
|
||||||
|
|
||||||
friend class LSM303D_mag;
|
friend class LSM303D_mag;
|
||||||
|
|
||||||
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
|
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
|
||||||
virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
|
virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
LSM303D_mag *_mag;
|
LSM303D_mag *_mag;
|
||||||
|
|
||||||
struct hrt_call _accel_call;
|
struct hrt_call _accel_call;
|
||||||
struct hrt_call _mag_call;
|
struct hrt_call _mag_call;
|
||||||
|
|
||||||
unsigned _call_accel_interval;
|
unsigned _call_accel_interval;
|
||||||
unsigned _call_mag_interval;
|
unsigned _call_mag_interval;
|
||||||
|
|
||||||
unsigned _num_accel_reports;
|
unsigned _num_accel_reports;
|
||||||
volatile unsigned _next_accel_report;
|
volatile unsigned _next_accel_report;
|
||||||
volatile unsigned _oldest_accel_report;
|
volatile unsigned _oldest_accel_report;
|
||||||
struct accel_report *_accel_reports;
|
struct accel_report *_accel_reports;
|
||||||
|
|
||||||
struct accel_scale _accel_scale;
|
struct accel_scale _accel_scale;
|
||||||
float _accel_range_scale;
|
float _accel_range_scale;
|
||||||
float _accel_range_m_s2;
|
float _accel_range_m_s2;
|
||||||
orb_advert_t _accel_topic;
|
orb_advert_t _accel_topic;
|
||||||
|
|
||||||
unsigned _num_mag_reports;
|
unsigned _num_mag_reports;
|
||||||
volatile unsigned _next_mag_report;
|
volatile unsigned _next_mag_report;
|
||||||
volatile unsigned _oldest_mag_report;
|
volatile unsigned _oldest_mag_report;
|
||||||
struct mag_report *_mag_reports;
|
struct mag_report *_mag_reports;
|
||||||
|
|
||||||
struct mag_scale _mag_scale;
|
struct mag_scale _mag_scale;
|
||||||
float _mag_range_scale;
|
float _mag_range_scale;
|
||||||
float _mag_range_ga;
|
float _mag_range_ga;
|
||||||
orb_advert_t _mag_topic;
|
orb_advert_t _mag_topic;
|
||||||
|
|
||||||
unsigned _current_accel_rate;
|
|
||||||
unsigned _current_accel_range;
|
|
||||||
|
|
||||||
unsigned _current_mag_rate;
|
|
||||||
unsigned _current_mag_range;
|
|
||||||
|
|
||||||
perf_counter_t _accel_sample_perf;
|
perf_counter_t _accel_sample_perf;
|
||||||
perf_counter_t _mag_sample_perf;
|
perf_counter_t _mag_sample_perf;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Start automatic measurement.
|
* Start automatic measurement.
|
||||||
*/
|
*/
|
||||||
void start();
|
void start();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Stop automatic measurement.
|
* Stop automatic measurement.
|
||||||
*/
|
*/
|
||||||
void stop();
|
void stop();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Static trampoline from the hrt_call context; because we don't have a
|
* Static trampoline from the hrt_call context; because we don't have a
|
||||||
|
@ -250,24 +250,24 @@ private:
|
||||||
*
|
*
|
||||||
* @param arg Instance pointer for the driver that is polling.
|
* @param arg Instance pointer for the driver that is polling.
|
||||||
*/
|
*/
|
||||||
static void measure_trampoline(void *arg);
|
static void measure_trampoline(void *arg);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Static trampoline for the mag because it runs at a lower rate
|
* Static trampoline for the mag because it runs at a lower rate
|
||||||
*
|
*
|
||||||
* @param arg Instance pointer for the driver that is polling.
|
* @param arg Instance pointer for the driver that is polling.
|
||||||
*/
|
*/
|
||||||
static void mag_measure_trampoline(void *arg);
|
static void mag_measure_trampoline(void *arg);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Fetch accel measurements from the sensor and update the report ring.
|
* Fetch accel measurements from the sensor and update the report ring.
|
||||||
*/
|
*/
|
||||||
void measure();
|
void measure();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Fetch mag measurements from the sensor and update the report ring.
|
* Fetch mag measurements from the sensor and update the report ring.
|
||||||
*/
|
*/
|
||||||
void mag_measure();
|
void mag_measure();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Read a register from the LSM303D
|
* Read a register from the LSM303D
|
||||||
|
@ -275,7 +275,7 @@ private:
|
||||||
* @param The register to read.
|
* @param The register to read.
|
||||||
* @return The value that was read.
|
* @return The value that was read.
|
||||||
*/
|
*/
|
||||||
uint8_t read_reg(unsigned reg);
|
uint8_t read_reg(unsigned reg);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Write a register in the LSM303D
|
* Write a register in the LSM303D
|
||||||
|
@ -283,7 +283,7 @@ private:
|
||||||
* @param reg The register to write.
|
* @param reg The register to write.
|
||||||
* @param value The new value to write.
|
* @param value The new value to write.
|
||||||
*/
|
*/
|
||||||
void write_reg(unsigned reg, uint8_t value);
|
void write_reg(unsigned reg, uint8_t value);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Modify a register in the LSM303D
|
* Modify a register in the LSM303D
|
||||||
|
@ -294,27 +294,54 @@ private:
|
||||||
* @param clearbits Bits in the register to clear.
|
* @param clearbits Bits in the register to clear.
|
||||||
* @param setbits Bits in the register to set.
|
* @param setbits Bits in the register to set.
|
||||||
*/
|
*/
|
||||||
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
|
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the LSM303D measurement range.
|
* Set the LSM303D accel measurement range.
|
||||||
*
|
*
|
||||||
* @param max_dps The measurement range is set to permit reading at least
|
* @param max_g The measurement range of the accel is in g (9.81m/s^2)
|
||||||
* this rate in degrees per second.
|
|
||||||
* Zero selects the maximum supported range.
|
* Zero selects the maximum supported range.
|
||||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||||
*/
|
*/
|
||||||
int set_range(unsigned max_dps);
|
int set_range(unsigned max_g);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set the LSM303D internal sampling frequency.
|
* Set the LSM303D mag measurement range.
|
||||||
*
|
*
|
||||||
* @param frequency The internal sampling frequency is set to not less than
|
* @param max_ga The measurement range of the mag is in Ga
|
||||||
|
* Zero selects the maximum supported range.
|
||||||
|
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||||
|
*/
|
||||||
|
int mag_set_range(unsigned max_g);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the LSM303D accel anti-alias filter.
|
||||||
|
*
|
||||||
|
* @param bandwidth The anti-alias filter bandwidth in Hz
|
||||||
|
* Zero selects the highest bandwidth
|
||||||
|
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||||
|
*/
|
||||||
|
int set_antialias_filter_bandwidth(unsigned max_g);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the LSM303D internal accel sampling frequency.
|
||||||
|
*
|
||||||
|
* @param frequency The internal accel sampling frequency is set to not less than
|
||||||
* this value.
|
* this value.
|
||||||
* Zero selects the maximum rate supported.
|
* Zero selects the maximum rate supported.
|
||||||
* @return OK if the value can be supported.
|
* @return OK if the value can be supported.
|
||||||
*/
|
*/
|
||||||
int set_samplerate(unsigned frequency);
|
int set_samplerate(unsigned frequency);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the LSM303D internal mag sampling frequency.
|
||||||
|
*
|
||||||
|
* @param frequency The internal mag sampling frequency is set to not less than
|
||||||
|
* this value.
|
||||||
|
* Zero selects the maximum rate supported.
|
||||||
|
* @return OK if the value can be supported.
|
||||||
|
*/
|
||||||
|
int mag_set_samplerate(unsigned frequency);
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -327,18 +354,18 @@ public:
|
||||||
~LSM303D_mag();
|
~LSM303D_mag();
|
||||||
|
|
||||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
friend class LSM303D;
|
friend class LSM303D;
|
||||||
|
|
||||||
void parent_poll_notify();
|
void parent_poll_notify();
|
||||||
private:
|
private:
|
||||||
LSM303D *_parent;
|
LSM303D *_parent;
|
||||||
|
|
||||||
void measure();
|
void measure();
|
||||||
|
|
||||||
void measure_trampoline(void *arg);
|
void measure_trampoline(void *arg);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -364,19 +391,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
|
||||||
_mag_reports(nullptr),
|
_mag_reports(nullptr),
|
||||||
_mag_range_scale(0.0f),
|
_mag_range_scale(0.0f),
|
||||||
_mag_range_ga(0.0f),
|
_mag_range_ga(0.0f),
|
||||||
_current_accel_rate(0),
|
|
||||||
_current_accel_range(0),
|
|
||||||
_current_mag_rate(0),
|
|
||||||
_current_mag_range(0),
|
|
||||||
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
|
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
|
||||||
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
|
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read"))
|
||||||
{
|
{
|
||||||
// enable debug() calls
|
// enable debug() calls
|
||||||
_debug_enabled = true;
|
_debug_enabled = true;
|
||||||
|
|
||||||
/* XXX fix this default values */
|
_mag_range_scale = 12.0f/32767.0f;
|
||||||
_accel_range_scale = 1.0f;
|
|
||||||
_mag_range_scale = 1.0f;
|
|
||||||
|
|
||||||
// default scale factors
|
// default scale factors
|
||||||
_accel_scale.x_offset = 0;
|
_accel_scale.x_offset = 0;
|
||||||
|
@ -446,18 +467,21 @@ LSM303D::init()
|
||||||
memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
|
memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
|
||||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
|
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
|
||||||
|
|
||||||
/* XXX do this with ioctls */
|
/* enable accel, XXX do this with an ioctl? */
|
||||||
/* set default configuration */
|
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
|
||||||
write_reg(ADDR_CTRL_REG1, REG1_RATE_400HZ_A | REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);
|
|
||||||
|
/* enable mag, XXX do this with an ioctl? */
|
||||||
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
|
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
|
||||||
write_reg(ADDR_CTRL_REG5, REG5_RATE_100HZ_M | REG5_RES_HIGH_M);
|
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
|
||||||
|
|
||||||
/* XXX should we enable FIFO */
|
/* XXX should we enable FIFO? */
|
||||||
|
|
||||||
set_range(500); /* default to 500dps */
|
set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
|
||||||
set_samplerate(0); /* max sample rate */
|
set_antialias_filter_bandwidth(194); /* XXX: choose bandwidth: 50, 194, 362 or 773 Hz */
|
||||||
|
set_samplerate(400); /* max sample rate */
|
||||||
|
|
||||||
// _current_accel_rate = 100;
|
mag_set_range(12); /* XXX: take highest sensor range of 12GA? */
|
||||||
|
mag_set_samplerate(100);
|
||||||
|
|
||||||
/* XXX test this when another mag is used */
|
/* XXX test this when another mag is used */
|
||||||
/* do CDev init for the mag device node, keep it optional */
|
/* do CDev init for the mag device node, keep it optional */
|
||||||
|
@ -590,9 +614,13 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
/* set default/max polling rate */
|
/* set default/max polling rate */
|
||||||
case SENSOR_POLLRATE_MAX:
|
case SENSOR_POLLRATE_MAX:
|
||||||
|
|
||||||
|
return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
|
||||||
|
|
||||||
case SENSOR_POLLRATE_DEFAULT:
|
case SENSOR_POLLRATE_DEFAULT:
|
||||||
/* With internal low pass filters enabled, 250 Hz is sufficient */
|
/* With internal low pass filters enabled, 250 Hz is sufficient */
|
||||||
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
|
/* XXX for vibration tests with 800 Hz */
|
||||||
|
return ioctl(filp, SENSORIOCSPOLLRATE, 800);
|
||||||
|
|
||||||
/* adjust to a legal polling interval in Hz */
|
/* adjust to a legal polling interval in Hz */
|
||||||
default: {
|
default: {
|
||||||
|
@ -606,6 +634,9 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
if (ticks < 1000)
|
if (ticks < 1000)
|
||||||
return -EINVAL;
|
return -EINVAL;
|
||||||
|
|
||||||
|
/* adjust sample rate of sensor */
|
||||||
|
set_samplerate(arg);
|
||||||
|
|
||||||
/* update interval for next measurement */
|
/* update interval for next measurement */
|
||||||
/* XXX this is a bit shady, but no other way to adjust... */
|
/* XXX this is a bit shady, but no other way to adjust... */
|
||||||
_accel_call.period = _call_accel_interval = ticks;
|
_accel_call.period = _call_accel_interval = ticks;
|
||||||
|
@ -801,33 +832,111 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
LSM303D::set_range(unsigned max_dps)
|
LSM303D::set_range(unsigned max_g)
|
||||||
{
|
{
|
||||||
/* XXX implement this */
|
uint8_t setbits = 0;
|
||||||
// uint8_t bits = REG4_BDU;
|
uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
|
||||||
//
|
float new_range = 0.0f;
|
||||||
// if (max_dps == 0)
|
|
||||||
// max_dps = 2000;
|
if (max_g == 0)
|
||||||
//
|
max_g = 16;
|
||||||
// if (max_dps <= 250) {
|
|
||||||
// _current_range = 250;
|
if (max_g <= 2) {
|
||||||
// bits |= RANGE_250DPS;
|
new_range = 2.0f;
|
||||||
//
|
setbits |= REG2_FULL_SCALE_2G_A;
|
||||||
// } else if (max_dps <= 500) {
|
|
||||||
// _current_range = 500;
|
} else if (max_g <= 4) {
|
||||||
// bits |= RANGE_500DPS;
|
new_range = 4.0f;
|
||||||
//
|
setbits |= REG2_FULL_SCALE_4G_A;
|
||||||
// } else if (max_dps <= 2000) {
|
|
||||||
// _current_range = 2000;
|
} else if (max_g <= 6) {
|
||||||
// bits |= RANGE_2000DPS;
|
new_range = 6.0f;
|
||||||
//
|
setbits |= REG2_FULL_SCALE_6G_A;
|
||||||
// } else {
|
|
||||||
// return -EINVAL;
|
} else if (max_g <= 8) {
|
||||||
// }
|
new_range = 8.0f;
|
||||||
//
|
setbits |= REG2_FULL_SCALE_8G_A;
|
||||||
// _gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
|
|
||||||
// _gyro_range_scale = _gyro_range_rad_s / 32768.0f;
|
} else if (max_g <= 16) {
|
||||||
// write_reg(ADDR_CTRL_REG4, bits);
|
new_range = 16.0f;
|
||||||
|
setbits |= REG2_FULL_SCALE_16G_A;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
_accel_range_m_s2 = new_range * 9.80665f;
|
||||||
|
_accel_range_scale = _accel_range_m_s2 / 32768.0f;
|
||||||
|
|
||||||
|
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
|
||||||
|
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LSM303D::mag_set_range(unsigned max_ga)
|
||||||
|
{
|
||||||
|
uint8_t setbits = 0;
|
||||||
|
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
|
||||||
|
float new_range = 0.0f;
|
||||||
|
|
||||||
|
if (max_ga == 0)
|
||||||
|
max_ga = 12;
|
||||||
|
|
||||||
|
if (max_ga <= 2) {
|
||||||
|
new_range = 2.0f;
|
||||||
|
setbits |= REG6_FULL_SCALE_2GA_M;
|
||||||
|
|
||||||
|
} else if (max_ga <= 4) {
|
||||||
|
new_range = 4.0f;
|
||||||
|
setbits |= REG6_FULL_SCALE_4GA_M;
|
||||||
|
|
||||||
|
} else if (max_ga <= 8) {
|
||||||
|
new_range = 8.0f;
|
||||||
|
setbits |= REG6_FULL_SCALE_8GA_M;
|
||||||
|
|
||||||
|
} else if (max_ga <= 12) {
|
||||||
|
new_range = 12.0f;
|
||||||
|
setbits |= REG6_FULL_SCALE_12GA_M;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
_mag_range_ga = new_range;
|
||||||
|
_mag_range_scale = _mag_range_ga / 32768.0f;
|
||||||
|
|
||||||
|
modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
|
||||||
|
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
|
||||||
|
{
|
||||||
|
uint8_t setbits = 0;
|
||||||
|
uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
|
||||||
|
|
||||||
|
if (bandwidth == 0)
|
||||||
|
bandwidth = 773;
|
||||||
|
|
||||||
|
if (bandwidth <= 50) {
|
||||||
|
setbits |= REG2_AA_FILTER_BW_50HZ_A;
|
||||||
|
|
||||||
|
} else if (bandwidth <= 194) {
|
||||||
|
setbits |= REG2_AA_FILTER_BW_194HZ_A;
|
||||||
|
|
||||||
|
} else if (bandwidth <= 362) {
|
||||||
|
setbits |= REG2_AA_FILTER_BW_362HZ_A;
|
||||||
|
|
||||||
|
} else if (bandwidth <= 773) {
|
||||||
|
setbits |= REG2_AA_FILTER_BW_773HZ_A;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
@ -835,33 +944,60 @@ LSM303D::set_range(unsigned max_dps)
|
||||||
int
|
int
|
||||||
LSM303D::set_samplerate(unsigned frequency)
|
LSM303D::set_samplerate(unsigned frequency)
|
||||||
{
|
{
|
||||||
/* XXX implement this */
|
uint8_t setbits = 0;
|
||||||
// uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
|
uint8_t clearbits = REG1_RATE_BITS_A;
|
||||||
//
|
|
||||||
// if (frequency == 0)
|
if (frequency == 0)
|
||||||
// frequency = 760;
|
frequency = 1600;
|
||||||
//
|
|
||||||
// if (frequency <= 95) {
|
if (frequency <= 100) {
|
||||||
// _current_rate = 95;
|
setbits |= REG1_RATE_100HZ_A;
|
||||||
// bits |= RATE_95HZ_LP_25HZ;
|
|
||||||
//
|
} else if (frequency <= 200) {
|
||||||
// } else if (frequency <= 190) {
|
setbits |= REG1_RATE_200HZ_A;
|
||||||
// _current_rate = 190;
|
|
||||||
// bits |= RATE_190HZ_LP_25HZ;
|
} else if (frequency <= 400) {
|
||||||
//
|
setbits |= REG1_RATE_400HZ_A;
|
||||||
// } else if (frequency <= 380) {
|
|
||||||
// _current_rate = 380;
|
} else if (frequency <= 800) {
|
||||||
// bits |= RATE_380HZ_LP_30HZ;
|
setbits |= REG1_RATE_800HZ_A;
|
||||||
//
|
|
||||||
// } else if (frequency <= 760) {
|
} else if (frequency <= 1600) {
|
||||||
// _current_rate = 760;
|
setbits |= REG1_RATE_1600HZ_A;
|
||||||
// bits |= RATE_760HZ_LP_30HZ;
|
|
||||||
//
|
} else {
|
||||||
// } else {
|
return -EINVAL;
|
||||||
// return -EINVAL;
|
}
|
||||||
// }
|
|
||||||
//
|
modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
|
||||||
// write_reg(ADDR_CTRL_REG1, bits);
|
|
||||||
|
return OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
LSM303D::mag_set_samplerate(unsigned frequency)
|
||||||
|
{
|
||||||
|
uint8_t setbits = 0;
|
||||||
|
uint8_t clearbits = REG5_RATE_BITS_M;
|
||||||
|
|
||||||
|
if (frequency == 0)
|
||||||
|
frequency = 100;
|
||||||
|
|
||||||
|
if (frequency <= 25) {
|
||||||
|
setbits |= REG5_RATE_25HZ_M;
|
||||||
|
|
||||||
|
} else if (frequency <= 50) {
|
||||||
|
setbits |= REG5_RATE_50HZ_M;
|
||||||
|
|
||||||
|
} else if (frequency <= 100) {
|
||||||
|
setbits |= REG5_RATE_100HZ_M;
|
||||||
|
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return -EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
modify_reg(ADDR_CTRL_REG5, clearbits, setbits);
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
@ -956,8 +1092,8 @@ LSM303D::measure()
|
||||||
accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
accel_report->x = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||||
accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
accel_report->y = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||||
accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
accel_report->z = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||||
// report->scaling = _gyro_range_scale;
|
accel_report->scaling = _accel_range_scale;
|
||||||
// report->range_rad_s = _gyro_range_rad_s;
|
accel_report->range_m_s2 = _accel_range_m_s2;
|
||||||
|
|
||||||
/* post a report to the ring - note, not locked */
|
/* post a report to the ring - note, not locked */
|
||||||
INCREMENT(_next_accel_report, _num_accel_reports);
|
INCREMENT(_next_accel_report, _num_accel_reports);
|
||||||
|
@ -1183,15 +1319,13 @@ test()
|
||||||
if (sz != sizeof(a_report))
|
if (sz != sizeof(a_report))
|
||||||
err(1, "immediate read failed");
|
err(1, "immediate read failed");
|
||||||
|
|
||||||
/* XXX fix the test output */
|
warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
|
||||||
// warnx("accel x: \t% 9.5f\tm/s^2", (double)a_report.x);
|
warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
|
||||||
// warnx("accel y: \t% 9.5f\tm/s^2", (double)a_report.y);
|
warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
|
||||||
// warnx("accel z: \t% 9.5f\tm/s^2", (double)a_report.z);
|
|
||||||
warnx("accel x: \t%d\traw", (int)a_report.x_raw);
|
warnx("accel x: \t%d\traw", (int)a_report.x_raw);
|
||||||
warnx("accel y: \t%d\traw", (int)a_report.y_raw);
|
warnx("accel y: \t%d\traw", (int)a_report.y_raw);
|
||||||
warnx("accel z: \t%d\traw", (int)a_report.z_raw);
|
warnx("accel z: \t%d\traw", (int)a_report.z_raw);
|
||||||
// warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
|
warnx("accel range: %8.4f m/s^2", (double)a_report.range_m_s2);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int fd_mag = -1;
|
int fd_mag = -1;
|
||||||
|
@ -1209,10 +1343,13 @@ test()
|
||||||
if (sz != sizeof(m_report))
|
if (sz != sizeof(m_report))
|
||||||
err(1, "immediate read failed");
|
err(1, "immediate read failed");
|
||||||
|
|
||||||
/* XXX fix the test output */
|
warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
|
||||||
|
warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
|
||||||
|
warnx("mag z: \t% 9.5f\tga", (double)m_report.z);
|
||||||
warnx("mag x: \t%d\traw", (int)m_report.x_raw);
|
warnx("mag x: \t%d\traw", (int)m_report.x_raw);
|
||||||
warnx("mag y: \t%d\traw", (int)m_report.y_raw);
|
warnx("mag y: \t%d\traw", (int)m_report.y_raw);
|
||||||
warnx("mag z: \t%d\traw", (int)m_report.z_raw);
|
warnx("mag z: \t%d\traw", (int)m_report.z_raw);
|
||||||
|
warnx("mag range: %8.4f ga", (double)m_report.range_ga);
|
||||||
|
|
||||||
/* XXX add poll-rate tests here too */
|
/* XXX add poll-rate tests here too */
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue