APM:Libraries
AP_InertialSensor_L3G4200D.cpp
Go to the documentation of this file.
1 /*
2  This program is free software: you can redistribute it and/or modify
3  it under the terms of the GNU General Public License as published by
4  the Free Software Foundation, either version 3 of the License, or
5  (at your option) any later version.
6 
7  This program is distributed in the hope that it will be useful,
8  but WITHOUT ANY WARRANTY; without even the implied warranty of
9  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  GNU General Public License for more details.
11 
12  You should have received a copy of the GNU General Public License
13  along with this program. If not, see <http://www.gnu.org/licenses/>.
14  */
15 /*
16  This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer.
17  This combination is available as a cheap 10DOF sensor on ebay
18 
19  This sensor driver is an example only - it should not be used in any
20  serious autopilot as the latencies on I2C prevent good timing at
21  high sample rates. It is useful when doing an initial port of
22  ardupilot to a board where only I2C is available, and a cheap sensor
23  can be used.
24 
25 Datasheets:
26  ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
27  L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf
28 */
29 #include <AP_HAL/AP_HAL.h>
30 
31 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
33 
34 #include <inttypes.h>
35 #include <utility>
36 
37 const extern AP_HAL::HAL &hal;
38 
41 #define ADXL345_ACCELEROMETER_ADDRESS 0x53
42 #define ADXL345_ACCELEROMETER_XL345_DEVID 0xe5
43 #define ADXL345_ACCELEROMETER_ADXLREG_BW_RATE 0x2c
44 #define ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL 0x2d
45 #define ADXL345_ACCELEROMETER_ADXLREG_DATA_FORMAT 0x31
46 #define ADXL345_ACCELEROMETER_ADXLREG_DEVID 0x00
47 #define ADXL345_ACCELEROMETER_ADXLREG_DATAX0 0x32
48 #define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL 0x38
49 #define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM 0x9F
50 #define ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS 0x39
51 
52 // ADXL345 accelerometer scaling
53 // Result will be scaled to 1m/s/s
54 // ADXL345 in Full resolution mode (any g scaling) is 256 counts/g, so scale by 9.81/256 = 0.038320312
55 #define ADXL345_ACCELEROMETER_SCALE_M_S (GRAVITY_MSS / 256.0f)
56 
58 #define L3G4200D_I2C_ADDRESS 0x69
59 
60 #define L3G4200D_REG_WHO_AM_I 0x0f
61 #define L3G4200D_REG_WHO_AM_I_VALUE 0xd3
62 
63 #define L3G4200D_REG_CTRL_REG1 0x20
64 #define L3G4200D_REG_CTRL_REG1_DRBW_800_110 0xf0
65 #define L3G4200D_REG_CTRL_REG1_PD 0x08
66 #define L3G4200D_REG_CTRL_REG1_XYZ_ENABLE 0x07
67 
68 #define L3G4200D_REG_CTRL_REG4 0x23
69 #define L3G4200D_REG_CTRL_REG4_FS_2000 0x30
70 
71 #define L3G4200D_REG_CTRL_REG5 0x24
72 #define L3G4200D_REG_CTRL_REG5_FIFO_EN 0x40
73 
74 #define L3G4200D_REG_FIFO_CTL 0x2e
75 #define L3G4200D_REG_FIFO_CTL_STREAM 0x40
76 
77 #define L3G4200D_REG_FIFO_SRC 0x2f
78 #define L3G4200D_REG_FIFO_SRC_ENTRIES_MASK 0x1f
79 #define L3G4200D_REG_FIFO_SRC_EMPTY 0x20
80 #define L3G4200D_REG_FIFO_SRC_OVERRUN 0x40
81 
82 #define L3G4200D_REG_XL 0x28
83 
84 // this bit is ORd into the register to enable auto-increment mode
85 #define L3G4200D_REG_AUTO_INCREMENT 0x80
86 
87 // L3G4200D Gyroscope scaling
88 // running at 2000 DPS full range, 16 bit signed data, datasheet
89 // specifies 70 mdps per bit
90 #define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f)
91 
92 // constructor
96  , _dev(std::move(dev))
97 {
98 }
99 
101 {
102 }
103 
104 /*
105  detect the sensor
106  */
109 {
110  if (!dev) {
111  return nullptr;
112  }
114  = new AP_InertialSensor_L3G4200D(imu, std::move(dev));
115  if (!sensor || !sensor->_init_sensor()) {
116  delete sensor;
117  return nullptr;
118  }
119 
120  return sensor;
121 }
122 
124 {
126  return false;
127  }
128 
129  // Init the accelerometer
130  uint8_t data = 0;
132  if (data != ADXL345_ACCELEROMETER_XL345_DEVID) {
133  AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find ADXL345 accelerometer sensor");
134  }
135 
137  hal.scheduler->delay(5);
139  hal.scheduler->delay(5);
140  // Measure mode:
142  hal.scheduler->delay(5);
143 
144  // Full resolution, 8g:
145  // Caution, this must agree with ADXL345_ACCELEROMETER_SCALE_1G
146  // In full resoution mode, the scale factor need not change
148  hal.scheduler->delay(5);
149 
150  // Normal power, 800Hz Output Data Rate, 400Hz bandwidth:
152  hal.scheduler->delay(5);
153 
154  // enable FIFO in stream mode. This will allow us to read the accelerometers
155  // much less frequently
158 
159  // Init the Gyro
160  // Expect to read the right 'WHO_AM_I' value
162  if (data != L3G4200D_REG_WHO_AM_I_VALUE) {
163  AP_HAL::panic("AP_InertialSensor_L3G4200D: could not find L3G4200D gyro sensor");
164  }
165 
166  // setup for 800Hz sampling with 110Hz filter
171  hal.scheduler->delay(1);
172 
177  hal.scheduler->delay(1);
178 
183  hal.scheduler->delay(1);
184 
185  // setup for 2000 degrees/sec full range
188  hal.scheduler->delay(1);
189 
190  // enable FIFO
193  hal.scheduler->delay(1);
194 
195  // enable FIFO in stream mode. This will allow us to read the gyros much less frequently
198  hal.scheduler->delay(1);
199 
200  _dev->get_semaphore()->give();
201 
202  return true;
203 }
204 
205 /*
206  startup the sensor
207  */
209 {
212 
213  // start the timer process to read samples
215 }
216 
217 /*
218  copy filtered data to the frontend
219  */
221 {
224 
225  return true;
226 }
227 
228 // Accumulate values from accels and gyros
230 {
231  uint8_t num_samples_available;
232  uint8_t fifo_status = 0;
233 
234  // Read gyro FIFO status
235  fifo_status = 0;
236  _dev->read_registers(L3G4200D_REG_FIFO_SRC, &fifo_status, 1);
237  if (fifo_status & L3G4200D_REG_FIFO_SRC_OVERRUN) {
238  // FIFO is full
239  num_samples_available = 32;
240  } else if (fifo_status & L3G4200D_REG_FIFO_SRC_EMPTY) {
241  // FIFO is empty
242  num_samples_available = 0;
243  } else {
244  // FIFO is partly full
245  num_samples_available = fifo_status & L3G4200D_REG_FIFO_SRC_ENTRIES_MASK;
246  }
247 
248  if (num_samples_available > 0) {
249  // read all the entries in one go, using AUTO_INCREMENT. This saves a lot of time on I2C setup
250  int16_t buffer[num_samples_available][3];
252  (uint8_t *)&buffer, sizeof(buffer))) {
253  for (uint8_t i=0; i < num_samples_available; i++) {
254  Vector3f gyro = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
255  // Adjust for chip scaling to get radians/sec
256  gyro *= L3G4200D_GYRO_SCALE_R_S;
259  }
260  }
261  }
262 
263  // Read accelerometer FIFO to find out how many samples are available
265  &fifo_status, 1);
266  num_samples_available = fifo_status & 0x3F;
267 
268  // read the samples and apply the filter
269  if (num_samples_available > 0) {
270  int16_t buffer[num_samples_available][3];
272  (uint8_t *)buffer, sizeof(buffer[0]),
273  num_samples_available)) {
274  for (uint8_t i=0; i<num_samples_available; i++) {
275  Vector3f accel = Vector3f(buffer[i][0], -buffer[i][1], -buffer[i][2]);
276  // Adjust for chip scaling to get m/s/s
280  }
281  }
282  }
283 }
284 
285 #endif
#define L3G4200D_REG_FIFO_CTL
#define ADXL345_ACCELEROMETER_XL345_DEVID
virtual Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, Device::PeriodicCb) override=0
Vector3< float > Vector3f
Definition: vector3.h:246
#define ADXL345_ACCELEROMETER_ADXLREG_DATA_FORMAT
#define L3G4200D_REG_AUTO_INCREMENT
#define L3G4200D_REG_CTRL_REG1_PD
#define L3G4200D_GYRO_SCALE_R_S
static uint8_t buffer[SRXL_FRAMELEN_MAX]
Definition: srxl.cpp:56
uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id)
#define L3G4200D_REG_CTRL_REG5_FIFO_EN
virtual Semaphore * get_semaphore() override=0
#define L3G4200D_REG_WHO_AM_I
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
#define L3G4200D_REG_CTRL_REG4_FS_2000
#define ADXL345_ACCELEROMETER_ADXLREG_DATAX0
#define HAL_SEMAPHORE_BLOCK_FOREVER
Definition: Semaphores.h:5
virtual bool take(uint32_t timeout_ms) WARN_IF_UNUSED=0
#define L3G4200D_REG_CTRL_REG1
#define L3G4200D_REG_CTRL_REG4
uint32_t get_bus_id_devtype(uint8_t devtype)
Definition: Device.h:255
#define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL_STREAM
AP_HAL::OwnPtr< AP_HAL::Device > _dev
#define ADXL345_ACCELEROMETER_ADXLREG_FIFO_CTL
#define L3G4200D_REG_FIFO_CTL_STREAM
virtual void delay(uint16_t ms)=0
#define L3G4200D_REG_CTRL_REG5
uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id)
#define L3G4200D_REG_FIFO_SRC_OVERRUN
#define L3G4200D_REG_FIFO_SRC_EMPTY
static AP_HAL::OwnPtr< AP_HAL::Device > dev
Definition: ICM20789.cpp:16
#define ADXL345_ACCELEROMETER_ADXLREG_POWER_CTL
#define ADXL345_ACCELEROMETER_ADXLREG_DEVID
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0)
#define L3G4200D_REG_WHO_AM_I_VALUE
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)
virtual bool give()=0
#define ADXL345_ACCELEROMETER_ADXLREG_BW_RATE
virtual bool read_registers_multiple(uint8_t first_reg, uint8_t *recv, uint32_t recv_len, uint8_t times)=0
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false)
#define L3G4200D_REG_XL
#define L3G4200D_REG_FIFO_SRC
#define ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS
AP_HAL::OwnPtr< AP_HAL::I2CDevice > _dev
static AP_InertialSensor_Backend * probe(AP_InertialSensor &imu, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev)
bool read_registers(uint8_t first_reg, uint8_t *recv, uint32_t recv_len)
Definition: Device.h:113
#define FUNCTOR_BIND_MEMBER(func, rettype,...)
Definition: functor.h:31
#define ADXL345_ACCELEROMETER_SCALE_M_S
AP_InertialSensor_L3G4200D(AP_InertialSensor &imu, AP_HAL::OwnPtr< AP_HAL::I2CDevice > dev)
void panic(const char *errormsg,...) FMT_PRINTF(1
Definition: system.cpp:140
#define L3G4200D_REG_CTRL_REG1_DRBW_800_110
bool write_register(uint8_t reg, uint8_t val, bool checked=false)
Definition: Device.h:125
#define L3G4200D_REG_CTRL_REG1_XYZ_ENABLE
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel)
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114
#define L3G4200D_REG_FIFO_SRC_ENTRIES_MASK