Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 1134 lines (1032 sloc) 44.529 kB
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
2 // board orientation and setup
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
3 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
4 //default board orientation
5 #if !defined(ACC_ORIENTATION)
6 #define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = X; accADC[PITCH] = Y; accADC[YAW] = Z;}
7 #endif
8 #if !defined(GYRO_ORIENTATION)
9 #define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;}
10 #endif
11 #if !defined(MAG_ORIENTATION)
12 #define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;}
13 #endif
14
15 /*** I2C address ***/
16 #if !defined(ADXL345_ADDRESS)
17 #define ADXL345_ADDRESS 0x3A
18 //#define ADXL345_ADDRESS 0xA6 //WARNING: Conflicts with a Wii Motion plus!
19 #endif
20
21 #if !defined(BMA180_ADDRESS)
22 #define BMA180_ADDRESS 0x80
23 //#define BMA180_ADDRESS 0x82
24 #endif
25
26 #if !defined(ITG3200_ADDRESS)
27 #define ITG3200_ADDRESS 0XD0
28 //#define ITG3200_ADDRESS 0XD2
29 #endif
30
88a4c79 MPU6050 first code:
dubusal@gmail.com authored
31 #if !defined(MPU6050_ADDRESS)
32 #define MPU6050_ADDRESS 0xD0 // address pin AD0 low (GND), default for FreeIMU v0.4 and InvenSense evaluation board
33 //#define MPU6050_ADDRESS 0xD2 // address pin AD0 high (VCC)
34 #endif
35
377cfd3 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@204 026…
dubusal@gmail.com authored
36 #if !defined(MS561101BA_ADDRESS)
37 #define MS561101BA_ADDRESS 0xEE //CBR=0 0xEE I2C address when pin CSB is connected to LOW (GND)
38 //#define MS561101BA_ADDRESS 0xEF //CBR=1 0xEF I2C address when pin CSB is connected to HIGH (VCC)
39 #endif
40
3ad3115 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@217 026…
dubusal@gmail.com authored
41 //ITG3200 and ITG3205 Gyro LPF setting
42 #if defined(ITG3200_LPF_256HZ) || defined(ITG3200_LPF_188HZ) || defined(ITG3200_LPF_98HZ) || defined(ITG3200_LPF_42HZ) || defined(ITG3200_LPF_20HZ) || defined(ITG3200_LPF_10HZ)
43 #if defined(ITG3200_LPF_256HZ)
44 #define ITG3200_SMPLRT_DIV 0 //8000Hz
45 #define ITG3200_DLPF_CFG 0
46 #endif
47 #if defined(ITG3200_LPF_188HZ)
48 #define ITG3200_SMPLRT_DIV 0 //1000Hz
49 #define ITG3200_DLPF_CFG 1
50 #endif
51 #if defined(ITG3200_LPF_98HZ)
52 #define ITG3200_SMPLRT_DIV 0
53 #define ITG3200_DLPF_CFG 2
54 #endif
55 #if defined(ITG3200_LPF_42HZ)
56 #define ITG3200_SMPLRT_DIV 0
57 #define ITG3200_DLPF_CFG 3
58 #endif
59 #if defined(ITG3200_LPF_20HZ)
60 #define ITG3200_SMPLRT_DIV 0
61 #define ITG3200_DLPF_CFG 4
62 #endif
63 #if defined(ITG3200_LPF_10HZ)
64 #define ITG3200_SMPLRT_DIV 0
65 #define ITG3200_DLPF_CFG 5
66 #endif
67 #else
68 //Default settings LPF 256Hz/8000Hz sample
69 #define ITG3200_SMPLRT_DIV 0 //8000Hz
70 #define ITG3200_DLPF_CFG 0
71 #endif
72
88a4c79 MPU6050 first code:
dubusal@gmail.com authored
73 //MPU6050 Gyro LPF setting
74 #if defined(MPU6050_LPF_256HZ) || defined(MPU6050_LPF_188HZ) || defined(MPU6050_LPF_98HZ) || defined(MPU6050_LPF_42HZ) || defined(MPU6050_LPF_20HZ) || defined(MPU6050_LPF_10HZ)
75 #if defined(MPU6050_LPF_256HZ)
76 #define MPU6050_SMPLRT_DIV 0 //8000Hz
77 #define MPU6050_DLPF_CFG 0
78 #endif
79 #if defined(MPU6050_LPF_188HZ)
80 #define MPU6050_SMPLRT_DIV 0 //1000Hz
81 #define MPU6050_DLPF_CFG 1
82 #endif
83 #if defined(MPU6050_LPF_98HZ)
84 #define MPU6050_SMPLRT_DIV 0
85 #define MPU6050_DLPF_CFG 2
86 #endif
87 #if defined(MPU6050_LPF_42HZ)
88 #define MPU6050_SMPLRT_DIV 0
89 #define MPU6050_DLPF_CFG 3
90 #endif
91 #if defined(MPU6050_LPF_20HZ)
92 #define MPU6050_SMPLRT_DIV 0
93 #define MPU6050_DLPF_CFG 4
94 #endif
95 #if defined(MPU6050_LPF_10HZ)
96 #define MPU6050_SMPLRT_DIV 0
97 #define MPU6050_DLPF_CFG 5
98 #endif
99 #else
100 //Default settings LPF 256Hz/8000Hz sample
101 #define MPU6050_SMPLRT_DIV 0 //8000Hz
102 #define MPU6050_DLPF_CFG 0
103 #endif
104
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
105 uint8_t rawADC[6];
106 static uint32_t neutralizeTime = 0;
107
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
108 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
109 // I2C general functions
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
110 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
111
112 void i2c_init(void) {
113 #if defined(INTERNAL_I2C_PULLUPS)
114 I2C_PULLUPS_ENABLE
115 #else
116 I2C_PULLUPS_DISABLE
117 #endif
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
118 TWSR = 0; // no prescaler => prescaler = 1
119 TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
120 TWCR = 1<<TWEN; // enable twi module, no interrupt
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
121 }
122
123 void i2c_rep_start(uint8_t address) {
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
124 TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) ; // send REPEAT START condition
125 waitTransmissionI2C(); // wait until transmission completed
126 TWDR = address; // send device address
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
127 TWCR = (1<<TWINT) | (1<<TWEN);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
128 waitTransmissionI2C(); // wail until transmission completed
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
129 }
130
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
131 void i2c_stop(void) {
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
132 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
133 // while(TWCR & (1<<TWSTO)); // <- can produce a blocking state with some WMP clones
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
134 }
135
136 void i2c_write(uint8_t data ) {
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
137 TWDR = data; // send data to the previously addressed device
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
138 TWCR = (1<<TWINT) | (1<<TWEN);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
139 waitTransmissionI2C();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
140 }
141
142 uint8_t i2c_readAck() {
143 TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
144 waitTransmissionI2C();
145 return TWDR;
146 }
147
148 uint8_t i2c_readNak(void) {
149 TWCR = (1<<TWINT) | (1<<TWEN);
150 waitTransmissionI2C();
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
151 uint8_t r = TWDR;
152 i2c_stop();
153 return r;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
154 }
155
156 void waitTransmissionI2C() {
cbf740f small bug correction:
dubusal@gmail.com authored
157 uint16_t count = 255;
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
158 while (!(TWCR & (1<<TWINT))) {
159 count--;
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
160 if (count==0) { //we are in a blocking state => we don't insist
161 TWCR = 0; //and we force a reset on TWINT register
162 neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay
26e0677 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@297 026…
dubusal@gmail.com authored
163 i2c_errors_count++;
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
164 break;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
165 }
166 }
167 }
168
169 void i2c_getSixRawADC(uint8_t add, uint8_t reg) {
170 i2c_rep_start(add);
171 i2c_write(reg); // Start multiple read at the reg register
172 i2c_rep_start(add +1); // I2C read direction => I2C address + 1
3ddd3eb - integration of SIRIUS 9DOF + baro IMU
dubusal@gmail.com authored
173 for(uint8_t i = 0; i < 5; i++)
174 rawADC[i]=i2c_readAck();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
175 rawADC[5]= i2c_readNak();
176 }
177
178 void i2c_writeReg(uint8_t add, uint8_t reg, uint8_t val) {
179 i2c_rep_start(add+0); // I2C write direction
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
180 i2c_write(reg); // register selection
181 i2c_write(val); // value to write in register
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
182 i2c_stop();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
183 }
184
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
185 uint8_t i2c_readReg(uint8_t add, uint8_t reg) {
186 i2c_rep_start(add+0); // I2C write direction
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
187 i2c_write(reg); // register selection
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
188 i2c_rep_start(add+1); // I2C read direction
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
189 return i2c_readNak(); // Read single register and return value
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
190 }
191
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
192 // ****************
193 // GYRO common part
194 // ****************
195 void GYRO_Common() {
196 static int16_t previousGyroADC[3] = {0,0,0};
197 static int32_t g[3];
198 uint8_t axis;
199
200 if (calibratingG>0) {
201 for (axis = 0; axis < 3; axis++) {
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
202 // Reset g[axis] at start of calibration
203 if (calibratingG == 400) g[axis]=0;
204 // Sum up 400 readings
205 g[axis] +=gyroADC[axis];
206 // Clear global variables for next reading
207 gyroADC[axis]=0;
208 gyroZero[axis]=0;
209 if (calibratingG == 1) {
210 gyroZero[axis]=g[axis]/400;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
211 blinkLED(10,15,1+3*nunchuk);
212 }
213 }
214 calibratingG--;
215 }
216 for (axis = 0; axis < 3; axis++) {
217 gyroADC[axis] -= gyroZero[axis];
9e2b8ba new supported sensor board ATAVRSBIN1
dubusal@gmail.com authored
218 //anti gyro glitch, limit the variation between two consecutive readings
219 gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
220 previousGyroADC[axis] = gyroADC[axis];
221 }
222 }
223
224 // ****************
225 // ACC common part
226 // ****************
227 void ACC_Common() {
228 static int32_t a[3];
229
230 if (calibratingA>0) {
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
231 for (uint8_t axis = 0; axis < 3; axis++) {
232 // Reset a[axis] at start of calibration
233 if (calibratingA == 400) a[axis]=0;
234 // Sum up 400 readings
235 a[axis] +=accADC[axis];
236 // Clear global variables for next reading
237 accADC[axis]=0;
238 accZero[axis]=0;
239 }
240 // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
241 if (calibratingA == 1) {
242 accZero[ROLL] = a[ROLL]/400;
243 accZero[PITCH] = a[PITCH]/400;
244 accZero[YAW] = a[YAW]/400-acc_1G; // for nunchuk 200=1G
6460a33 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@214 026…
dubusal@gmail.com authored
245 accTrim[ROLL] = 0;
246 accTrim[PITCH] = 0;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
247 writeParams(); // write accZero in EEPROM
248 }
249 calibratingA--;
250 }
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
251 #if defined(InflightAccCalibration)
252 static int32_t b[3];
253 static int16_t accZero_saved[3] = {0,0,0};
254 static int16_t accTrim_saved[2] = {0, 0};
255 //Saving old zeropoints before measurement
256 if (InflightcalibratingA==50) {
257 accZero_saved[ROLL] = accZero[ROLL] ;
258 accZero_saved[PITCH] = accZero[PITCH];
259 accZero_saved[YAW] = accZero[YAW] ;
260 accTrim_saved[ROLL] = accTrim[ROLL] ;
261 accTrim_saved[PITCH] = accTrim[PITCH] ;
262 }
263 if (InflightcalibratingA>0) {
264 for (uint8_t axis = 0; axis < 3; axis++) {
265 // Reset a[axis] at start of calibration
266 if (InflightcalibratingA == 50) b[axis]=0;
267 // Sum up 50 readings
268 b[axis] +=accADC[axis];
269 // Clear global variables for next reading
270 accADC[axis]=0;
271 accZero[axis]=0;
272 }
273 //all values are measured
274 if (InflightcalibratingA == 1) {
275 AccInflightCalibrationMeasurementDone = 1;
276 blinkLED(10,10,2); //buzzer for indicatiing the start inflight
277 // recover saved values to maintain current flight behavior until new values are transferred
278 accZero[ROLL] = accZero_saved[ROLL] ;
279 accZero[PITCH] = accZero_saved[PITCH];
280 accZero[YAW] = accZero_saved[YAW] ;
281 accTrim[ROLL] = accTrim_saved[ROLL] ;
282 accTrim[PITCH] = accTrim_saved[PITCH] ;
283 }
284 InflightcalibratingA--;
285 }
286 // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
287 if (AccInflightCalibrationSavetoEEProm == 1){ //the copter is landed, disarmed and the combo has been done again
288 AccInflightCalibrationSavetoEEProm = 0;
289 accZero[ROLL] = b[ROLL]/50;
290 accZero[PITCH] = b[PITCH]/50;
291 accZero[YAW] = b[YAW]/50-acc_1G; // for nunchuk 200=1G
292 accTrim[ROLL] = 0;
293 accTrim[PITCH] = 0;
294 writeParams(); // write accZero in EEPROM
295 }
296 #endif
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
297 accADC[ROLL] -= accZero[ROLL] ;
298 accADC[PITCH] -= accZero[PITCH];
299 accADC[YAW] -= accZero[YAW] ;
300 }
301
302
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
303 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
304 // I2C Barometer BOSCH BMP085
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
305 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
306 // I2C adress: 0xEE (8bit) 0x77 (7bit)
307 // principle:
308 // 1) read the calibration register (only once at the initialization)
309 // 2) read uncompensated temperature (not mandatory at every cycle)
310 // 3) read uncompensated pressure
311 // 4) raw temp + raw pressure => calculation of the adjusted pressure
312 // the following code uses the maximum precision setting (oversampling setting 3)
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
313 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
314
315 #if defined(BMP085)
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
316 #define BMP085_ADDRESS 0xEE
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
317 static struct {
318 // sensor registers from the BOSCH BMP085 datasheet
319 int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
320 uint16_t ac4, ac5, ac6;
321 union {uint16_t val; uint8_t raw[2]; } ut; //uncompensated T
322 union {uint32_t val; uint8_t raw[4]; } up; //uncompensated P
323 uint8_t state;
324 uint32_t deadline;
325 } bmp085_ctx;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
326 #define OSS 3
327
328 void i2c_BMP085_readCalibration(){
329 delay(10);
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
330 bmp085_ctx.ac1 = i2c_BMP085_readIntRegister(0xAA);
331 bmp085_ctx.ac2 = i2c_BMP085_readIntRegister(0xAC);
332 bmp085_ctx.ac3 = i2c_BMP085_readIntRegister(0xAE);
333 bmp085_ctx.ac4 = i2c_BMP085_readIntRegister(0xB0);
334 bmp085_ctx.ac5 = i2c_BMP085_readIntRegister(0xB2);
335 bmp085_ctx.ac6 = i2c_BMP085_readIntRegister(0xB4);
336 bmp085_ctx.b1 = i2c_BMP085_readIntRegister(0xB6);
337 bmp085_ctx.b2 = i2c_BMP085_readIntRegister(0xB8);
338 bmp085_ctx.mb = i2c_BMP085_readIntRegister(0xBA);
339 bmp085_ctx.mc = i2c_BMP085_readIntRegister(0xBC);
340 bmp085_ctx.md = i2c_BMP085_readIntRegister(0xBE);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
341 }
342
343 void Baro_init() {
344 delay(10);
345 i2c_BMP085_readCalibration();
346 i2c_BMP085_UT_Start();
347 delay(5);
348 i2c_BMP085_UT_Read();
349 }
350
351 // read a 16 bit register
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
352 int16_t i2c_BMP085_readIntRegister(uint8_t r) {
353 union {int16_t val; uint8_t raw[2]; } data;
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
354 i2c_rep_start(BMP085_ADDRESS + 0);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
355 i2c_write(r);
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
356 i2c_rep_start(BMP085_ADDRESS + 1);//I2C read direction => 1
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
357 data.raw[1] = i2c_readAck();
358 data.raw[0] = i2c_readNak();
359 return data.val;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
360 }
361
362 // read uncompensated temperature value: send command first
363 void i2c_BMP085_UT_Start() {
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
364 i2c_writeReg(BMP085_ADDRESS,0xf4,0x2e);
365 i2c_rep_start(BMP085_ADDRESS + 0);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
366 i2c_write(0xF6);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
367 i2c_stop();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
368 }
369
370 // read uncompensated pressure value: send command first
371 void i2c_BMP085_UP_Start () {
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
372 i2c_writeReg(BMP085_ADDRESS,0xf4,0x34+(OSS<<6)); // control register value for oversampling setting 3
373 i2c_rep_start(BMP085_ADDRESS + 0); //I2C write direction => 0
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
374 i2c_write(0xF6);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
375 i2c_stop();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
376 }
377
378 // read uncompensated pressure value: read result bytes
379 // the datasheet suggests a delay of 25.5 ms (oversampling settings 3) after the send command
380 void i2c_BMP085_UP_Read () {
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
381 i2c_rep_start(BMP085_ADDRESS + 1);//I2C read direction => 1
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
382 bmp085_ctx.up.raw[2] = i2c_readAck();
383 bmp085_ctx.up.raw[1] = i2c_readAck();
384 bmp085_ctx.up.raw[0] = i2c_readNak();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
385 }
386
387 // read uncompensated temperature value: read result bytes
388 // the datasheet suggests a delay of 4.5 ms after the send command
389 void i2c_BMP085_UT_Read() {
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
390 i2c_rep_start(BMP085_ADDRESS + 1);//I2C read direction => 1
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
391 bmp085_ctx.ut.raw[1] = i2c_readAck();
392 bmp085_ctx.ut.raw[0] = i2c_readNak();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
393 }
394
395 void i2c_BMP085_Calculate() {
eb5b7d4 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@199 026…
dubusal@gmail.com authored
396 int32_t x1, x2, x3, b3, b5, b6, p, tmp;
397 uint32_t b4, b7;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
398 // Temperature calculations
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
399 x1 = ((int32_t)bmp085_ctx.ut.val - bmp085_ctx.ac6) * bmp085_ctx.ac5 >> 15;
400 x2 = ((int32_t)bmp085_ctx.mc << 11) / (x1 + bmp085_ctx.md);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
401 b5 = x1 + x2;
402 // Pressure calculations
403 b6 = b5 - 4000;
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
404 x1 = (bmp085_ctx.b2 * (b6 * b6 >> 12)) >> 11;
405 x2 = bmp085_ctx.ac2 * b6 >> 11;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
406 x3 = x1 + x2;
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
407 tmp = bmp085_ctx.ac1;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
408 tmp = (tmp*4 + x3) << OSS;
409 b3 = (tmp+2)/4;
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
410 x1 = bmp085_ctx.ac3 * b6 >> 13;
411 x2 = (bmp085_ctx.b1 * (b6 * b6 >> 12)) >> 16;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
412 x3 = ((x1 + x2) + 2) >> 2;
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
413 b4 = (bmp085_ctx.ac4 * (uint32_t)(x3 + 32768)) >> 15;
414 b7 = ((uint32_t) (bmp085_ctx.up.val >> (8-OSS)) - b3) * (50000 >> OSS);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
415 p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
416 x1 = (p >> 8) * (p >> 8);
417 x1 = (x1 * 3038) >> 16;
418 x2 = (-7357 * p) >> 16;
419 pressure = p + ((x1 + x2 + 3791) >> 4);
420 }
421
422 void Baro_update() {
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
423 if (currentTime < bmp085_ctx.deadline) return;
424 bmp085_ctx.deadline = currentTime;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
425 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, BMP085 is ok with this speed
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
426 switch (bmp085_ctx.state) {
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
427 case 0:
428 i2c_BMP085_UT_Start();
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
429 bmp085_ctx.state++; bmp085_ctx.deadline += 4600;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
430 break;
431 case 1:
432 i2c_BMP085_UT_Read();
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
433 bmp085_ctx.state++;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
434 break;
435 case 2:
436 i2c_BMP085_UP_Start();
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
437 bmp085_ctx.state++; bmp085_ctx.deadline += 26000;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
438 break;
439 case 3:
440 i2c_BMP085_UP_Read();
441 i2c_BMP085_Calculate();
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
442 BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 443300.0f; //decimeter
84a3e9d - many code optimizations
dubusal@gmail.com authored
443 bmp085_ctx.state = 0; bmp085_ctx.deadline += 20000;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
444 break;
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
445 }
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
446 }
447 #endif
448
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
449 // ************************************************************************************************************
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
450 // I2C Barometer MS561101BA
451 // ************************************************************************************************************
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
452 // first contribution from Fabio
453 // modification from Alex (September 2011)
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
454 //
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
455 // specs are here: http://www.meas-spec.com/downloads/MS5611-01BA03.pdf
456 // useful info on pages 7 -> 12
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
457 #if defined(MS561101BA)
458
459 // registers of the device
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
460 #define MS561101BA_PRESSURE 0x40
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
461 #define MS561101BA_TEMPERATURE 0x50
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
462 #define MS561101BA_RESET 0x1E
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
463
464 // OSR (Over Sampling Ratio) constants
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
465 #define MS561101BA_OSR_256 0x00
466 #define MS561101BA_OSR_512 0x02
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
467 #define MS561101BA_OSR_1024 0x04
468 #define MS561101BA_OSR_2048 0x06
469 #define MS561101BA_OSR_4096 0x08
470
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
471 #define OSR MS561101BA_OSR_4096
472
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
473 static struct {
474 // sensor registers from the MS561101BA datasheet
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
475 uint16_t c[7];
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
476 union {uint32_t val; uint8_t raw[4]; } ut; //uncompensated T
477 union {uint32_t val; uint8_t raw[4]; } up; //uncompensated P
478 uint8_t state;
479 uint32_t deadline;
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
480 } ms561101ba_ctx;
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
481
482 void i2c_MS561101BA_reset(){
483 i2c_writeReg(MS561101BA_ADDRESS, MS561101BA_RESET, 0);
484 }
485
486 void i2c_MS561101BA_readCalibration(){
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
487 union {uint16_t val; uint8_t raw[2]; } data;
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
488 delay(10);
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
489 for(uint8_t i=0;i<6;i++) {
490 i2c_rep_start(MS561101BA_ADDRESS + 0);
491 i2c_write(0xA2+2*i);
492 i2c_rep_start(MS561101BA_ADDRESS + 1);//I2C read direction => 1
493 data.raw[1] = i2c_readAck(); // read a 16 bit register
494 data.raw[0] = i2c_readNak();
495 ms561101ba_ctx.c[i+1] = data.val;
496 }
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
497 }
498
499 void Baro_init() {
500 delay(10);
501 i2c_MS561101BA_reset();
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
502 delay(100);
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
503 i2c_MS561101BA_readCalibration();
504 }
505
506 // read uncompensated temperature value: send command first
507 void i2c_MS561101BA_UT_Start() {
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
508 i2c_rep_start(MS561101BA_ADDRESS+0); // I2C write direction
509 i2c_write(MS561101BA_TEMPERATURE + OSR); // register selection
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
510 i2c_stop();
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
511 }
512
513 // read uncompensated pressure value: send command first
514 void i2c_MS561101BA_UP_Start () {
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
515 i2c_rep_start(MS561101BA_ADDRESS+0); // I2C write direction
516 i2c_write(MS561101BA_PRESSURE + OSR); // register selection
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
517 i2c_stop();
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
518 }
519
520 // read uncompensated pressure value: read result bytes
521 void i2c_MS561101BA_UP_Read () {
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
522 i2c_rep_start(MS561101BA_ADDRESS + 0);
523 i2c_write(0);
524 i2c_rep_start(MS561101BA_ADDRESS + 1);
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
525 ms561101ba_ctx.up.raw[2] = i2c_readAck();
526 ms561101ba_ctx.up.raw[1] = i2c_readAck();
527 ms561101ba_ctx.up.raw[0] = i2c_readNak();
528 }
529
530 // read uncompensated temperature value: read result bytes
531 void i2c_MS561101BA_UT_Read() {
532 i2c_rep_start(MS561101BA_ADDRESS + 0);
533 i2c_write(0);
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
534 i2c_rep_start(MS561101BA_ADDRESS + 1);
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
535 ms561101ba_ctx.ut.raw[2] = i2c_readAck();
536 ms561101ba_ctx.ut.raw[1] = i2c_readAck();
537 ms561101ba_ctx.ut.raw[0] = i2c_readNak();
538 }
539
540 void i2c_MS561101BA_Calculate() {
3ddd3eb - integration of SIRIUS 9DOF + baro IMU
dubusal@gmail.com authored
541 int64_t dT = ms561101ba_ctx.ut.val - ((uint32_t)ms561101ba_ctx.c[5] << 8); //int32_t according to the spec, but int64_t here to avoid cast after
542 int64_t off = ((uint32_t)ms561101ba_ctx.c[2] <<16) + ((dT * ms561101ba_ctx.c[4]) >> 7);
543 int64_t sens = ((uint32_t)ms561101ba_ctx.c[1] <<15) + ((dT * ms561101ba_ctx.c[3]) >> 8);
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
544 pressure = (( (ms561101ba_ctx.up.val * sens ) >> 21) - off) >> 15;
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
545 }
546
547 void Baro_update() {
548 if (currentTime < ms561101ba_ctx.deadline) return;
549 ms561101ba_ctx.deadline = currentTime;
550 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, MS5611 is ok with this speed
551 switch (ms561101ba_ctx.state) {
552 case 0:
553 i2c_MS561101BA_UT_Start();
3ddd3eb - integration of SIRIUS 9DOF + baro IMU
dubusal@gmail.com authored
554 ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 15000; //according to the specs, the pause should be at least 8.22ms
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
555 break;
556 case 1:
557 i2c_MS561101BA_UT_Read();
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
558 ms561101ba_ctx.state++;
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
559 break;
560 case 2:
561 i2c_MS561101BA_UP_Start();
3ddd3eb - integration of SIRIUS 9DOF + baro IMU
dubusal@gmail.com authored
562 ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 15000; //according to the specs, the pause should be at least 8.22ms
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
563 break;
564 case 3:
9f57d95 - SIRIUS IMU integration
dubusal@gmail.com authored
565 i2c_MS561101BA_UP_Read();
566 i2c_MS561101BA_Calculate();
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
567 BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 443300.0f; //decimeter
568 ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 35000;
94d819b git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@197 026…
dubusal@gmail.com authored
569 break;
570 }
571 }
572 #endif
573
574
575 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
576 // I2C Accelerometer ADXL345
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
577 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
578 // I2C adress: 0x3A (8bit) 0x1D (7bit)
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
579 // Resolution: 10bit (Full range - 14bit, but this is autoscaling 10bit ADC to the range +- 16g)
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
580 // principle:
581 // 1) CS PIN must be linked to VCC to select the I2C mode
582 // 2) SD0 PIN must be linked to VCC to select the right I2C adress
583 // 3) bit b00000100 must be set on register 0x2D to read data (only once at the initialization)
584 // 4) bits b00001011 must be set on register 0x31 to select the data format (only once at the initialization)
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
585 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
586 #if defined(ADXL345)
587 void ACC_init () {
588 delay(10);
589 i2c_writeReg(ADXL345_ADDRESS,0x2D,1<<3); // register: Power CTRL -- value: Set measure bit 3 on
590 i2c_writeReg(ADXL345_ADDRESS,0x31,0x0B); // register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
591 i2c_writeReg(ADXL345_ADDRESS,0x2C,0x09); // register: BW_RATE -- value: rate=50hz, bw=20hz
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
592 acc_1G = 256;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
593 }
594
595 void ACC_getADC () {
596 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, ADXL435 is ok with this speed
597 i2c_getSixRawADC(ADXL345_ADDRESS,0x32);
598
599 ACC_ORIENTATION( - ((rawADC[3]<<8) | rawADC[2]) ,
600 ((rawADC[1]<<8) | rawADC[0]) ,
601 ((rawADC[5]<<8) | rawADC[4]) );
602 ACC_Common();
603 }
604 #endif
605
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
606 // ************************************************************************************************************
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
607 // contribution initially from opie11 (rc-groups)
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
608 // adaptation from C2po (may 2011)
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
609 // contribution from ziss_dm (June 2011)
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
610 // contribution from ToLuSe (Jully 2011)
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
611 // contribution from Alex (December 2011)
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
612 // I2C Accelerometer BMA180
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
613 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
614 // I2C adress: 0x80 (8bit) 0x40 (7bit) (SDO connection to VCC)
615 // I2C adress: 0x82 (8bit) 0x41 (7bit) (SDO connection to VDDIO)
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
616 // Resolution: 14bit
617 //
618 // Control registers:
619 //
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
620 // 0x20 bw_tcs: | bw<3:0> | tcs<3:0> |
621 // | 150Hz | xxxxxxxx |
622 // 0x30 tco_z: | tco_z<5:0> | mode_config<1:0> |
623 // | xxxxxxxxxx | 00 |
624 // 0x35 offset_lsb1: | offset_x<3:0> | range<2:0> | smp_skip |
625 // | xxxxxxxxxxxxx | 8G: 101 | xxxxxxxx |
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
626 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
627 #if defined(BMA180)
628 void ACC_init () {
629 delay(10);
6460a33 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@214 026…
dubusal@gmail.com authored
630 //default range 2G: 1G = 4096 unit.
c79b640 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@330 026…
dubusal@gmail.com authored
631 i2c_writeReg(BMA180_ADDRESS,0x0D,1<<4); // register: ctrl_reg0 -- value: set bit ee_w to 1 to enable writing
632 delay(5);
6460a33 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@214 026…
dubusal@gmail.com authored
633 uint8_t control = i2c_readReg(BMA180_ADDRESS, 0x20);
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
634 control = control & 0x0F; // save tcs register
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
635 control = control | (0x01 << 4); // register: bw_tcs reg: bits 4-7 to set bw -- value: set low pass filter to 20Hz
9abecae git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@324 026…
dubusal@gmail.com authored
636 i2c_writeReg(BMA180_ADDRESS, 0x20, control);
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
637 delay(5);
9abecae git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@324 026…
dubusal@gmail.com authored
638 control = i2c_readReg(BMA180_ADDRESS, 0x30);
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
639 control = control & 0xFC; // save tco_z register
640 control = control | 0x00; // set mode_config to 0
9abecae git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@324 026…
dubusal@gmail.com authored
641 i2c_writeReg(BMA180_ADDRESS, 0x30, control);
642 delay(5);
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
643 control = i2c_readReg(BMA180_ADDRESS, 0x35);
644 control = control & 0xF1; // save offset_x and smp_skip register
645 control = control | (0x05 << 1); // set range to 8G
646 i2c_writeReg(BMA180_ADDRESS, 0x35, control);
647 delay(5);
648 acc_1G = 255;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
649 }
650
651 void ACC_getADC () {
652 TWBR = ((16000000L / 400000L) - 16) / 2; // Optional line. Sensor is good for it in the spec.
653 i2c_getSixRawADC(BMA180_ADDRESS,0x02);
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
654 //usefull info is on the 14 bits [2-15] bits /4 => [0-13] bits /4 => 12 bit resolution
655 ACC_ORIENTATION( - ((rawADC[1]<<8) | rawADC[0])/16 ,
656 - ((rawADC[3]<<8) | rawADC[2])/16 ,
657 ((rawADC[5]<<8) | rawADC[4])/16 );
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
658 ACC_Common();
659 }
660 #endif
661
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
662 // ************************************************************************************************************
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
663 // contribution from Point65 and mgros (rc-groups)
664 // contribution from ziss_dm (June 2011)
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
665 // contribution from ToLuSe (Jully 2011)
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
666 // I2C Accelerometer BMA020
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
667 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
668 // I2C adress: 0x70 (8bit)
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
669 // Resolution: 10bit
670 // Control registers:
671 //
672 // Datasheet: After power on reset or soft reset it is recommended to set the SPI4-bit to the correct value.
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
673 // 0x80 = SPI four-wire = Default setting
674 // | 0x15: | SPI4 | enable_adv_INT | new_data_INT | latch_INT | shadow_dis | wake_up_pause<1:0> | wake_up |
675 // | | 1 | 0 | 0 | 0 | 0 | 00 | 0 |
676 //
677 // | 0x14: | reserved <2:0> | range <1:0> | bandwith <2:0> |
678 // | | !!Calibration!! | 2g | 25Hz |
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
679 //
680 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
681 #if defined(BMA020)
682 void ACC_init(){
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
683 i2c_writeReg(0x70,0x15,0x80); // set SPI4 bit
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
684 uint8_t control = i2c_readReg(0x70, 0x14);
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
685 control = control & 0xE0; // save bits 7,6,5
686 control = control | (0x02 << 3); // Range 8G (10)
687 control = control | 0x00; // Bandwidth 25 Hz 000
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
688 i2c_writeReg(0x70,0x14,control);
6f1ca9d - consolidation with shared_
dubusal@gmail.com authored
689 acc_1G = 63;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
690 }
691
692 void ACC_getADC(){
693 TWBR = ((16000000L / 400000L) - 16) / 2;
694 i2c_getSixRawADC(0x70,0x02);
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
695 ACC_ORIENTATION( ((rawADC[1]<<8) | rawADC[0])/64 ,
696 ((rawADC[3]<<8) | rawADC[2])/64 ,
697 ((rawADC[5]<<8) | rawADC[4])/64 );
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
698 ACC_Common();
699 }
700 #endif
701
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
702 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
703 // standalone I2C Nunchuk
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
704 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
705 #if defined(NUNCHACK)
706 void ACC_init() {
707 i2c_writeReg(0xA4 ,0xF0 ,0x55 );
708 i2c_writeReg(0xA4 ,0xFB ,0x00 );
709 delay(250);
710 acc_1G = 200;
711 }
712
713 void ACC_getADC() {
714 TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate. !! you must check if the nunchuk is ok with this freq
715 i2c_getSixRawADC(0xA4,0x00);
716
717 ACC_ORIENTATION( ( (rawADC[3]<<2) + ((rawADC[5]>>4)&0x2) ) ,
718 - ( (rawADC[2]<<2) + ((rawADC[5]>>3)&0x2) ) ,
719 ( ((rawADC[4]&0xFE)<<2) + ((rawADC[5]>>5)&0x6) ));
720 ACC_Common();
721 }
722 #endif
723
4e43422 correction of LIS3LV02 code
dubusal@gmail.com authored
724 // ************************************************************************
c4edfde git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@195 026…
dubusal@gmail.com authored
725 // LIS3LV02 I2C Accelerometer
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
726 //contribution from adver (http://multiwii.com/forum/viewtopic.php?f=8&t=451)
4e43422 correction of LIS3LV02 code
dubusal@gmail.com authored
727 // ************************************************************************
c4edfde git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@195 026…
dubusal@gmail.com authored
728 #if defined(LIS3LV02)
729 #define LIS3A 0x3A // I2C adress: 0x3A (8bit)
730
731 void i2c_ACC_init(){
732 i2c_writeReg(LIS3A ,0x20 ,0xD7 ); // CTRL_REG1 1101 0111 Pwr on, 160Hz
733 i2c_writeReg(LIS3A ,0x21 ,0x50 ); // CTRL_REG2 0100 0000 Littl endian, 12 Bit, Boot
734 acc_1G = 256;
735 }
736
737 void i2c_ACC_getADC(){
738 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
4e43422 correction of LIS3LV02 code
dubusal@gmail.com authored
739 i2c_getSixRawADC(LIS3A,0x28+0x80);
c4edfde git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@195 026…
dubusal@gmail.com authored
740 ACC_ORIENTATION( (rawADC[3]<<8 | rawADC[2])/4 ,
741 -(rawADC[1]<<8 | rawADC[0])/4 ,
742 -(rawADC[5]<<8 | rawADC[4])/4);
4e43422 correction of LIS3LV02 code
dubusal@gmail.com authored
743 ACC_Common();
c4edfde git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@195 026…
dubusal@gmail.com authored
744 }
745 #endif
746
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
747 // ************************************************************************************************************
748 // I2C Accelerometer LSM303DLx
749 // contribution from wektorx (http://www.multiwii.com/forum/viewtopic.php?f=8&t=863)
750 // ************************************************************************************************************
751 #if defined(LSM303DLx_ACC)
752 void ACC_init () {
753 delay(10);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
754 i2c_writeReg(0x30,0x20,0x27);
755 i2c_writeReg(0x30,0x23,0x30);
756 i2c_writeReg(0x30,0x21,0x00);
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
757
758 acc_1G = 256;
759 }
760
761 void ACC_getADC () {
762 TWBR = ((16000000L / 400000L) - 16) / 2;
763 i2c_getSixRawADC(0x30,0xA8);
c4edfde git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@195 026…
dubusal@gmail.com authored
764
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
765 ACC_ORIENTATION( - ((rawADC[3]<<8) | rawADC[2])/16 ,
766 ((rawADC[1]<<8) | rawADC[0])/16 ,
767 ((rawADC[5]<<8) | rawADC[4])/16 );
768 ACC_Common();
769 }
770 #endif
4e43422 correction of LIS3LV02 code
dubusal@gmail.com authored
771
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
772 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
773 // ADC ACC
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
774 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
775 #if defined(ADCACC)
776 void ACC_init(){
777 pinMode(A1,INPUT);
778 pinMode(A2,INPUT);
779 pinMode(A3,INPUT);
780 acc_1G = 75;
781 }
782
783 void ACC_getADC() {
784 ACC_ORIENTATION( -analogRead(A1) ,
785 -analogRead(A2) ,
786 analogRead(A3) );
787 ACC_Common();
788 }
789 #endif
790
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
791 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
792 // contribution from Ciskje
793 // I2C Gyroscope L3G4200D
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
794 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
795 #if defined(L3G4200D)
796 void Gyro_init() {
797 delay(100);
798 i2c_writeReg(0XD2+0 ,0x20 ,0x8F ); // CTRL_REG1 400Hz ODR, 20hz filter, run!
799 delay(5);
800 i2c_writeReg(0XD2+0 ,0x24 ,0x02 ); // CTRL_REG5 low pass filter enable
801 }
802
803 void Gyro_getADC () {
804 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
805 i2c_getSixRawADC(0XD2,0x80|0x28);
806
807 GYRO_ORIENTATION( ((rawADC[1]<<8) | rawADC[0])/20 ,
808 ((rawADC[3]<<8) | rawADC[2])/20 ,
809 -((rawADC[5]<<8) | rawADC[4])/20 );
810 GYRO_Common();
811 }
812 #endif
813
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
814 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
815 // I2C Gyroscope ITG3200
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
816 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
817 // I2C adress: 0xD2 (8bit) 0x69 (7bit)
818 // I2C adress: 0xD0 (8bit) 0x68 (7bit)
819 // principle:
820 // 1) VIO is connected to VDD
821 // 2) I2C adress is set to 0x69 (AD0 PIN connected to VDD)
822 // or 2) I2C adress is set to 0x68 (AD0 PIN connected to GND)
823 // 3) sample rate = 1000Hz ( 1kHz/(div+1) )
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
824 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
825 #if defined(ITG3200)
826 void Gyro_init() {
827 delay(100);
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
828 i2c_writeReg(ITG3200_ADDRESS, 0x3E, 0x80); //register: Power Management -- value: reset device
3ad3115 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@217 026…
dubusal@gmail.com authored
829 // delay(5);
830 // i2c_writeReg(ITG3200_ADDRESS, 0x15, ITG3200_SMPLRT_DIV); //register: Sample Rate Divider -- default value = 0: OK
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
831 delay(5);
3ad3115 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@217 026…
dubusal@gmail.com authored
832 i2c_writeReg(ITG3200_ADDRESS, 0x16, 0x18 + ITG3200_DLPF_CFG); //register: DLPF_CFG - low pass filter configuration
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
833 delay(5);
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
834 i2c_writeReg(ITG3200_ADDRESS, 0x3E, 0x03); //register: Power Management -- value: PLL with Z Gyro reference
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
835 delay(100);
836 }
837
838 void Gyro_getADC () {
839 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
840 i2c_getSixRawADC(ITG3200_ADDRESS,0X1D);
665d59a integration of last ziss_dm alt hold code
dubusal@gmail.com authored
841 GYRO_ORIENTATION( + ( ((rawADC[2]<<8) | rawADC[3])/4) , // range: +/- 8192; +/- 2000 deg/sec
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
842 - ( ((rawADC[0]<<8) | rawADC[1])/4 ) ,
843 - ( ((rawADC[4]<<8) | rawADC[5])/4 ) );
844 GYRO_Common();
845 }
846 #endif
847
848
88a4c79 MPU6050 first code:
dubusal@gmail.com authored
849 // ************************************************************************************************************
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
850 // I2C Compass common function
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
851 // ************************************************************************************************************
6460a33 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@214 026…
dubusal@gmail.com authored
852 #if MAG
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
853 static float magCal[3] = {1.0,1.0,1.0}; // gain for each axis, populated at sensor init
854 static uint8_t magInit = 0;
855
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
856 void Mag_getADC() {
857 static uint32_t t,tCal = 0;
858 static int16_t magZeroTempMin[3];
859 static int16_t magZeroTempMax[3];
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
860 uint8_t axis;
3ddd3eb - integration of SIRIUS 9DOF + baro IMU
dubusal@gmail.com authored
861 if ( currentTime < t ) return; //each read is spaced by 100ms
862 t = currentTime + 100000;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
863 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
864 Device_Mag_getADC();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
865 if (calibratingM == 1) {
866 tCal = t;
867 for(axis=0;axis<3;axis++) {magZero[axis] = 0;magZeroTempMin[axis] = 0; magZeroTempMax[axis] = 0;}
868 calibratingM = 0;
869 }
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
870 magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
871 magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
872 magADC[YAW] = magADC[YAW] * magCal[YAW];
873 if (magInit) { // we apply offset only once mag calibration is done
874 magADC[ROLL] -= magZero[ROLL];
875 magADC[PITCH] -= magZero[PITCH];
876 magADC[YAW] -= magZero[YAW];
877 }
878
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
879 if (tCal != 0) {
880 if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
658030a git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@341 026…
dubusal@gmail.com authored
881 LEDPIN_TOGGLE;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
882 for(axis=0;axis<3;axis++) {
883 if (magADC[axis] < magZeroTempMin[axis]) magZeroTempMin[axis] = magADC[axis];
884 if (magADC[axis] > magZeroTempMax[axis]) magZeroTempMax[axis] = magADC[axis];
885 }
886 } else {
887 tCal = 0;
888 for(axis=0;axis<3;axis++)
889 magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis])/2;
890 writeParams();
891 }
892 }
893 }
6460a33 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@214 026…
dubusal@gmail.com authored
894 #endif
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
895
896 // ************************************************************************************************************
897 // I2C Compass HMC5843 & HMC5883
898 // ************************************************************************************************************
899 // I2C adress: 0x3C (8bit) 0x1E (7bit)
900 // ************************************************************************************************************
901 #if defined(HMC5843) || defined(HMC5883)
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
902 #define MAG_ADDRESS 0x3C
903 #define MAG_DATA_REGISTER 0x03
904
905 void Mag_init() {
906 delay(100);
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
907 // force positiveBias
908 i2c_writeReg(MAG_ADDRESS ,0x00 ,0x71 ); //Configuration Register A -- 0 11 100 01 num samples: 8 ; output rate: 15Hz ; positive bias
909 delay(50);
910 // set gains for calibration
911 i2c_writeReg(MAG_ADDRESS ,0x01 ,0x60 ); //Configuration Register B -- 011 00000 configuration gain 2.5Ga
912 i2c_writeReg(MAG_ADDRESS ,0x02 ,0x01 ); //Mode register -- 000000 01 single Conversion Mode
913
914 // read values from the compass - self test operation
915 // by placing the mode register into single-measurement mode (0x01), two data acquisition cycles will be made on each magnetic vector.
916 // The first acquisition values will be subtracted from the second acquisition, and the net measurement will be placed into the data output registers
917 delay(100);
918 getADC();
919 delay(10);
920 magCal[ROLL] = 1000.0 / magADC[ROLL];
921 magCal[PITCH] = 1000.0 / magADC[PITCH];
922 magCal[YAW] = - 1000.0 / magADC[YAW];
923
924 // leave test mode
925 i2c_writeReg(MAG_ADDRESS ,0x00 ,0x70 ); //Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
926 i2c_writeReg(MAG_ADDRESS ,0x01 ,0x20 ); //Configuration Register B -- 001 00000 configuration gain 1.3Ga
927 i2c_writeReg(MAG_ADDRESS ,0x02 ,0x00 ); //Mode register -- 000000 00 continuous Conversion Mode
928
929 magInit = 1;
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
930 }
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
931
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
932 void getADC() {
933 i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER);
934 #if defined(HMC5843)
935 MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) ,
936 ((rawADC[2]<<8) | rawADC[3]) ,
937 -((rawADC[4]<<8) | rawADC[5]) );
938 #endif
939 #if defined (HMC5883)
940 MAG_ORIENTATION( ((rawADC[4]<<8) | rawADC[5]) ,
941 -((rawADC[0]<<8) | rawADC[1]) ,
942 -((rawADC[2]<<8) | rawADC[3]) );
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
943 #endif
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
944 }
945
946 #if not defined(MPU6050_EN_I2C_BYPASS)
947 void Device_Mag_getADC() {
948 getADC();
949 }
950 #endif
88a4c79 MPU6050 first code:
dubusal@gmail.com authored
951 #endif
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
952
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
953 // ************************************************************************************************************
954 // I2C Compass AK8975 (Contribution by EOSBandi)
955 // ************************************************************************************************************
956 // I2C adress: 0x18 (8bit) 0x0C (7bit)
957 // ************************************************************************************************************
958 #if defined(AK8975)
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
959 #define MAG_ADDRESS 0x18
960 #define MAG_DATA_REGISTER 0x03
961
962 void Mag_init() {
963 delay(100);
964 i2c_writeReg(MAG_ADDRESS,0x0a,0x01); //Start the first conversion
965 delay(100);
308c643 - introducing Felix code to simplify and keep generic motor & servo o…
dubusal@gmail.com authored
966 magInit = 1;
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
967 }
968
969 #if not defined(MPU6050_EN_I2C_BYPASS)
970 void Device_Mag_getADC() {
971 i2c_getSixRawADC(MAG_ADDRESS,MAG_DATA_REGISTER);
972 MAG_ORIENTATION( ((rawADC[3]<<8) | rawADC[2]) ,
973 ((rawADC[1]<<8) | rawADC[0]) ,
974 -((rawADC[5]<<8) | rawADC[4]) );
975 //Start another meassurement
976 i2c_writeReg(MAG_ADDRESS,0x0a,0x01);
977 }
978 #endif
979 #endif
980
981 // ************************************************************************************************************
982 // I2C Gyroscope and Accelerometer MPU6050
983 // ************************************************************************************************************
984 #if defined(MPU6050)
985
986 void Gyro_init() {
987 TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
988 i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
989 delay(5);
990 i2c_writeReg(MPU6050_ADDRESS, 0x19, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
991 i2c_writeReg(MPU6050_ADDRESS, 0x1A, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
992 i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
993 i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
994 // enable I2C bypass for AUX I2C
995 #if defined(MAG)
996 i2c_writeReg(MPU6050_ADDRESS, 0x6A, 0x00); //USER_CTRL -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=0 (I2C bypass mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
997 i2c_writeReg(MPU6050_ADDRESS, 0x37, 0x02); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0
998 #endif
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
999 }
1000
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
1001 void Gyro_getADC () {
1002 i2c_getSixRawADC(MPU6050_ADDRESS, 0x43);
1003 GYRO_ORIENTATION( + ( ((rawADC[2]<<8) | rawADC[3])/4 ) , // range: +/- 8192; +/- 2000 deg/sec
1004 - ( ((rawADC[0]<<8) | rawADC[1])/4 ) ,
1005 - ( ((rawADC[4]<<8) | rawADC[5])/4 ) );
1006 GYRO_Common();
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
1007 }
d44dd60 merging shared & main trunk
dubusal@gmail.com authored
1008
1009 void ACC_init () {
1010 i2c_writeReg(MPU6050_ADDRESS, 0x1C, 0x10); //ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0 //note something is wrong in the spec.
1011 //note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
1012 //confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480
1013 acc_1G = 255;
1014
1015 #if defined(MPU6050_EN_I2C_BYPASS)
1016 //at this stage, the MAG is configured via the original MAG init function in I2C bypass mode
1017 //now we configure MPU as a I2C Master device to handle the MAG via the I2C AUX port (done here for HMC5883)
1018 i2c_writeReg(MPU6050_ADDRESS, 0x6A, 0b00100000); //USER_CTRL -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=1 (I2C master mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
1019 i2c_writeReg(MPU6050_ADDRESS, 0x37, 0x00); //INT_PIN_CFG -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=0 ; CLKOUT_EN=0
1020 i2c_writeReg(MPU6050_ADDRESS, 0x24, 0x0D); //I2C_MST_CTRL -- MULT_MST_EN=0 ; WAIT_FOR_ES=0 ; SLV_3_FIFO_EN=0 ; I2C_MST_P_NSR=0 ; I2C_MST_CLK=13 (I2C slave speed bus = 400kHz)
1021 i2c_writeReg(MPU6050_ADDRESS, 0x25, 0x80|(MAG_ADDRESS>>1));//I2C_SLV0_ADDR -- I2C_SLV4_RW=1 (read operation) ; I2C_SLV4_ADDR=MAG_ADDRESS
1022 i2c_writeReg(MPU6050_ADDRESS, 0x26, MAG_DATA_REGISTER);//I2C_SLV0_REG -- 6 data bytes of MAG are stored in 6 registers. First register address is MAG_DATA_REGISTER
1023 i2c_writeReg(MPU6050_ADDRESS, 0x27, 0x86); //I2C_SLV0_CTRL -- I2C_SLV0_EN=1 ; I2C_SLV0_BYTE_SW=0 ; I2C_SLV0_REG_DIS=0 ; I2C_SLV0_GRP=0 ; I2C_SLV0_LEN=3 (3x2 bytes)
1024 #endif
1025 }
1026
1027 void ACC_getADC () {
1028 i2c_getSixRawADC(MPU6050_ADDRESS, 0x3B);
1029 ACC_ORIENTATION( - ((rawADC[0]<<8) | rawADC[1])/8 ,
1030 - ((rawADC[2]<<8) | rawADC[3])/8 ,
1031 ((rawADC[4]<<8) | rawADC[5])/8 );
1032 ACC_Common();
1033 }
1034
1035 //The MAG acquisition function must be replaced because we now talk to the MPU device
1036 #if defined(MPU6050_EN_I2C_BYPASS)
1037 void Device_Mag_getADC() {
1038 i2c_getSixRawADC(MPU6050_ADDRESS, 0x49); //0x49 is the first memory room for EXT_SENS_DATA
1039 #if defined(HMC5843)
1040 MAG_ORIENTATION( ((rawADC[0]<<8) | rawADC[1]) ,
1041 ((rawADC[2]<<8) | rawADC[3]) ,
1042 -((rawADC[4]<<8) | rawADC[5]) );
1043 #endif
1044 #if defined (HMC5883)
1045 MAG_ORIENTATION( ((rawADC[4]<<8) | rawADC[5]) ,
1046 -((rawADC[0]<<8) | rawADC[1]) ,
1047 -((rawADC[2]<<8) | rawADC[3]) );
1048 #endif
1049 #if defined (AK8975)
1050 MAG_ORIENTATION( ((rawADC[3]<<8) | rawADC[2]) ,
1051 ((rawADC[1]<<8) | rawADC[0]) ,
1052 -((rawADC[5]<<8) | rawADC[4]) );
1053 #endif
1054 }
1055 #endif
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
1056 #endif
1057
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1058 #if !GYRO
1059 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1060 // I2C Wii Motion Plus + optional Nunchuk
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1061 // ************************************************************************************************************
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1062 // I2C adress 1: 0xA6 (8bit) 0x53 (7bit)
1063 // I2C adress 2: 0xA4 (8bit) 0x52 (7bit)
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1064 // ************************************************************************************************************
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
1065 void WMP_init() {
1066 delay(250);
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1067 i2c_writeReg(0xA6, 0xF0, 0x55); // Initialize Extension
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
1068 delay(250);
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1069 i2c_writeReg(0xA6, 0xFE, 0x05); // Activate Nunchuck pass-through mode
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
1070 delay(250);
1071
1072 // We need to set acc_1G for the Nunchuk beforehand; It's used in WMP_getRawADC() and ACC_Common()
1073 // If a different accelerometer is used, it will be overwritten by its ACC_init() later.
1074 acc_1G = 200;
1075 acc_25deg = acc_1G * 0.423;
1076 uint8_t numberAccRead = 0;
1077 // Read from WMP 100 times, this should return alternating WMP and Nunchuk data
1078 for(uint8_t i=0;i<100;i++) {
1079 delay(4);
1080 if (WMP_getRawADC() == 0) numberAccRead++; // Count number of times we read from the Nunchuk extension
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1081 }
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
1082 // If we got at least 25 Nunchuck reads, we assume the Nunchuk is present
1083 if (numberAccRead>25)
1084 nunchuk = 1;
1085 delay(10);
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1086 }
1087
1088 uint8_t WMP_getRawADC() {
1089 uint8_t axis;
1090 TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
1091 i2c_getSixRawADC(0xA4,0x00);
1092
1093 if (micros() < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state
1094 for (axis = 0; axis < 3; axis++) {gyroADC[axis]=0;accADC[axis]=0;}
1095 accADC[YAW] = acc_1G;
1096 return 1;
1097 }
1098
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
1099 // Wii Motion Plus Data
1100 if ( (rawADC[5]&0x03) == 0x02 ) {
1101 // Assemble 14bit data
3ad3115 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@217 026…
dubusal@gmail.com authored
1102 gyroADC[ROLL] = - ( ((rawADC[5]>>2)<<8) | rawADC[2] ); //range: +/- 8192
1103 gyroADC[PITCH] = - ( ((rawADC[4]>>2)<<8) | rawADC[1] );
1104 gyroADC[YAW] = - ( ((rawADC[3]>>2)<<8) | rawADC[0] );
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1105 GYRO_Common();
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
1106 // Check if slow bit is set and normalize to fast mode range
3ad3115 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@217 026…
dubusal@gmail.com authored
1107 gyroADC[ROLL] = (rawADC[3]&0x01) ? gyroADC[ROLL]/5 : gyroADC[ROLL]; //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification
1108 gyroADC[PITCH] = (rawADC[4]&0x02)>>1 ? gyroADC[PITCH]/5 : gyroADC[PITCH]; //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
1109 gyroADC[YAW] = (rawADC[3]&0x02)>>1 ? gyroADC[YAW]/5 : gyroADC[YAW]; // this step must be done after zero compensation
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1110 return 1;
d56a352 git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@212 026…
dubusal@gmail.com authored
1111 } else if ( (rawADC[5]&0x03) == 0x00 ) { // Nunchuk Data
1112 ACC_ORIENTATION( ( (rawADC[3]<<2) | ((rawADC[5]>>4)&0x02) ) ,
1113 - ( (rawADC[2]<<2) | ((rawADC[5]>>3)&0x02) ) ,
1114 ( ((rawADC[4]>>1)<<3) | ((rawADC[5]>>5)&0x06) ) );
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1115 ACC_Common();
1116 return 0;
1117 } else
1118 return 2;
1119 }
4331d51 - last code change from ziss_dm and Hamburger
dubusal@gmail.com authored
1120 #endif
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1121
1122 void initSensors() {
1123 delay(200);
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
1124 POWERPIN_ON;
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1125 delay(100);
1126 i2c_init();
1127 delay(100);
1128 if (GYRO) Gyro_init();
f4f3dd2 - new I2C functions with a dissociated STOP command
dubusal@gmail.com authored
1129 else WMP_init();
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1130 if (BARO) Baro_init();
1131 if (MAG) Mag_init();
88a4c79 MPU6050 first code:
dubusal@gmail.com authored
1132 if (ACC) {ACC_init();acc_25deg = acc_1G * 0.423;}
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 026…
dubusal@gmail.com authored
1133 }
Something went wrong with that request. Please try again.