AIR DATA ACQUISITION & ANALYSIS
This project tutorial shows how to acquire, analyze and display air data on multifunction display using LabVIEW Graphic User Interface(GUI).
Components and supplies
1
USB-6008 National Instruments USB Data Acquisition Card
1
Pressure Sensor, Differential
1
Arduino UNO
1
RTD Sensor, PT100
1
Inertial Measurement Unit (IMU) (6 deg of freedom)
Project description
Code
Gyro Code
arduino
Code for IMU 6050 gyro sensor.
1 2// MPU-6050 Accelerometer + Gyro + Arduino Uno 3// ----------------------------- 4// 5// 6// June 2012 7// Open Source / Public Domain 8// 9// Using Arduino 1.0.1 10// It will not work with an older version, 11// since Wire.endTransmission() uses a parameter 12// to hold or release the I2C bus. 13// 14// Documentation: 15// - The InvenSense documents: 16// - "MPU-6000 and MPU-6050 Product Specification", 17// PS-MPU-6000A.pdf 18// - "MPU-6000 and MPU-6050 Register Map and Descriptions", 19// RM-MPU-6000A.pdf or RS-MPU-6000A.pdf 20// - "MPU-6000/MPU-6050 9-Axis Evaluation Board User Guide" 21// AN-MPU-6000EVB.pdf 22// 23// The accuracy is 16-bits. 24// 25// Temperature sensor from -40 to +85 degrees Celsius 26// 340 per degrees, -512 at 35 degrees. 27// 28// At power-up, all registers are zero, except these two: 29// Register 0x6B (PWR_MGMT_2) = 0x40 (I read zero). 30// Register 0x75 (WHO_AM_I) = 0x68. 31// 32 33#include <Wire.h> 34 35 36// Register names according to the datasheet. 37// According to the InvenSense document 38// "MPU-6000 and MPU-6050 Register Map 39// and Descriptions Revision 3.2", there are no registers 40// at 0x02 ... 0x18, but according other information 41// the registers in that unknown area are for gain 42// and offsets. 43// 44#define MPU6050_AUX_VDDIO 0x01 // R/W 45#define MPU6050_SMPLRT_DIV 0x19 // R/W 46#define MPU6050_CONFIG 0x1A // R/W 47#define MPU6050_GYRO_CONFIG 0x1B // R/W 48#define MPU6050_ACCEL_CONFIG 0x1C // R/W 49#define MPU6050_FF_THR 0x1D // R/W 50#define MPU6050_FF_DUR 0x1E // R/W 51#define MPU6050_MOT_THR 0x1F // R/W 52#define MPU6050_MOT_DUR 0x20 // R/W 53#define MPU6050_ZRMOT_THR 0x21 // R/W 54#define MPU6050_ZRMOT_DUR 0x22 // R/W 55#define MPU6050_FIFO_EN 0x23 // R/W 56#define MPU6050_I2C_MST_CTRL 0x24 // R/W 57#define MPU6050_I2C_SLV0_ADDR 0x25 // R/W 58#define MPU6050_I2C_SLV0_REG 0x26 // R/W 59#define MPU6050_I2C_SLV0_CTRL 0x27 // R/W 60#define MPU6050_I2C_SLV1_ADDR 0x28 // R/W 61#define MPU6050_I2C_SLV1_REG 0x29 // R/W 62#define MPU6050_I2C_SLV1_CTRL 0x2A // R/W 63#define MPU6050_I2C_SLV2_ADDR 0x2B // R/W 64#define MPU6050_I2C_SLV2_REG 0x2C // R/W 65#define MPU6050_I2C_SLV2_CTRL 0x2D // R/W 66#define MPU6050_I2C_SLV3_ADDR 0x2E // R/W 67#define MPU6050_I2C_SLV3_REG 0x2F // R/W 68#define MPU6050_I2C_SLV3_CTRL 0x30 // R/W 69#define MPU6050_I2C_SLV4_ADDR 0x31 // R/W 70#define MPU6050_I2C_SLV4_REG 0x32 // R/W 71#define MPU6050_I2C_SLV4_DO 0x33 // R/W 72#define MPU6050_I2C_SLV4_CTRL 0x34 // R/W 73#define MPU6050_I2C_SLV4_DI 0x35 // R 74#define MPU6050_I2C_MST_STATUS 0x36 // R 75#define MPU6050_INT_PIN_CFG 0x37 // R/W 76#define MPU6050_INT_ENABLE 0x38 // R/W 77#define MPU6050_INT_STATUS 0x3A // R 78#define MPU6050_ACCEL_XOUT_H 0x3B // R 79#define MPU6050_ACCEL_XOUT_L 0x3C // R 80#define MPU6050_ACCEL_YOUT_H 0x3D // R 81#define MPU6050_ACCEL_YOUT_L 0x3E // R 82#define MPU6050_ACCEL_ZOUT_H 0x3F // R 83#define MPU6050_ACCEL_ZOUT_L 0x40 // R 84#define MPU6050_TEMP_OUT_H 0x41 // R 85#define MPU6050_TEMP_OUT_L 0x42 // R 86#define MPU6050_GYRO_XOUT_H 0x43 // R 87#define MPU6050_GYRO_XOUT_L 0x44 // R 88#define MPU6050_GYRO_YOUT_H 0x45 // R 89#define MPU6050_GYRO_YOUT_L 0x46 // R 90#define MPU6050_GYRO_ZOUT_H 0x47 // R 91#define MPU6050_GYRO_ZOUT_L 0x48 // R 92#define MPU6050_EXT_SENS_DATA_00 0x49 // R 93#define MPU6050_EXT_SENS_DATA_01 0x4A // R 94#define MPU6050_EXT_SENS_DATA_02 0x4B // R 95#define MPU6050_EXT_SENS_DATA_03 0x4C // R 96#define MPU6050_EXT_SENS_DATA_04 0x4D // R 97#define MPU6050_EXT_SENS_DATA_05 0x4E // R 98#define MPU6050_EXT_SENS_DATA_06 0x4F // R 99#define MPU6050_EXT_SENS_DATA_07 0x50 // R 100#define MPU6050_EXT_SENS_DATA_08 0x51 // R 101#define MPU6050_EXT_SENS_DATA_09 0x52 // R 102#define MPU6050_EXT_SENS_DATA_10 0x53 // R 103#define MPU6050_EXT_SENS_DATA_11 0x54 // R 104#define MPU6050_EXT_SENS_DATA_12 0x55 // R 105#define MPU6050_EXT_SENS_DATA_13 0x56 // R 106#define MPU6050_EXT_SENS_DATA_14 0x57 // R 107#define MPU6050_EXT_SENS_DATA_15 0x58 // R 108#define MPU6050_EXT_SENS_DATA_16 0x59 // R 109#define MPU6050_EXT_SENS_DATA_17 0x5A // R 110#define MPU6050_EXT_SENS_DATA_18 0x5B // R 111#define MPU6050_EXT_SENS_DATA_19 0x5C // R 112#define MPU6050_EXT_SENS_DATA_20 0x5D // R 113#define MPU6050_EXT_SENS_DATA_21 0x5E // R 114#define MPU6050_EXT_SENS_DATA_22 0x5F // R 115#define MPU6050_EXT_SENS_DATA_23 0x60 // R 116#define MPU6050_MOT_DETECT_STATUS 0x61 // R 117#define MPU6050_I2C_SLV0_DO 0x63 // R/W 118#define MPU6050_I2C_SLV1_DO 0x64 // R/W 119#define MPU6050_I2C_SLV2_DO 0x65 // R/W 120#define MPU6050_I2C_SLV3_DO 0x66 // R/W 121#define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W 122#define MPU6050_SIGNAL_PATH_RESET 0x68 // R/W 123#define MPU6050_MOT_DETECT_CTRL 0x69 // R/W 124#define MPU6050_USER_CTRL 0x6A // R/W 125#define MPU6050_PWR_MGMT_1 0x6B // R/W 126#define MPU6050_PWR_MGMT_2 0x6C // R/W 127#define MPU6050_FIFO_COUNTH 0x72 // R/W 128#define MPU6050_FIFO_COUNTL 0x73 // R/W 129#define MPU6050_FIFO_R_W 0x74 // R/W 130#define MPU6050_WHO_AM_I 0x75 // R 131 132 133// Defines for the bits, to be able to change 134// between bit number and binary definition. 135// By using the bit number, programming the sensor 136// is like programming the AVR microcontroller. 137// But instead of using "(1<<X)", or "_BV(X)", 138// the Arduino "bit(X)" is used. 139#define MPU6050_D0 0 140#define MPU6050_D1 1 141#define MPU6050_D2 2 142#define MPU6050_D3 3 143#define MPU6050_D4 4 144#define MPU6050_D5 5 145#define MPU6050_D6 6 146#define MPU6050_D7 7 147 148// AUX_VDDIO Register 149#define MPU6050_AUX_VDDIO MPU6050_D7 // I2C high: 1=VDD, 0=VLOGIC 150 151// CONFIG Register 152// DLPF is Digital Low Pass Filter for both gyro and accelerometers. 153// These are the names for the bits. 154// Use these only with the bit() macro. 155#define MPU6050_DLPF_CFG0 MPU6050_D0 156#define MPU6050_DLPF_CFG1 MPU6050_D1 157#define MPU6050_DLPF_CFG2 MPU6050_D2 158#define MPU6050_EXT_SYNC_SET0 MPU6050_D3 159#define MPU6050_EXT_SYNC_SET1 MPU6050_D4 160#define MPU6050_EXT_SYNC_SET2 MPU6050_D5 161 162// Combined definitions for the EXT_SYNC_SET values 163#define MPU6050_EXT_SYNC_SET_0 (0) 164#define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0)) 165#define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1)) 166#define MPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) 167#define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2)) 168#define MPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0)) 169#define MPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)) 170#define MPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) 171 172// Alternative names for the combined definitions. 173#define MPU6050_EXT_SYNC_DISABLED MPU6050_EXT_SYNC_SET_0 174#define MPU6050_EXT_SYNC_TEMP_OUT_L MPU6050_EXT_SYNC_SET_1 175#define MPU6050_EXT_SYNC_GYRO_XOUT_L MPU6050_EXT_SYNC_SET_2 176#define MPU6050_EXT_SYNC_GYRO_YOUT_L MPU6050_EXT_SYNC_SET_3 177#define MPU6050_EXT_SYNC_GYRO_ZOUT_L MPU6050_EXT_SYNC_SET_4 178#define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5 179#define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6 180#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7 181 182// Combined definitions for the DLPF_CFG values 183#define MPU6050_DLPF_CFG_0 (0) 184#define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0)) 185#define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1)) 186#define MPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) 187#define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2)) 188#define MPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0)) 189#define MPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)) 190#define MPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) 191 192// Alternative names for the combined definitions 193// This name uses the bandwidth (Hz) for the accelometer, 194// for the gyro the bandwidth is almost the same. 195#define MPU6050_DLPF_260HZ MPU6050_DLPF_CFG_0 196#define MPU6050_DLPF_184HZ MPU6050_DLPF_CFG_1 197#define MPU6050_DLPF_94HZ MPU6050_DLPF_CFG_2 198#define MPU6050_DLPF_44HZ MPU6050_DLPF_CFG_3 199#define MPU6050_DLPF_21HZ MPU6050_DLPF_CFG_4 200#define MPU6050_DLPF_10HZ MPU6050_DLPF_CFG_5 201#define MPU6050_DLPF_5HZ MPU6050_DLPF_CFG_6 202#define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7 203 204// GYRO_CONFIG Register 205// The XG_ST, YG_ST, ZG_ST are bits for selftest. 206// The FS_SEL sets the range for the gyro. 207// These are the names for the bits. 208// Use these only with the bit() macro. 209#define MPU6050_FS_SEL0 MPU6050_D3 210#define MPU6050_FS_SEL1 MPU6050_D4 211#define MPU6050_ZG_ST MPU6050_D5 212#define MPU6050_YG_ST MPU6050_D6 213#define MPU6050_XG_ST MPU6050_D7 214 215// Combined definitions for the FS_SEL values 216#define MPU6050_FS_SEL_0 (0) 217#define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0)) 218#define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1)) 219#define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0)) 220 221// Alternative names for the combined definitions 222// The name uses the range in degrees per second. 223#define MPU6050_FS_SEL_250 MPU6050_FS_SEL_0 224#define MPU6050_FS_SEL_500 MPU6050_FS_SEL_1 225#define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2 226#define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3 227 228// ACCEL_CONFIG Register 229// The XA_ST, YA_ST, ZA_ST are bits for selftest. 230// The AFS_SEL sets the range for the accelerometer. 231// These are the names for the bits. 232// Use these only with the bit() macro. 233#define MPU6050_ACCEL_HPF0 MPU6050_D0 234#define MPU6050_ACCEL_HPF1 MPU6050_D1 235#define MPU6050_ACCEL_HPF2 MPU6050_D2 236#define MPU6050_AFS_SEL0 MPU6050_D3 237#define MPU6050_AFS_SEL1 MPU6050_D4 238#define MPU6050_ZA_ST MPU6050_D5 239#define MPU6050_YA_ST MPU6050_D6 240#define MPU6050_XA_ST MPU6050_D7 241 242// Combined definitions for the ACCEL_HPF values 243#define MPU6050_ACCEL_HPF_0 (0) 244#define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0)) 245#define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1)) 246#define MPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) 247#define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2)) 248#define MPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) 249 250// Alternative names for the combined definitions 251// The name uses the Cut-off frequency. 252#define MPU6050_ACCEL_HPF_RESET MPU6050_ACCEL_HPF_0 253#define MPU6050_ACCEL_HPF_5HZ MPU6050_ACCEL_HPF_1 254#define MPU6050_ACCEL_HPF_2_5HZ MPU6050_ACCEL_HPF_2 255#define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3 256#define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4 257#define MPU6050_ACCEL_HPF_HOLD MPU6050_ACCEL_HPF_7 258 259// Combined definitions for the AFS_SEL values 260#define MPU6050_AFS_SEL_0 (0) 261#define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0)) 262#define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1)) 263#define MPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0)) 264 265// Alternative names for the combined definitions 266// The name uses the full scale range for the accelerometer. 267#define MPU6050_AFS_SEL_2G MPU6050_AFS_SEL_0 268#define MPU6050_AFS_SEL_4G MPU6050_AFS_SEL_1 269#define MPU6050_AFS_SEL_8G MPU6050_AFS_SEL_2 270#define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3 271 272// FIFO_EN Register 273// These are the names for the bits. 274// Use these only with the bit() macro. 275#define MPU6050_SLV0_FIFO_EN MPU6050_D0 276#define MPU6050_SLV1_FIFO_EN MPU6050_D1 277#define MPU6050_SLV2_FIFO_EN MPU6050_D2 278#define MPU6050_ACCEL_FIFO_EN MPU6050_D3 279#define MPU6050_ZG_FIFO_EN MPU6050_D4 280#define MPU6050_YG_FIFO_EN MPU6050_D5 281#define MPU6050_XG_FIFO_EN MPU6050_D6 282#define MPU6050_TEMP_FIFO_EN MPU6050_D7 283 284// I2C_MST_CTRL Register 285// These are the names for the bits. 286// Use these only with the bit() macro. 287#define MPU6050_I2C_MST_CLK0 MPU6050_D0 288#define MPU6050_I2C_MST_CLK1 MPU6050_D1 289#define MPU6050_I2C_MST_CLK2 MPU6050_D2 290#define MPU6050_I2C_MST_CLK3 MPU6050_D3 291#define MPU6050_I2C_MST_P_NSR MPU6050_D4 292#define MPU6050_SLV_3_FIFO_EN MPU6050_D5 293#define MPU6050_WAIT_FOR_ES MPU6050_D6 294#define MPU6050_MULT_MST_EN MPU6050_D7 295 296// Combined definitions for the I2C_MST_CLK 297#define MPU6050_I2C_MST_CLK_0 (0) 298#define MPU6050_I2C_MST_CLK_1 (bit(MPU6050_I2C_MST_CLK0)) 299#define MPU6050_I2C_MST_CLK_2 (bit(MPU6050_I2C_MST_CLK1)) 300#define MPU6050_I2C_MST_CLK_3 (bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 301#define MPU6050_I2C_MST_CLK_4 (bit(MPU6050_I2C_MST_CLK2)) 302#define MPU6050_I2C_MST_CLK_5 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) 303#define MPU6050_I2C_MST_CLK_6 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) 304#define MPU6050_I2C_MST_CLK_7 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 305#define MPU6050_I2C_MST_CLK_8 (bit(MPU6050_I2C_MST_CLK3)) 306#define MPU6050_I2C_MST_CLK_9 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK0)) 307#define MPU6050_I2C_MST_CLK_10 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)) 308#define MPU6050_I2C_MST_CLK_11 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 309#define MPU6050_I2C_MST_CLK_12 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)) 310#define MPU6050_I2C_MST_CLK_13 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) 311#define MPU6050_I2C_MST_CLK_14 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) 312#define MPU6050_I2C_MST_CLK_15 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 313 314// Alternative names for the combined definitions 315// The names uses I2C Master Clock Speed in kHz. 316#define MPU6050_I2C_MST_CLK_348KHZ MPU6050_I2C_MST_CLK_0 317#define MPU6050_I2C_MST_CLK_333KHZ MPU6050_I2C_MST_CLK_1 318#define MPU6050_I2C_MST_CLK_320KHZ MPU6050_I2C_MST_CLK_2 319#define MPU6050_I2C_MST_CLK_308KHZ MPU6050_I2C_MST_CLK_3 320#define MPU6050_I2C_MST_CLK_296KHZ MPU6050_I2C_MST_CLK_4 321#define MPU6050_I2C_MST_CLK_286KHZ MPU6050_I2C_MST_CLK_5 322#define MPU6050_I2C_MST_CLK_276KHZ MPU6050_I2C_MST_CLK_6 323#define MPU6050_I2C_MST_CLK_267KHZ MPU6050_I2C_MST_CLK_7 324#define MPU6050_I2C_MST_CLK_258KHZ MPU6050_I2C_MST_CLK_8 325#define MPU6050_I2C_MST_CLK_500KHZ MPU6050_I2C_MST_CLK_9 326#define MPU6050_I2C_MST_CLK_471KHZ MPU6050_I2C_MST_CLK_10 327#define MPU6050_I2C_MST_CLK_444KHZ MPU6050_I2C_MST_CLK_11 328#define MPU6050_I2C_MST_CLK_421KHZ MPU6050_I2C_MST_CLK_12 329#define MPU6050_I2C_MST_CLK_400KHZ MPU6050_I2C_MST_CLK_13 330#define MPU6050_I2C_MST_CLK_381KHZ MPU6050_I2C_MST_CLK_14 331#define MPU6050_I2C_MST_CLK_364KHZ MPU6050_I2C_MST_CLK_15 332 333// I2C_SLV0_ADDR Register 334// These are the names for the bits. 335// Use these only with the bit() macro. 336#define MPU6050_I2C_SLV0_RW MPU6050_D7 337 338// I2C_SLV0_CTRL Register 339// These are the names for the bits. 340// Use these only with the bit() macro. 341#define MPU6050_I2C_SLV0_LEN0 MPU6050_D0 342#define MPU6050_I2C_SLV0_LEN1 MPU6050_D1 343#define MPU6050_I2C_SLV0_LEN2 MPU6050_D2 344#define MPU6050_I2C_SLV0_LEN3 MPU6050_D3 345#define MPU6050_I2C_SLV0_GRP MPU6050_D4 346#define MPU6050_I2C_SLV0_REG_DIS MPU6050_D5 347#define MPU6050_I2C_SLV0_BYTE_SW MPU6050_D6 348#define MPU6050_I2C_SLV0_EN MPU6050_D7 349 350// A mask for the length 351#define MPU6050_I2C_SLV0_LEN_MASK 0x0F 352 353// I2C_SLV1_ADDR Register 354// These are the names for the bits. 355// Use these only with the bit() macro. 356#define MPU6050_I2C_SLV1_RW MPU6050_D7 357 358// I2C_SLV1_CTRL Register 359// These are the names for the bits. 360// Use these only with the bit() macro. 361#define MPU6050_I2C_SLV1_LEN0 MPU6050_D0 362#define MPU6050_I2C_SLV1_LEN1 MPU6050_D1 363#define MPU6050_I2C_SLV1_LEN2 MPU6050_D2 364#define MPU6050_I2C_SLV1_LEN3 MPU6050_D3 365#define MPU6050_I2C_SLV1_GRP MPU6050_D4 366#define MPU6050_I2C_SLV1_REG_DIS MPU6050_D5 367#define MPU6050_I2C_SLV1_BYTE_SW MPU6050_D6 368#define MPU6050_I2C_SLV1_EN MPU6050_D7 369 370// A mask for the length 371#define MPU6050_I2C_SLV1_LEN_MASK 0x0F 372 373// I2C_SLV2_ADDR Register 374// These are the names for the bits. 375// Use these only with the bit() macro. 376#define MPU6050_I2C_SLV2_RW MPU6050_D7 377 378// I2C_SLV2_CTRL Register 379// These are the names for the bits. 380// Use these only with the bit() macro. 381#define MPU6050_I2C_SLV2_LEN0 MPU6050_D0 382#define MPU6050_I2C_SLV2_LEN1 MPU6050_D1 383#define MPU6050_I2C_SLV2_LEN2 MPU6050_D2 384#define MPU6050_I2C_SLV2_LEN3 MPU6050_D3 385#define MPU6050_I2C_SLV2_GRP MPU6050_D4 386#define MPU6050_I2C_SLV2_REG_DIS MPU6050_D5 387#define MPU6050_I2C_SLV2_BYTE_SW MPU6050_D6 388#define MPU6050_I2C_SLV2_EN MPU6050_D7 389 390// A mask for the length 391#define MPU6050_I2C_SLV2_LEN_MASK 0x0F 392 393// I2C_SLV3_ADDR Register 394// These are the names for the bits. 395// Use these only with the bit() macro. 396#define MPU6050_I2C_SLV3_RW MPU6050_D7 397 398// I2C_SLV3_CTRL Register 399// These are the names for the bits. 400// Use these only with the bit() macro. 401#define MPU6050_I2C_SLV3_LEN0 MPU6050_D0 402#define MPU6050_I2C_SLV3_LEN1 MPU6050_D1 403#define MPU6050_I2C_SLV3_LEN2 MPU6050_D2 404#define MPU6050_I2C_SLV3_LEN3 MPU6050_D3 405#define MPU6050_I2C_SLV3_GRP MPU6050_D4 406#define MPU6050_I2C_SLV3_REG_DIS MPU6050_D5 407#define MPU6050_I2C_SLV3_BYTE_SW MPU6050_D6 408#define MPU6050_I2C_SLV3_EN MPU6050_D7 409 410// A mask for the length 411#define MPU6050_I2C_SLV3_LEN_MASK 0x0F 412 413// I2C_SLV4_ADDR Register 414// These are the names for the bits. 415// Use these only with the bit() macro. 416#define MPU6050_I2C_SLV4_RW MPU6050_D7 417 418// I2C_SLV4_CTRL Register 419// These are the names for the bits. 420// Use these only with the bit() macro. 421#define MPU6050_I2C_MST_DLY0 MPU6050_D0 422#define MPU6050_I2C_MST_DLY1 MPU6050_D1 423#define MPU6050_I2C_MST_DLY2 MPU6050_D2 424#define MPU6050_I2C_MST_DLY3 MPU6050_D3 425#define MPU6050_I2C_MST_DLY4 MPU6050_D4 426#define MPU6050_I2C_SLV4_REG_DIS MPU6050_D5 427#define MPU6050_I2C_SLV4_INT_EN MPU6050_D6 428#define MPU6050_I2C_SLV4_EN MPU6050_D7 429 430// A mask for the delay 431#define MPU6050_I2C_MST_DLY_MASK 0x1F 432 433// I2C_MST_STATUS Register 434// These are the names for the bits. 435// Use these only with the bit() macro. 436#define MPU6050_I2C_SLV0_NACK MPU6050_D0 437#define MPU6050_I2C_SLV1_NACK MPU6050_D1 438#define MPU6050_I2C_SLV2_NACK MPU6050_D2 439#define MPU6050_I2C_SLV3_NACK MPU6050_D3 440#define MPU6050_I2C_SLV4_NACK MPU6050_D4 441#define MPU6050_I2C_LOST_ARB MPU6050_D5 442#define MPU6050_I2C_SLV4_DONE MPU6050_D6 443#define MPU6050_PASS_THROUGH MPU6050_D7 444 445// I2C_PIN_CFG Register 446// These are the names for the bits. 447// Use these only with the bit() macro. 448#define MPU6050_CLKOUT_EN MPU6050_D0 449#define MPU6050_I2C_BYPASS_EN MPU6050_D1 450#define MPU6050_FSYNC_INT_EN MPU6050_D2 451#define MPU6050_FSYNC_INT_LEVEL MPU6050_D3 452#define MPU6050_INT_RD_CLEAR MPU6050_D4 453#define MPU6050_LATCH_INT_EN MPU6050_D5 454#define MPU6050_INT_OPEN MPU6050_D6 455#define MPU6050_INT_LEVEL MPU6050_D7 456 457// INT_ENABLE Register 458// These are the names for the bits. 459// Use these only with the bit() macro. 460#define MPU6050_DATA_RDY_EN MPU6050_D0 461#define MPU6050_I2C_MST_INT_EN MPU6050_D3 462#define MPU6050_FIFO_OFLOW_EN MPU6050_D4 463#define MPU6050_ZMOT_EN MPU6050_D5 464#define MPU6050_MOT_EN MPU6050_D6 465#define MPU6050_FF_EN MPU6050_D7 466 467// INT_STATUS Register 468// These are the names for the bits. 469// Use these only with the bit() macro. 470#define MPU6050_DATA_RDY_INT MPU6050_D0 471#define MPU6050_I2C_MST_INT MPU6050_D3 472#define MPU6050_FIFO_OFLOW_INT MPU6050_D4 473#define MPU6050_ZMOT_INT MPU6050_D5 474#define MPU6050_MOT_INT MPU6050_D6 475#define MPU6050_FF_INT MPU6050_D7 476 477// MOT_DETECT_STATUS Register 478// These are the names for the bits. 479// Use these only with the bit() macro. 480#define MPU6050_MOT_ZRMOT MPU6050_D0 481#define MPU6050_MOT_ZPOS MPU6050_D2 482#define MPU6050_MOT_ZNEG MPU6050_D3 483#define MPU6050_MOT_YPOS MPU6050_D4 484#define MPU6050_MOT_YNEG MPU6050_D5 485#define MPU6050_MOT_XPOS MPU6050_D6 486#define MPU6050_MOT_XNEG MPU6050_D7 487 488// IC2_MST_DELAY_CTRL Register 489// These are the names for the bits. 490// Use these only with the bit() macro. 491#define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0 492#define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1 493#define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2 494#define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3 495#define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4 496#define MPU6050_DELAY_ES_SHADOW MPU6050_D7 497 498// SIGNAL_PATH_RESET Register 499// These are the names for the bits. 500// Use these only with the bit() macro. 501#define MPU6050_TEMP_RESET MPU6050_D0 502#define MPU6050_ACCEL_RESET MPU6050_D1 503#define MPU6050_GYRO_RESET MPU6050_D2 504 505// MOT_DETECT_CTRL Register 506// These are the names for the bits. 507// Use these only with the bit() macro. 508#define MPU6050_MOT_COUNT0 MPU6050_D0 509#define MPU6050_MOT_COUNT1 MPU6050_D1 510#define MPU6050_FF_COUNT0 MPU6050_D2 511#define MPU6050_FF_COUNT1 MPU6050_D3 512#define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4 513#define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5 514 515// Combined definitions for the MOT_COUNT 516#define MPU6050_MOT_COUNT_0 (0) 517#define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0)) 518#define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1)) 519#define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0)) 520 521// Alternative names for the combined definitions 522#define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0 523 524// Combined definitions for the FF_COUNT 525#define MPU6050_FF_COUNT_0 (0) 526#define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0)) 527#define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1)) 528#define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0)) 529 530// Alternative names for the combined definitions 531#define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0 532 533// Combined definitions for the ACCEL_ON_DELAY 534#define MPU6050_ACCEL_ON_DELAY_0 (0) 535#define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0)) 536#define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1)) 537#define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0)) 538 539// Alternative names for the ACCEL_ON_DELAY 540#define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0 541#define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1 542#define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2 543#define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3 544 545// USER_CTRL Register 546// These are the names for the bits. 547// Use these only with the bit() macro. 548#define MPU6050_SIG_COND_RESET MPU6050_D0 549#define MPU6050_I2C_MST_RESET MPU6050_D1 550#define MPU6050_FIFO_RESET MPU6050_D2 551#define MPU6050_I2C_IF_DIS MPU6050_D4 // must be 0 for MPU-6050 552#define MPU6050_I2C_MST_EN MPU6050_D5 553#define MPU6050_FIFO_EN MPU6050_D6 554 555// PWR_MGMT_1 Register 556// These are the names for the bits. 557// Use these only with the bit() macro. 558#define MPU6050_CLKSEL0 MPU6050_D0 559#define MPU6050_CLKSEL1 MPU6050_D1 560#define MPU6050_CLKSEL2 MPU6050_D2 561#define MPU6050_TEMP_DIS MPU6050_D3 // 1: disable temperature sensor 562#define MPU6050_CYCLE MPU6050_D5 // 1: sample and sleep 563#define MPU6050_SLEEP MPU6050_D6 // 1: sleep mode 564#define MPU6050_DEVICE_RESET MPU6050_D7 // 1: reset to default values 565 566// Combined definitions for the CLKSEL 567#define MPU6050_CLKSEL_0 (0) 568#define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0)) 569#define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1)) 570#define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) 571#define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2)) 572#define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0)) 573#define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)) 574#define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) 575 576// Alternative names for the combined definitions 577#define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0 578#define MPU6050_CLKSEL_X MPU6050_CLKSEL_1 579#define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2 580#define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3 581#define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4 582#define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5 583#define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6 584#define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7 585 586// PWR_MGMT_2 Register 587// These are the names for the bits. 588// Use these only with the bit() macro. 589#define MPU6050_STBY_ZG MPU6050_D0 590#define MPU6050_STBY_YG MPU6050_D1 591#define MPU6050_STBY_XG MPU6050_D2 592#define MPU6050_STBY_ZA MPU6050_D3 593#define MPU6050_STBY_YA MPU6050_D4 594#define MPU6050_STBY_XA MPU6050_D5 595#define MPU6050_LP_WAKE_CTRL0 MPU6050_D6 596#define MPU6050_LP_WAKE_CTRL1 MPU6050_D7 597 598// Combined definitions for the LP_WAKE_CTRL 599#define MPU6050_LP_WAKE_CTRL_0 (0) 600#define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0)) 601#define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1)) 602#define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0)) 603 604// Alternative names for the combined definitions 605// The names uses the Wake-up Frequency. 606#define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0 607#define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1 608#define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2 609#define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3 610 611 612// Default I2C address for the MPU-6050 is 0x68. 613// But only if the AD0 pin is low. 614// Some sensor boards have AD0 high, and the 615// I2C address thus becomes 0x69. 616#define MPU6050_I2C_ADDRESS 0x68 617 618 619// Declaring an union for the registers and the axis values. 620// The byte order does not match the byte order of 621// the compiler and AVR chip. 622// The AVR chip (on the Arduino board) has the Low Byte 623// at the lower address. 624// But the MPU-6050 has a different order: High Byte at 625// lower address, so that has to be corrected. 626// The register part "reg" is only used internally, 627// and are swapped in code. 628typedef union accel_t_gyro_union 629{ 630 struct 631 { 632 uint8_t x_accel_h; 633 uint8_t x_accel_l; 634 uint8_t y_accel_h; 635 uint8_t y_accel_l; 636 uint8_t z_accel_h; 637 uint8_t z_accel_l; 638 uint8_t t_h; 639 uint8_t t_l; 640 uint8_t x_gyro_h; 641 uint8_t x_gyro_l; 642 uint8_t y_gyro_h; 643 uint8_t y_gyro_l; 644 uint8_t z_gyro_h; 645 uint8_t z_gyro_l; 646 } reg; 647 struct 648 { 649 int x_accel; 650 int y_accel; 651 int z_accel; 652 int temperature; 653 int x_gyro; 654 int y_gyro; 655 int z_gyro; 656 } value; 657}; 658 659// Use the following global variables and access functions to help store the overall 660// rotation angle of the sensor 661unsigned long last_read_time; 662float last_x_angle; // These are the filtered angles 663float last_y_angle; 664float last_z_angle; 665float last_gyro_x_angle; // Store the gyro angles to compare drift 666float last_gyro_y_angle; 667float last_gyro_z_angle; 668float last_x_kalangle; //Kalman angle 669float last_y_kalangle; 670float last_z_kalangle; 671 672void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro, float kx, float ky, float kz) { 673 last_read_time = time; 674 last_x_angle = x; 675 last_y_angle = y; 676 last_z_angle = z; 677 last_gyro_x_angle = x_gyro; 678 last_gyro_y_angle = y_gyro; 679 last_gyro_z_angle = z_gyro; 680 last_x_kalangle = kx; 681 last_y_kalangle = ky; 682 last_z_kalangle = kz; 683} 684 685inline unsigned long get_last_time() {return last_read_time;} 686inline float get_last_x_angle() {return last_x_angle;} 687inline float get_last_y_angle() {return last_y_angle;} 688inline float get_last_z_angle() {return last_z_angle;} 689inline float get_last_gyro_x_angle() {return last_gyro_x_angle;} 690inline float get_last_gyro_y_angle() {return last_gyro_y_angle;} 691inline float get_last_gyro_z_angle() {return last_gyro_z_angle;} 692inline float get_last_x_kalangle() {return last_x_kalangle;} 693inline float get_last_y_kalangle() {return last_y_kalangle;} 694inline float get_last_z_kalangle() {return last_z_kalangle;} 695 696// Use the following global variables and access functions 697// to calibrate the acceleration sensor 698float base_x_accel; 699float base_y_accel; 700float base_z_accel; 701 702float base_x_gyro; 703float base_y_gyro; 704float base_z_gyro; 705 706 707int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) { 708 // Read the raw values. 709 // Read 14 bytes at once, 710 // containing acceleration, temperature and gyro. 711 // With the default settings of the MPU-6050, 712 // there is no filter enabled, and the values 713 // are not very stable. Returns the error value 714 715 accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr; 716 717 int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro)); 718 719 // Swap all high and low bytes. 720 // After this, the registers values are swapped, 721 // so the structure name like x_accel_l does no 722 // longer contain the lower byte. 723 uint8_t swap; 724 #define SWAP(x,y) swap = x; x = y; y = swap 725 726 SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l); 727 SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l); 728 SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l); 729 SWAP ((*accel_t_gyro).reg.t_h, (*accel_t_gyro).reg.t_l); 730 SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l); 731 SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l); 732 SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l); 733 734 return error; 735} 736 737// The sensor should be motionless on a horizontal surface 738// while calibration is happening 739void calibrate_sensors() { 740 int num_readings = 10; 741 float x_accel = 0; 742 float y_accel = 0; 743 float z_accel = 0; 744 float x_gyro = 0; 745 float y_gyro = 0; 746 float z_gyro = 0; 747 accel_t_gyro_union accel_t_gyro; 748 749 //Serial.println("Starting Calibration"); 750 751 // Discard the first set of values read from the IMU 752 read_gyro_accel_vals((uint8_t *) &accel_t_gyro); 753 754 // Read and average the raw values from the IMU 755 for (int i = 0; i < num_readings; i++) { 756 read_gyro_accel_vals((uint8_t *) &accel_t_gyro); 757 x_accel += accel_t_gyro.value.x_accel; 758 y_accel += accel_t_gyro.value.y_accel; 759 z_accel += accel_t_gyro.value.z_accel; 760 x_gyro += accel_t_gyro.value.x_gyro; 761 y_gyro += accel_t_gyro.value.y_gyro; 762 z_gyro += accel_t_gyro.value.z_gyro; 763 delay(100); 764 } 765 x_accel /= num_readings; 766 y_accel /= num_readings; 767 z_accel /= num_readings; 768 x_gyro /= num_readings; 769 y_gyro /= num_readings; 770 z_gyro /= num_readings; 771 772 // Store the raw calibration values globally 773 base_x_accel = x_accel; 774 base_y_accel = y_accel; 775 base_z_accel = z_accel; 776 base_x_gyro = x_gyro; 777 base_y_gyro = y_gyro; 778 base_z_gyro = z_gyro; 779 780 //Serial.println("Finishing Calibration"); 781} 782 783 784/************************ 785* KALMAN - Filter setup * 786*************************/ 787class Kalman { 788public: 789 Kalman() { 790 /* We will set the varibles like so, these can also be tuned by the user */ 791 Q_angle = 0.001; 792 Q_bias = 0.003; 793 R_measure = 0.03; 794 795 bias = 0; // Reset bias 796 P[0][0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 797 P[0][1] = 0; 798 P[1][0] = 0; 799 P[1][1] = 0; 800 }; 801 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 802 float getAngle(float newAngle, float newRate, float dt) { 803 // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 804 // Modified by Kristian Lauszus 805 // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 806 807 // Discrete Kalman filter time update equations - Time Update ("Predict") 808 // Update xhat - Project the state ahead 809 /* Step 1 */ 810 rate = newRate - bias; 811 angle += dt * rate; 812 813 // Update estimation error covariance - Project the error covariance ahead 814 /* Step 2 */ 815 P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 816 P[0][1] -= dt * P[1][1]; 817 P[1][0] -= dt * P[1][1]; 818 P[1][1] += Q_bias * dt; 819 820 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 821 // Calculate Kalman gain - Compute the Kalman gain 822 /* Step 4 */ 823 S = P[0][0] + R_measure; 824 /* Step 5 */ 825 K[0] = P[0][0] / S; 826 K[1] = P[1][0] / S; 827 828 // Calculate angle and bias - Update estimate with measurement zk (newAngle) 829 /* Step 3 */ 830 y = newAngle - angle; 831 /* Step 6 */ 832 angle += K[0] * y; 833 bias += K[1] * y; 834 835 // Calculate estimation error covariance - Update the error covariance 836 /* Step 7 */ 837 P[0][0] -= K[0] * P[0][0]; 838 P[0][1] -= K[0] * P[0][1]; 839 P[1][0] -= K[1] * P[0][0]; 840 P[1][1] -= K[1] * P[0][1]; 841 842 return angle; 843 }; 844 void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 845 double getRate() { return rate; }; // Return the unbiased rate 846 847 /* These are used to tune the Kalman filter */ 848 void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 849 void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 850 void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 851 852 double getQangle() { return Q_angle; }; 853 double getQbias() { return Q_bias; }; 854 double getRmeasure() { return R_measure; }; 855 856private: 857 /* Kalman filter variables */ 858 double Q_angle; // Process noise variance for the accelerometer 859 double Q_bias; // Process noise variance for the gyro bias 860 double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 861 862 double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix 863 double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix 864 double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 865 866 double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 867 double K[2]; // Kalman gain - This is a 2x1 matrix 868 double y; // Angle difference - 1x1 matrix 869 double S; // Estimate error - 1x1 matrix 870}; 871Kalman kalmanX; // Create the Kalman instances 872Kalman kalmanY; 873 874 875 876/******** 877* SETUP * 878*********/ 879 880void setup() 881{ 882 int error; 883 uint8_t c; 884 accel_t_gyro_union accel_t_gyro; 885 886 887 Serial.begin(19200); 888 889 // Initialize the 'Wire' class for the I2C-bus. 890 Wire.begin(); 891 892 893 // default at power-up: 894 // Gyro at 250 degrees second 895 // Acceleration at 2g 896 // Clock source at internal 8MHz 897 // The device is in sleep mode. 898 // 899 900 error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); 901 902 903 // According to the datasheet, the 'sleep' bit 904 // should read a '1'. But I read a '0'. 905 // That bit has to be cleared, since the sensor 906 // is in sleep mode at power-up. Even if the 907 // bit reads '0'. 908 error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1); 909 910 // Clear the 'sleep' bit to start the sensor. 911 MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); 912 913 //Initialize the angles 914 calibrate_sensors(); 915 set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 0, 0, 0); 916 917 918 // Get raw acceleration values 919 //float G_CONVERT = 16384; 920 float accel_x = accel_t_gyro.value.x_accel; 921 float accel_y = accel_t_gyro.value.y_accel; 922 float accel_z = accel_t_gyro.value.z_accel; 923 924 // Get angle values from accelerometer 925 float RADIANS_TO_DEGREES = 180/3.14159; 926// float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); 927 float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 928 float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 929 930 float accel_angle_z = 0; 931 932 kalmanX.setAngle(accel_angle_x); // Set starting angle 933 kalmanY.setAngle(accel_angle_y); 934 935} 936 937 938 939/************ 940* Main loop * 941*************/ 942void loop() 943{ 944 int error; 945 double dT; 946 accel_t_gyro_union accel_t_gyro; 947 948 float kalAngleX, kalAngleY, kalAngleZ; // Calculate the angle using a Kalman filter 949 950 // Read the raw values. 951 error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); 952 953 // Get the time of reading for rotation computations 954 unsigned long t_now = millis(); 955 956 957 // Convert gyro values to degrees/sec 958 float FS_SEL = 131; 959 960 float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL; 961 float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL; 962 float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL; 963 964 965 // Get raw acceleration values 966 //float G_CONVERT = 16384; 967 float accel_x = accel_t_gyro.value.x_accel; 968 float accel_y = accel_t_gyro.value.y_accel; 969 float accel_z = accel_t_gyro.value.z_accel; 970 971 // Get angle values from accelerometer 972 float RADIANS_TO_DEGREES = 180/3.14159; 973// float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); 974 float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 975 float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 976 977 float accel_angle_z = 0; 978 979 // Compute the (filtered) gyro angles 980 float dt =(t_now - get_last_time())/1000.0; 981 float gyro_angle_x = gyro_x*dt + get_last_x_angle(); 982 float gyro_angle_y = gyro_y*dt + get_last_y_angle(); 983 float gyro_angle_z = gyro_z*dt + get_last_z_angle(); 984 985 // Compute the drifting gyro angles 986 float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); 987 float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); 988 float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); 989 990 // Apply the complementary filter to figure out the change in angle - choice of alpha is 991 // estimated now. Alpha depends on the sampling rate... 992 float alpha = 0.96; 993 float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; 994 float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; 995 float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle 996 997 998 //KALMAN filter 999 kalAngleX = kalmanX.getAngle(accel_angle_x, gyro_x, dt); 1000 //delay(5); 1001 kalAngleY = kalmanY.getAngle(accel_angle_y, gyro_y, dt); 1002 kalAngleZ = gyro_angle_z; 1003 1004 1005 // Update the saved data with the latest values 1006 set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, kalAngleX, kalAngleY, kalAngleZ); 1007 1008 //Note added: 10/08/13 pad() creates padding that is required to ensure 128Bytes are sent. May not be needed as new version of LV code maybe able to handle 1009 1010 //Serial.print("START"); 1011 // Send the data to the serial port 1012 Serial.print(F("DEL:")); //Delta T 1013 Serial.print(dt, DEC); 1014 1015 Serial.print(F("#ACC:")); //Accelerometer angle 1016 pad(accel_angle_x); 1017 //Serial.print(accel_angle_x, 2); 1018 Serial.print(F(",")); 1019 pad(accel_angle_y); 1020 //Serial.print(accel_angle_y, 2); 1021 Serial.print(F(",")); 1022 pad(accel_angle_z); 1023 //Serial.print(accel_angle_z, 2); 1024 1025 Serial.print(F("#GYR:")); 1026 pad(unfiltered_gyro_angle_x); 1027 //Serial.print(unfiltered_gyro_angle_x, 2); //Gyroscope angle 1028 Serial.print(F(",")); 1029 pad(unfiltered_gyro_angle_y); 1030 //Serial.print(unfiltered_gyro_angle_y, 2); 1031 Serial.print(F(",")); 1032 pad(unfiltered_gyro_angle_z); 1033 //Serial.print(unfiltered_gyro_angle_z, 2); 1034 1035 Serial.print(F("#FIL:")); //Filtered angle 1036 pad(angle_x); 1037 //Serial.print(angle_x, 2); 1038 Serial.print(F(",")); 1039 pad(angle_y); 1040 //Serial.print(angle_y, 2); 1041 Serial.print(F(",")); 1042 pad(angle_z); 1043 //Serial.print(angle_z, 2); 1044 Serial.print(F(",")); 1045 1046 Serial.print(F("#KAL:")); //Kalman Filtered angle 1047 pad(kalAngleX); 1048 //Serial.print(kalAngleX, 2); 1049 Serial.print(F(",")); 1050 pad (kalAngleY); 1051 //Serial.print(kalAngleY, 2); 1052 Serial.print(F(",")); 1053 pad(kalAngleZ); 1054 //Serial.print(kalAngleZ, 2); 1055 //Serial.print("END"); 1056 Serial.print("\ 1057"); 1058 //Serial.println(F("")); 1059 Serial.flush(); 1060 // Delay so we don't swamp the serial port 1061 delay(5); //so not to swamp the serial port 1062} 1063 1064 1065// -------------------------------------------------------- 1066// MPU6050_read 1067// 1068// This is a common function to read multiple bytes 1069// from an I2C device. 1070// 1071// It uses the boolean parameter for Wire.endTransMission() 1072// to be able to hold or release the I2C-bus. 1073// This is implemented in Arduino 1.0.1. 1074// 1075// Only this function is used to read. 1076// There is no function for a single byte. 1077// 1078int MPU6050_read(int start, uint8_t *buffer, int size) 1079{ 1080 int i, n, error; 1081 1082 Wire.beginTransmission(MPU6050_I2C_ADDRESS); 1083 n = Wire.write(start); 1084 if (n != 1) 1085 return (-10); 1086 1087 n = Wire.endTransmission(false); // hold the I2C-bus 1088 if (n != 0) 1089 return (n); 1090 1091 // Third parameter is true: relase I2C-bus after data is read. 1092 Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true); 1093 i = 0; 1094 while(Wire.available() && i<size) 1095 { 1096 buffer[i++]=Wire.read(); 1097 } 1098 if ( i != size) 1099 return (-11); 1100 1101 return (0); // return : no error 1102} 1103 1104 1105// -------------------------------------------------------- 1106// MPU6050_write 1107// 1108// This is a common function to write multiple bytes to an I2C device. 1109// 1110// If only a single register is written, 1111// use the function MPU_6050_write_reg(). 1112// 1113// Parameters: 1114// start : Start address, use a define for the register 1115// pData : A pointer to the data to write. 1116// size : The number of bytes to write. 1117// 1118// If only a single register is written, a pointer 1119// to the data has to be used, and the size is 1120// a single byte: 1121// int data = 0; // the data to write 1122// MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1); 1123// 1124int MPU6050_write(int start, const uint8_t *pData, int size) 1125{ 1126 int n, error; 1127 1128 Wire.beginTransmission(MPU6050_I2C_ADDRESS); 1129 n = Wire.write(start); // write the start address 1130 if (n != 1) 1131 return (-20); 1132 1133 n = Wire.write(pData, size); // write data bytes 1134 if (n != size) 1135 return (-21); 1136 1137 error = Wire.endTransmission(true); // release the I2C-bus 1138 if (error != 0) 1139 return (error); 1140 1141 return (0); // return : no error 1142} 1143 1144// -------------------------------------------------------- 1145// MPU6050_write_reg 1146// 1147// An extra function to write a single register. 1148// It is just a wrapper around the MPU_6050_write() 1149// function, and it is only a convenient function 1150// to make it easier to write a single register. 1151// 1152int MPU6050_write_reg(int reg, uint8_t data) 1153{ 1154 int error; 1155 1156 error = MPU6050_write(reg, &data, 1); 1157 1158 return (error); 1159} 1160 1161//This function pads the printed value to take up 1162//7 characters Example: **20.16 1163void pad(float PadVal){ 1164 1165 //Positve Value 1166 if (PadVal > 0){ //For positive value 1167 if(PadVal < 10){ //For positive value less than 10 1168 Serial.print("***"); //0-9 ***0-9.00 1169 } 1170 else if (PadVal < 100){ //values less than 100 1171 Serial.print("**"); //10-99 **10-99.00 1172 } 1173 1174 } 1175 else if(PadVal < 0){ //for negative values 1176 if(PadVal > -10){ //for negative value greater than -10 1177 Serial.print("**"); //-9 - 0 **-1 - -9.00 1178 } 1179 else if (PadVal > -100){ //for negative values lesss than -99 1180 Serial.print("*"); //*-99.00 1181 } 1182 } 1183 1184 Serial.print(PadVal, 2); //print the padded value 1185}
Gyro Code
arduino
Code for IMU 6050 gyro sensor.
1 2// MPU-6050 Accelerometer + Gyro + Arduino Uno 3// ----------------------------- 4// 5// 6// June 2012 7// Open Source / Public Domain 8// 9// Using Arduino 1.0.1 10// It will not work with an older version, 11// since Wire.endTransmission() uses a parameter 12// to hold or release the I2C bus. 13// 14// Documentation: 15// - The InvenSense documents: 16// - "MPU-6000 and MPU-6050 Product Specification", 17// PS-MPU-6000A.pdf 18// - "MPU-6000 and MPU-6050 Register Map and Descriptions", 19// RM-MPU-6000A.pdf or RS-MPU-6000A.pdf 20// - "MPU-6000/MPU-6050 9-Axis Evaluation Board User Guide" 21// AN-MPU-6000EVB.pdf 22// 23// The accuracy is 16-bits. 24// 25// Temperature sensor from -40 to +85 degrees Celsius 26// 340 per degrees, -512 at 35 degrees. 27// 28// At power-up, all registers are zero, except these two: 29// Register 0x6B (PWR_MGMT_2) = 0x40 (I read zero). 30// Register 0x75 (WHO_AM_I) = 0x68. 31// 32 33#include <Wire.h> 34 35 36// Register names according to the datasheet. 37// According to the InvenSense document 38// "MPU-6000 and MPU-6050 Register Map 39// and Descriptions Revision 3.2", there are no registers 40// at 0x02 ... 0x18, but according other information 41// the registers in that unknown area are for gain 42// and offsets. 43// 44#define MPU6050_AUX_VDDIO 0x01 // R/W 45#define MPU6050_SMPLRT_DIV 0x19 // R/W 46#define MPU6050_CONFIG 0x1A // R/W 47#define MPU6050_GYRO_CONFIG 0x1B // R/W 48#define MPU6050_ACCEL_CONFIG 0x1C // R/W 49#define MPU6050_FF_THR 0x1D // R/W 50#define MPU6050_FF_DUR 0x1E // R/W 51#define MPU6050_MOT_THR 0x1F // R/W 52#define MPU6050_MOT_DUR 0x20 // R/W 53#define MPU6050_ZRMOT_THR 0x21 // R/W 54#define MPU6050_ZRMOT_DUR 0x22 // R/W 55#define MPU6050_FIFO_EN 0x23 // R/W 56#define MPU6050_I2C_MST_CTRL 0x24 // R/W 57#define MPU6050_I2C_SLV0_ADDR 0x25 // R/W 58#define MPU6050_I2C_SLV0_REG 0x26 // R/W 59#define MPU6050_I2C_SLV0_CTRL 0x27 // R/W 60#define MPU6050_I2C_SLV1_ADDR 0x28 // R/W 61#define MPU6050_I2C_SLV1_REG 0x29 // R/W 62#define MPU6050_I2C_SLV1_CTRL 0x2A // R/W 63#define MPU6050_I2C_SLV2_ADDR 0x2B // R/W 64#define MPU6050_I2C_SLV2_REG 0x2C // R/W 65#define MPU6050_I2C_SLV2_CTRL 0x2D // R/W 66#define MPU6050_I2C_SLV3_ADDR 0x2E // R/W 67#define MPU6050_I2C_SLV3_REG 0x2F // R/W 68#define MPU6050_I2C_SLV3_CTRL 0x30 // R/W 69#define MPU6050_I2C_SLV4_ADDR 0x31 // R/W 70#define MPU6050_I2C_SLV4_REG 0x32 // R/W 71#define MPU6050_I2C_SLV4_DO 0x33 // R/W 72#define MPU6050_I2C_SLV4_CTRL 0x34 // R/W 73#define MPU6050_I2C_SLV4_DI 0x35 // R 74#define MPU6050_I2C_MST_STATUS 0x36 // R 75#define MPU6050_INT_PIN_CFG 0x37 // R/W 76#define MPU6050_INT_ENABLE 0x38 // R/W 77#define MPU6050_INT_STATUS 0x3A // R 78#define MPU6050_ACCEL_XOUT_H 0x3B // R 79#define MPU6050_ACCEL_XOUT_L 0x3C // R 80#define MPU6050_ACCEL_YOUT_H 0x3D // R 81#define MPU6050_ACCEL_YOUT_L 0x3E // R 82#define MPU6050_ACCEL_ZOUT_H 0x3F // R 83#define MPU6050_ACCEL_ZOUT_L 0x40 // R 84#define MPU6050_TEMP_OUT_H 0x41 // R 85#define MPU6050_TEMP_OUT_L 0x42 // R 86#define MPU6050_GYRO_XOUT_H 0x43 // R 87#define MPU6050_GYRO_XOUT_L 0x44 // R 88#define MPU6050_GYRO_YOUT_H 0x45 // R 89#define MPU6050_GYRO_YOUT_L 0x46 // R 90#define MPU6050_GYRO_ZOUT_H 0x47 // R 91#define MPU6050_GYRO_ZOUT_L 0x48 // R 92#define MPU6050_EXT_SENS_DATA_00 0x49 // R 93#define MPU6050_EXT_SENS_DATA_01 0x4A // R 94#define MPU6050_EXT_SENS_DATA_02 0x4B // R 95#define MPU6050_EXT_SENS_DATA_03 0x4C // R 96#define MPU6050_EXT_SENS_DATA_04 0x4D // R 97#define MPU6050_EXT_SENS_DATA_05 0x4E // R 98#define MPU6050_EXT_SENS_DATA_06 0x4F // R 99#define MPU6050_EXT_SENS_DATA_07 0x50 // R 100#define MPU6050_EXT_SENS_DATA_08 0x51 // R 101#define MPU6050_EXT_SENS_DATA_09 0x52 // R 102#define MPU6050_EXT_SENS_DATA_10 0x53 // R 103#define MPU6050_EXT_SENS_DATA_11 0x54 // R 104#define MPU6050_EXT_SENS_DATA_12 0x55 // R 105#define MPU6050_EXT_SENS_DATA_13 0x56 // R 106#define MPU6050_EXT_SENS_DATA_14 0x57 // R 107#define MPU6050_EXT_SENS_DATA_15 0x58 // R 108#define MPU6050_EXT_SENS_DATA_16 0x59 // R 109#define MPU6050_EXT_SENS_DATA_17 0x5A // R 110#define MPU6050_EXT_SENS_DATA_18 0x5B // R 111#define MPU6050_EXT_SENS_DATA_19 0x5C // R 112#define MPU6050_EXT_SENS_DATA_20 0x5D // R 113#define MPU6050_EXT_SENS_DATA_21 0x5E // R 114#define MPU6050_EXT_SENS_DATA_22 0x5F // R 115#define MPU6050_EXT_SENS_DATA_23 0x60 // R 116#define MPU6050_MOT_DETECT_STATUS 0x61 // R 117#define MPU6050_I2C_SLV0_DO 0x63 // R/W 118#define MPU6050_I2C_SLV1_DO 0x64 // R/W 119#define MPU6050_I2C_SLV2_DO 0x65 // R/W 120#define MPU6050_I2C_SLV3_DO 0x66 // R/W 121#define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W 122#define MPU6050_SIGNAL_PATH_RESET 0x68 // R/W 123#define MPU6050_MOT_DETECT_CTRL 0x69 // R/W 124#define MPU6050_USER_CTRL 0x6A // R/W 125#define MPU6050_PWR_MGMT_1 0x6B // R/W 126#define MPU6050_PWR_MGMT_2 0x6C // R/W 127#define MPU6050_FIFO_COUNTH 0x72 // R/W 128#define MPU6050_FIFO_COUNTL 0x73 // R/W 129#define MPU6050_FIFO_R_W 0x74 // R/W 130#define MPU6050_WHO_AM_I 0x75 // R 131 132 133// Defines for the bits, to be able to change 134// between bit number and binary definition. 135// By using the bit number, programming the sensor 136// is like programming the AVR microcontroller. 137// But instead of using "(1<<X)", or "_BV(X)", 138// the Arduino "bit(X)" is used. 139#define MPU6050_D0 0 140#define MPU6050_D1 1 141#define MPU6050_D2 2 142#define MPU6050_D3 3 143#define MPU6050_D4 4 144#define MPU6050_D5 5 145#define MPU6050_D6 6 146#define MPU6050_D7 7 147 148// AUX_VDDIO Register 149#define MPU6050_AUX_VDDIO MPU6050_D7 // I2C high: 1=VDD, 0=VLOGIC 150 151// CONFIG Register 152// DLPF is Digital Low Pass Filter for both gyro and accelerometers. 153// These are the names for the bits. 154// Use these only with the bit() macro. 155#define MPU6050_DLPF_CFG0 MPU6050_D0 156#define MPU6050_DLPF_CFG1 MPU6050_D1 157#define MPU6050_DLPF_CFG2 MPU6050_D2 158#define MPU6050_EXT_SYNC_SET0 MPU6050_D3 159#define MPU6050_EXT_SYNC_SET1 MPU6050_D4 160#define MPU6050_EXT_SYNC_SET2 MPU6050_D5 161 162// Combined definitions for the EXT_SYNC_SET values 163#define MPU6050_EXT_SYNC_SET_0 (0) 164#define MPU6050_EXT_SYNC_SET_1 (bit(MPU6050_EXT_SYNC_SET0)) 165#define MPU6050_EXT_SYNC_SET_2 (bit(MPU6050_EXT_SYNC_SET1)) 166#define MPU6050_EXT_SYNC_SET_3 (bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) 167#define MPU6050_EXT_SYNC_SET_4 (bit(MPU6050_EXT_SYNC_SET2)) 168#define MPU6050_EXT_SYNC_SET_5 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET0)) 169#define MPU6050_EXT_SYNC_SET_6 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)) 170#define MPU6050_EXT_SYNC_SET_7 (bit(MPU6050_EXT_SYNC_SET2)|bit(MPU6050_EXT_SYNC_SET1)|bit(MPU6050_EXT_SYNC_SET0)) 171 172// Alternative names for the combined definitions. 173#define MPU6050_EXT_SYNC_DISABLED MPU6050_EXT_SYNC_SET_0 174#define MPU6050_EXT_SYNC_TEMP_OUT_L MPU6050_EXT_SYNC_SET_1 175#define MPU6050_EXT_SYNC_GYRO_XOUT_L MPU6050_EXT_SYNC_SET_2 176#define MPU6050_EXT_SYNC_GYRO_YOUT_L MPU6050_EXT_SYNC_SET_3 177#define MPU6050_EXT_SYNC_GYRO_ZOUT_L MPU6050_EXT_SYNC_SET_4 178#define MPU6050_EXT_SYNC_ACCEL_XOUT_L MPU6050_EXT_SYNC_SET_5 179#define MPU6050_EXT_SYNC_ACCEL_YOUT_L MPU6050_EXT_SYNC_SET_6 180#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L MPU6050_EXT_SYNC_SET_7 181 182// Combined definitions for the DLPF_CFG values 183#define MPU6050_DLPF_CFG_0 (0) 184#define MPU6050_DLPF_CFG_1 (bit(MPU6050_DLPF_CFG0)) 185#define MPU6050_DLPF_CFG_2 (bit(MPU6050_DLPF_CFG1)) 186#define MPU6050_DLPF_CFG_3 (bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) 187#define MPU6050_DLPF_CFG_4 (bit(MPU6050_DLPF_CFG2)) 188#define MPU6050_DLPF_CFG_5 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG0)) 189#define MPU6050_DLPF_CFG_6 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)) 190#define MPU6050_DLPF_CFG_7 (bit(MPU6050_DLPF_CFG2)|bit(MPU6050_DLPF_CFG1)|bit(MPU6050_DLPF_CFG0)) 191 192// Alternative names for the combined definitions 193// This name uses the bandwidth (Hz) for the accelometer, 194// for the gyro the bandwidth is almost the same. 195#define MPU6050_DLPF_260HZ MPU6050_DLPF_CFG_0 196#define MPU6050_DLPF_184HZ MPU6050_DLPF_CFG_1 197#define MPU6050_DLPF_94HZ MPU6050_DLPF_CFG_2 198#define MPU6050_DLPF_44HZ MPU6050_DLPF_CFG_3 199#define MPU6050_DLPF_21HZ MPU6050_DLPF_CFG_4 200#define MPU6050_DLPF_10HZ MPU6050_DLPF_CFG_5 201#define MPU6050_DLPF_5HZ MPU6050_DLPF_CFG_6 202#define MPU6050_DLPF_RESERVED MPU6050_DLPF_CFG_7 203 204// GYRO_CONFIG Register 205// The XG_ST, YG_ST, ZG_ST are bits for selftest. 206// The FS_SEL sets the range for the gyro. 207// These are the names for the bits. 208// Use these only with the bit() macro. 209#define MPU6050_FS_SEL0 MPU6050_D3 210#define MPU6050_FS_SEL1 MPU6050_D4 211#define MPU6050_ZG_ST MPU6050_D5 212#define MPU6050_YG_ST MPU6050_D6 213#define MPU6050_XG_ST MPU6050_D7 214 215// Combined definitions for the FS_SEL values 216#define MPU6050_FS_SEL_0 (0) 217#define MPU6050_FS_SEL_1 (bit(MPU6050_FS_SEL0)) 218#define MPU6050_FS_SEL_2 (bit(MPU6050_FS_SEL1)) 219#define MPU6050_FS_SEL_3 (bit(MPU6050_FS_SEL1)|bit(MPU6050_FS_SEL0)) 220 221// Alternative names for the combined definitions 222// The name uses the range in degrees per second. 223#define MPU6050_FS_SEL_250 MPU6050_FS_SEL_0 224#define MPU6050_FS_SEL_500 MPU6050_FS_SEL_1 225#define MPU6050_FS_SEL_1000 MPU6050_FS_SEL_2 226#define MPU6050_FS_SEL_2000 MPU6050_FS_SEL_3 227 228// ACCEL_CONFIG Register 229// The XA_ST, YA_ST, ZA_ST are bits for selftest. 230// The AFS_SEL sets the range for the accelerometer. 231// These are the names for the bits. 232// Use these only with the bit() macro. 233#define MPU6050_ACCEL_HPF0 MPU6050_D0 234#define MPU6050_ACCEL_HPF1 MPU6050_D1 235#define MPU6050_ACCEL_HPF2 MPU6050_D2 236#define MPU6050_AFS_SEL0 MPU6050_D3 237#define MPU6050_AFS_SEL1 MPU6050_D4 238#define MPU6050_ZA_ST MPU6050_D5 239#define MPU6050_YA_ST MPU6050_D6 240#define MPU6050_XA_ST MPU6050_D7 241 242// Combined definitions for the ACCEL_HPF values 243#define MPU6050_ACCEL_HPF_0 (0) 244#define MPU6050_ACCEL_HPF_1 (bit(MPU6050_ACCEL_HPF0)) 245#define MPU6050_ACCEL_HPF_2 (bit(MPU6050_ACCEL_HPF1)) 246#define MPU6050_ACCEL_HPF_3 (bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) 247#define MPU6050_ACCEL_HPF_4 (bit(MPU6050_ACCEL_HPF2)) 248#define MPU6050_ACCEL_HPF_7 (bit(MPU6050_ACCEL_HPF2)|bit(MPU6050_ACCEL_HPF1)|bit(MPU6050_ACCEL_HPF0)) 249 250// Alternative names for the combined definitions 251// The name uses the Cut-off frequency. 252#define MPU6050_ACCEL_HPF_RESET MPU6050_ACCEL_HPF_0 253#define MPU6050_ACCEL_HPF_5HZ MPU6050_ACCEL_HPF_1 254#define MPU6050_ACCEL_HPF_2_5HZ MPU6050_ACCEL_HPF_2 255#define MPU6050_ACCEL_HPF_1_25HZ MPU6050_ACCEL_HPF_3 256#define MPU6050_ACCEL_HPF_0_63HZ MPU6050_ACCEL_HPF_4 257#define MPU6050_ACCEL_HPF_HOLD MPU6050_ACCEL_HPF_7 258 259// Combined definitions for the AFS_SEL values 260#define MPU6050_AFS_SEL_0 (0) 261#define MPU6050_AFS_SEL_1 (bit(MPU6050_AFS_SEL0)) 262#define MPU6050_AFS_SEL_2 (bit(MPU6050_AFS_SEL1)) 263#define MPU6050_AFS_SEL_3 (bit(MPU6050_AFS_SEL1)|bit(MPU6050_AFS_SEL0)) 264 265// Alternative names for the combined definitions 266// The name uses the full scale range for the accelerometer. 267#define MPU6050_AFS_SEL_2G MPU6050_AFS_SEL_0 268#define MPU6050_AFS_SEL_4G MPU6050_AFS_SEL_1 269#define MPU6050_AFS_SEL_8G MPU6050_AFS_SEL_2 270#define MPU6050_AFS_SEL_16G MPU6050_AFS_SEL_3 271 272// FIFO_EN Register 273// These are the names for the bits. 274// Use these only with the bit() macro. 275#define MPU6050_SLV0_FIFO_EN MPU6050_D0 276#define MPU6050_SLV1_FIFO_EN MPU6050_D1 277#define MPU6050_SLV2_FIFO_EN MPU6050_D2 278#define MPU6050_ACCEL_FIFO_EN MPU6050_D3 279#define MPU6050_ZG_FIFO_EN MPU6050_D4 280#define MPU6050_YG_FIFO_EN MPU6050_D5 281#define MPU6050_XG_FIFO_EN MPU6050_D6 282#define MPU6050_TEMP_FIFO_EN MPU6050_D7 283 284// I2C_MST_CTRL Register 285// These are the names for the bits. 286// Use these only with the bit() macro. 287#define MPU6050_I2C_MST_CLK0 MPU6050_D0 288#define MPU6050_I2C_MST_CLK1 MPU6050_D1 289#define MPU6050_I2C_MST_CLK2 MPU6050_D2 290#define MPU6050_I2C_MST_CLK3 MPU6050_D3 291#define MPU6050_I2C_MST_P_NSR MPU6050_D4 292#define MPU6050_SLV_3_FIFO_EN MPU6050_D5 293#define MPU6050_WAIT_FOR_ES MPU6050_D6 294#define MPU6050_MULT_MST_EN MPU6050_D7 295 296// Combined definitions for the I2C_MST_CLK 297#define MPU6050_I2C_MST_CLK_0 (0) 298#define MPU6050_I2C_MST_CLK_1 (bit(MPU6050_I2C_MST_CLK0)) 299#define MPU6050_I2C_MST_CLK_2 (bit(MPU6050_I2C_MST_CLK1)) 300#define MPU6050_I2C_MST_CLK_3 (bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 301#define MPU6050_I2C_MST_CLK_4 (bit(MPU6050_I2C_MST_CLK2)) 302#define MPU6050_I2C_MST_CLK_5 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) 303#define MPU6050_I2C_MST_CLK_6 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) 304#define MPU6050_I2C_MST_CLK_7 (bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 305#define MPU6050_I2C_MST_CLK_8 (bit(MPU6050_I2C_MST_CLK3)) 306#define MPU6050_I2C_MST_CLK_9 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK0)) 307#define MPU6050_I2C_MST_CLK_10 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)) 308#define MPU6050_I2C_MST_CLK_11 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 309#define MPU6050_I2C_MST_CLK_12 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)) 310#define MPU6050_I2C_MST_CLK_13 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK0)) 311#define MPU6050_I2C_MST_CLK_14 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)) 312#define MPU6050_I2C_MST_CLK_15 (bit(MPU6050_I2C_MST_CLK3)|bit(MPU6050_I2C_MST_CLK2)|bit(MPU6050_I2C_MST_CLK1)|bit(MPU6050_I2C_MST_CLK0)) 313 314// Alternative names for the combined definitions 315// The names uses I2C Master Clock Speed in kHz. 316#define MPU6050_I2C_MST_CLK_348KHZ MPU6050_I2C_MST_CLK_0 317#define MPU6050_I2C_MST_CLK_333KHZ MPU6050_I2C_MST_CLK_1 318#define MPU6050_I2C_MST_CLK_320KHZ MPU6050_I2C_MST_CLK_2 319#define MPU6050_I2C_MST_CLK_308KHZ MPU6050_I2C_MST_CLK_3 320#define MPU6050_I2C_MST_CLK_296KHZ MPU6050_I2C_MST_CLK_4 321#define MPU6050_I2C_MST_CLK_286KHZ MPU6050_I2C_MST_CLK_5 322#define MPU6050_I2C_MST_CLK_276KHZ MPU6050_I2C_MST_CLK_6 323#define MPU6050_I2C_MST_CLK_267KHZ MPU6050_I2C_MST_CLK_7 324#define MPU6050_I2C_MST_CLK_258KHZ MPU6050_I2C_MST_CLK_8 325#define MPU6050_I2C_MST_CLK_500KHZ MPU6050_I2C_MST_CLK_9 326#define MPU6050_I2C_MST_CLK_471KHZ MPU6050_I2C_MST_CLK_10 327#define MPU6050_I2C_MST_CLK_444KHZ MPU6050_I2C_MST_CLK_11 328#define MPU6050_I2C_MST_CLK_421KHZ MPU6050_I2C_MST_CLK_12 329#define MPU6050_I2C_MST_CLK_400KHZ MPU6050_I2C_MST_CLK_13 330#define MPU6050_I2C_MST_CLK_381KHZ MPU6050_I2C_MST_CLK_14 331#define MPU6050_I2C_MST_CLK_364KHZ MPU6050_I2C_MST_CLK_15 332 333// I2C_SLV0_ADDR Register 334// These are the names for the bits. 335// Use these only with the bit() macro. 336#define MPU6050_I2C_SLV0_RW MPU6050_D7 337 338// I2C_SLV0_CTRL Register 339// These are the names for the bits. 340// Use these only with the bit() macro. 341#define MPU6050_I2C_SLV0_LEN0 MPU6050_D0 342#define MPU6050_I2C_SLV0_LEN1 MPU6050_D1 343#define MPU6050_I2C_SLV0_LEN2 MPU6050_D2 344#define MPU6050_I2C_SLV0_LEN3 MPU6050_D3 345#define MPU6050_I2C_SLV0_GRP MPU6050_D4 346#define MPU6050_I2C_SLV0_REG_DIS MPU6050_D5 347#define MPU6050_I2C_SLV0_BYTE_SW MPU6050_D6 348#define MPU6050_I2C_SLV0_EN MPU6050_D7 349 350// A mask for the length 351#define MPU6050_I2C_SLV0_LEN_MASK 0x0F 352 353// I2C_SLV1_ADDR Register 354// These are the names for the bits. 355// Use these only with the bit() macro. 356#define MPU6050_I2C_SLV1_RW MPU6050_D7 357 358// I2C_SLV1_CTRL Register 359// These are the names for the bits. 360// Use these only with the bit() macro. 361#define MPU6050_I2C_SLV1_LEN0 MPU6050_D0 362#define MPU6050_I2C_SLV1_LEN1 MPU6050_D1 363#define MPU6050_I2C_SLV1_LEN2 MPU6050_D2 364#define MPU6050_I2C_SLV1_LEN3 MPU6050_D3 365#define MPU6050_I2C_SLV1_GRP MPU6050_D4 366#define MPU6050_I2C_SLV1_REG_DIS MPU6050_D5 367#define MPU6050_I2C_SLV1_BYTE_SW MPU6050_D6 368#define MPU6050_I2C_SLV1_EN MPU6050_D7 369 370// A mask for the length 371#define MPU6050_I2C_SLV1_LEN_MASK 0x0F 372 373// I2C_SLV2_ADDR Register 374// These are the names for the bits. 375// Use these only with the bit() macro. 376#define MPU6050_I2C_SLV2_RW MPU6050_D7 377 378// I2C_SLV2_CTRL Register 379// These are the names for the bits. 380// Use these only with the bit() macro. 381#define MPU6050_I2C_SLV2_LEN0 MPU6050_D0 382#define MPU6050_I2C_SLV2_LEN1 MPU6050_D1 383#define MPU6050_I2C_SLV2_LEN2 MPU6050_D2 384#define MPU6050_I2C_SLV2_LEN3 MPU6050_D3 385#define MPU6050_I2C_SLV2_GRP MPU6050_D4 386#define MPU6050_I2C_SLV2_REG_DIS MPU6050_D5 387#define MPU6050_I2C_SLV2_BYTE_SW MPU6050_D6 388#define MPU6050_I2C_SLV2_EN MPU6050_D7 389 390// A mask for the length 391#define MPU6050_I2C_SLV2_LEN_MASK 0x0F 392 393// I2C_SLV3_ADDR Register 394// These are the names for the bits. 395// Use these only with the bit() macro. 396#define MPU6050_I2C_SLV3_RW MPU6050_D7 397 398// I2C_SLV3_CTRL Register 399// These are the names for the bits. 400// Use these only with the bit() macro. 401#define MPU6050_I2C_SLV3_LEN0 MPU6050_D0 402#define MPU6050_I2C_SLV3_LEN1 MPU6050_D1 403#define MPU6050_I2C_SLV3_LEN2 MPU6050_D2 404#define MPU6050_I2C_SLV3_LEN3 MPU6050_D3 405#define MPU6050_I2C_SLV3_GRP MPU6050_D4 406#define MPU6050_I2C_SLV3_REG_DIS MPU6050_D5 407#define MPU6050_I2C_SLV3_BYTE_SW MPU6050_D6 408#define MPU6050_I2C_SLV3_EN MPU6050_D7 409 410// A mask for the length 411#define MPU6050_I2C_SLV3_LEN_MASK 0x0F 412 413// I2C_SLV4_ADDR Register 414// These are the names for the bits. 415// Use these only with the bit() macro. 416#define MPU6050_I2C_SLV4_RW MPU6050_D7 417 418// I2C_SLV4_CTRL Register 419// These are the names for the bits. 420// Use these only with the bit() macro. 421#define MPU6050_I2C_MST_DLY0 MPU6050_D0 422#define MPU6050_I2C_MST_DLY1 MPU6050_D1 423#define MPU6050_I2C_MST_DLY2 MPU6050_D2 424#define MPU6050_I2C_MST_DLY3 MPU6050_D3 425#define MPU6050_I2C_MST_DLY4 MPU6050_D4 426#define MPU6050_I2C_SLV4_REG_DIS MPU6050_D5 427#define MPU6050_I2C_SLV4_INT_EN MPU6050_D6 428#define MPU6050_I2C_SLV4_EN MPU6050_D7 429 430// A mask for the delay 431#define MPU6050_I2C_MST_DLY_MASK 0x1F 432 433// I2C_MST_STATUS Register 434// These are the names for the bits. 435// Use these only with the bit() macro. 436#define MPU6050_I2C_SLV0_NACK MPU6050_D0 437#define MPU6050_I2C_SLV1_NACK MPU6050_D1 438#define MPU6050_I2C_SLV2_NACK MPU6050_D2 439#define MPU6050_I2C_SLV3_NACK MPU6050_D3 440#define MPU6050_I2C_SLV4_NACK MPU6050_D4 441#define MPU6050_I2C_LOST_ARB MPU6050_D5 442#define MPU6050_I2C_SLV4_DONE MPU6050_D6 443#define MPU6050_PASS_THROUGH MPU6050_D7 444 445// I2C_PIN_CFG Register 446// These are the names for the bits. 447// Use these only with the bit() macro. 448#define MPU6050_CLKOUT_EN MPU6050_D0 449#define MPU6050_I2C_BYPASS_EN MPU6050_D1 450#define MPU6050_FSYNC_INT_EN MPU6050_D2 451#define MPU6050_FSYNC_INT_LEVEL MPU6050_D3 452#define MPU6050_INT_RD_CLEAR MPU6050_D4 453#define MPU6050_LATCH_INT_EN MPU6050_D5 454#define MPU6050_INT_OPEN MPU6050_D6 455#define MPU6050_INT_LEVEL MPU6050_D7 456 457// INT_ENABLE Register 458// These are the names for the bits. 459// Use these only with the bit() macro. 460#define MPU6050_DATA_RDY_EN MPU6050_D0 461#define MPU6050_I2C_MST_INT_EN MPU6050_D3 462#define MPU6050_FIFO_OFLOW_EN MPU6050_D4 463#define MPU6050_ZMOT_EN MPU6050_D5 464#define MPU6050_MOT_EN MPU6050_D6 465#define MPU6050_FF_EN MPU6050_D7 466 467// INT_STATUS Register 468// These are the names for the bits. 469// Use these only with the bit() macro. 470#define MPU6050_DATA_RDY_INT MPU6050_D0 471#define MPU6050_I2C_MST_INT MPU6050_D3 472#define MPU6050_FIFO_OFLOW_INT MPU6050_D4 473#define MPU6050_ZMOT_INT MPU6050_D5 474#define MPU6050_MOT_INT MPU6050_D6 475#define MPU6050_FF_INT MPU6050_D7 476 477// MOT_DETECT_STATUS Register 478// These are the names for the bits. 479// Use these only with the bit() macro. 480#define MPU6050_MOT_ZRMOT MPU6050_D0 481#define MPU6050_MOT_ZPOS MPU6050_D2 482#define MPU6050_MOT_ZNEG MPU6050_D3 483#define MPU6050_MOT_YPOS MPU6050_D4 484#define MPU6050_MOT_YNEG MPU6050_D5 485#define MPU6050_MOT_XPOS MPU6050_D6 486#define MPU6050_MOT_XNEG MPU6050_D7 487 488// IC2_MST_DELAY_CTRL Register 489// These are the names for the bits. 490// Use these only with the bit() macro. 491#define MPU6050_I2C_SLV0_DLY_EN MPU6050_D0 492#define MPU6050_I2C_SLV1_DLY_EN MPU6050_D1 493#define MPU6050_I2C_SLV2_DLY_EN MPU6050_D2 494#define MPU6050_I2C_SLV3_DLY_EN MPU6050_D3 495#define MPU6050_I2C_SLV4_DLY_EN MPU6050_D4 496#define MPU6050_DELAY_ES_SHADOW MPU6050_D7 497 498// SIGNAL_PATH_RESET Register 499// These are the names for the bits. 500// Use these only with the bit() macro. 501#define MPU6050_TEMP_RESET MPU6050_D0 502#define MPU6050_ACCEL_RESET MPU6050_D1 503#define MPU6050_GYRO_RESET MPU6050_D2 504 505// MOT_DETECT_CTRL Register 506// These are the names for the bits. 507// Use these only with the bit() macro. 508#define MPU6050_MOT_COUNT0 MPU6050_D0 509#define MPU6050_MOT_COUNT1 MPU6050_D1 510#define MPU6050_FF_COUNT0 MPU6050_D2 511#define MPU6050_FF_COUNT1 MPU6050_D3 512#define MPU6050_ACCEL_ON_DELAY0 MPU6050_D4 513#define MPU6050_ACCEL_ON_DELAY1 MPU6050_D5 514 515// Combined definitions for the MOT_COUNT 516#define MPU6050_MOT_COUNT_0 (0) 517#define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0)) 518#define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1)) 519#define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0)) 520 521// Alternative names for the combined definitions 522#define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0 523 524// Combined definitions for the FF_COUNT 525#define MPU6050_FF_COUNT_0 (0) 526#define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0)) 527#define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1)) 528#define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0)) 529 530// Alternative names for the combined definitions 531#define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0 532 533// Combined definitions for the ACCEL_ON_DELAY 534#define MPU6050_ACCEL_ON_DELAY_0 (0) 535#define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0)) 536#define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1)) 537#define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0)) 538 539// Alternative names for the ACCEL_ON_DELAY 540#define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0 541#define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1 542#define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2 543#define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3 544 545// USER_CTRL Register 546// These are the names for the bits. 547// Use these only with the bit() macro. 548#define MPU6050_SIG_COND_RESET MPU6050_D0 549#define MPU6050_I2C_MST_RESET MPU6050_D1 550#define MPU6050_FIFO_RESET MPU6050_D2 551#define MPU6050_I2C_IF_DIS MPU6050_D4 // must be 0 for MPU-6050 552#define MPU6050_I2C_MST_EN MPU6050_D5 553#define MPU6050_FIFO_EN MPU6050_D6 554 555// PWR_MGMT_1 Register 556// These are the names for the bits. 557// Use these only with the bit() macro. 558#define MPU6050_CLKSEL0 MPU6050_D0 559#define MPU6050_CLKSEL1 MPU6050_D1 560#define MPU6050_CLKSEL2 MPU6050_D2 561#define MPU6050_TEMP_DIS MPU6050_D3 // 1: disable temperature sensor 562#define MPU6050_CYCLE MPU6050_D5 // 1: sample and sleep 563#define MPU6050_SLEEP MPU6050_D6 // 1: sleep mode 564#define MPU6050_DEVICE_RESET MPU6050_D7 // 1: reset to default values 565 566// Combined definitions for the CLKSEL 567#define MPU6050_CLKSEL_0 (0) 568#define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0)) 569#define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1)) 570#define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) 571#define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2)) 572#define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0)) 573#define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)) 574#define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) 575 576// Alternative names for the combined definitions 577#define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0 578#define MPU6050_CLKSEL_X MPU6050_CLKSEL_1 579#define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2 580#define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3 581#define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4 582#define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5 583#define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6 584#define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7 585 586// PWR_MGMT_2 Register 587// These are the names for the bits. 588// Use these only with the bit() macro. 589#define MPU6050_STBY_ZG MPU6050_D0 590#define MPU6050_STBY_YG MPU6050_D1 591#define MPU6050_STBY_XG MPU6050_D2 592#define MPU6050_STBY_ZA MPU6050_D3 593#define MPU6050_STBY_YA MPU6050_D4 594#define MPU6050_STBY_XA MPU6050_D5 595#define MPU6050_LP_WAKE_CTRL0 MPU6050_D6 596#define MPU6050_LP_WAKE_CTRL1 MPU6050_D7 597 598// Combined definitions for the LP_WAKE_CTRL 599#define MPU6050_LP_WAKE_CTRL_0 (0) 600#define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0)) 601#define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1)) 602#define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0)) 603 604// Alternative names for the combined definitions 605// The names uses the Wake-up Frequency. 606#define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0 607#define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1 608#define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2 609#define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3 610 611 612// Default I2C address for the MPU-6050 is 0x68. 613// But only if the AD0 pin is low. 614// Some sensor boards have AD0 high, and the 615// I2C address thus becomes 0x69. 616#define MPU6050_I2C_ADDRESS 0x68 617 618 619// Declaring an union for the registers and the axis values. 620// The byte order does not match the byte order of 621// the compiler and AVR chip. 622// The AVR chip (on the Arduino board) has the Low Byte 623// at the lower address. 624// But the MPU-6050 has a different order: High Byte at 625// lower address, so that has to be corrected. 626// The register part "reg" is only used internally, 627// and are swapped in code. 628typedef union accel_t_gyro_union 629{ 630 struct 631 { 632 uint8_t x_accel_h; 633 uint8_t x_accel_l; 634 uint8_t y_accel_h; 635 uint8_t y_accel_l; 636 uint8_t z_accel_h; 637 uint8_t z_accel_l; 638 uint8_t t_h; 639 uint8_t t_l; 640 uint8_t x_gyro_h; 641 uint8_t x_gyro_l; 642 uint8_t y_gyro_h; 643 uint8_t y_gyro_l; 644 uint8_t z_gyro_h; 645 uint8_t z_gyro_l; 646 } reg; 647 struct 648 { 649 int x_accel; 650 int y_accel; 651 int z_accel; 652 int temperature; 653 int x_gyro; 654 int y_gyro; 655 int z_gyro; 656 } value; 657}; 658 659// Use the following global variables and access functions to help store the overall 660// rotation angle of the sensor 661unsigned long last_read_time; 662float last_x_angle; // These are the filtered angles 663float last_y_angle; 664float last_z_angle; 665float last_gyro_x_angle; // Store the gyro angles to compare drift 666float last_gyro_y_angle; 667float last_gyro_z_angle; 668float last_x_kalangle; //Kalman angle 669float last_y_kalangle; 670float last_z_kalangle; 671 672void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro, float kx, float ky, float kz) { 673 last_read_time = time; 674 last_x_angle = x; 675 last_y_angle = y; 676 last_z_angle = z; 677 last_gyro_x_angle = x_gyro; 678 last_gyro_y_angle = y_gyro; 679 last_gyro_z_angle = z_gyro; 680 last_x_kalangle = kx; 681 last_y_kalangle = ky; 682 last_z_kalangle = kz; 683} 684 685inline unsigned long get_last_time() {return last_read_time;} 686inline float get_last_x_angle() {return last_x_angle;} 687inline float get_last_y_angle() {return last_y_angle;} 688inline float get_last_z_angle() {return last_z_angle;} 689inline float get_last_gyro_x_angle() {return last_gyro_x_angle;} 690inline float get_last_gyro_y_angle() {return last_gyro_y_angle;} 691inline float get_last_gyro_z_angle() {return last_gyro_z_angle;} 692inline float get_last_x_kalangle() {return last_x_kalangle;} 693inline float get_last_y_kalangle() {return last_y_kalangle;} 694inline float get_last_z_kalangle() {return last_z_kalangle;} 695 696// Use the following global variables and access functions 697// to calibrate the acceleration sensor 698float base_x_accel; 699float base_y_accel; 700float base_z_accel; 701 702float base_x_gyro; 703float base_y_gyro; 704float base_z_gyro; 705 706 707int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) { 708 // Read the raw values. 709 // Read 14 bytes at once, 710 // containing acceleration, temperature and gyro. 711 // With the default settings of the MPU-6050, 712 // there is no filter enabled, and the values 713 // are not very stable. Returns the error value 714 715 accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr; 716 717 int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro)); 718 719 // Swap all high and low bytes. 720 // After this, the registers values are swapped, 721 // so the structure name like x_accel_l does no 722 // longer contain the lower byte. 723 uint8_t swap; 724 #define SWAP(x,y) swap = x; x = y; y = swap 725 726 SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l); 727 SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l); 728 SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l); 729 SWAP ((*accel_t_gyro).reg.t_h, (*accel_t_gyro).reg.t_l); 730 SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l); 731 SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l); 732 SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l); 733 734 return error; 735} 736 737// The sensor should be motionless on a horizontal surface 738// while calibration is happening 739void calibrate_sensors() { 740 int num_readings = 10; 741 float x_accel = 0; 742 float y_accel = 0; 743 float z_accel = 0; 744 float x_gyro = 0; 745 float y_gyro = 0; 746 float z_gyro = 0; 747 accel_t_gyro_union accel_t_gyro; 748 749 //Serial.println("Starting Calibration"); 750 751 // Discard the first set of values read from the IMU 752 read_gyro_accel_vals((uint8_t *) &accel_t_gyro); 753 754 // Read and average the raw values from the IMU 755 for (int i = 0; i < num_readings; i++) { 756 read_gyro_accel_vals((uint8_t *) &accel_t_gyro); 757 x_accel += accel_t_gyro.value.x_accel; 758 y_accel += accel_t_gyro.value.y_accel; 759 z_accel += accel_t_gyro.value.z_accel; 760 x_gyro += accel_t_gyro.value.x_gyro; 761 y_gyro += accel_t_gyro.value.y_gyro; 762 z_gyro += accel_t_gyro.value.z_gyro; 763 delay(100); 764 } 765 x_accel /= num_readings; 766 y_accel /= num_readings; 767 z_accel /= num_readings; 768 x_gyro /= num_readings; 769 y_gyro /= num_readings; 770 z_gyro /= num_readings; 771 772 // Store the raw calibration values globally 773 base_x_accel = x_accel; 774 base_y_accel = y_accel; 775 base_z_accel = z_accel; 776 base_x_gyro = x_gyro; 777 base_y_gyro = y_gyro; 778 base_z_gyro = z_gyro; 779 780 //Serial.println("Finishing Calibration"); 781} 782 783 784/************************ 785* KALMAN - Filter setup * 786*************************/ 787class Kalman { 788public: 789 Kalman() { 790 /* We will set the varibles like so, these can also be tuned by the user */ 791 Q_angle = 0.001; 792 Q_bias = 0.003; 793 R_measure = 0.03; 794 795 bias = 0; // Reset bias 796 P[0][0] = 0; // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 797 P[0][1] = 0; 798 P[1][0] = 0; 799 P[1][1] = 0; 800 }; 801 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 802 float getAngle(float newAngle, float newRate, float dt) { 803 // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 804 // Modified by Kristian Lauszus 805 // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 806 807 // Discrete Kalman filter time update equations - Time Update ("Predict") 808 // Update xhat - Project the state ahead 809 /* Step 1 */ 810 rate = newRate - bias; 811 angle += dt * rate; 812 813 // Update estimation error covariance - Project the error covariance ahead 814 /* Step 2 */ 815 P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 816 P[0][1] -= dt * P[1][1]; 817 P[1][0] -= dt * P[1][1]; 818 P[1][1] += Q_bias * dt; 819 820 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 821 // Calculate Kalman gain - Compute the Kalman gain 822 /* Step 4 */ 823 S = P[0][0] + R_measure; 824 /* Step 5 */ 825 K[0] = P[0][0] / S; 826 K[1] = P[1][0] / S; 827 828 // Calculate angle and bias - Update estimate with measurement zk (newAngle) 829 /* Step 3 */ 830 y = newAngle - angle; 831 /* Step 6 */ 832 angle += K[0] * y; 833 bias += K[1] * y; 834 835 // Calculate estimation error covariance - Update the error covariance 836 /* Step 7 */ 837 P[0][0] -= K[0] * P[0][0]; 838 P[0][1] -= K[0] * P[0][1]; 839 P[1][0] -= K[1] * P[0][0]; 840 P[1][1] -= K[1] * P[0][1]; 841 842 return angle; 843 }; 844 void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 845 double getRate() { return rate; }; // Return the unbiased rate 846 847 /* These are used to tune the Kalman filter */ 848 void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 849 void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 850 void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 851 852 double getQangle() { return Q_angle; }; 853 double getQbias() { return Q_bias; }; 854 double getRmeasure() { return R_measure; }; 855 856private: 857 /* Kalman filter variables */ 858 double Q_angle; // Process noise variance for the accelerometer 859 double Q_bias; // Process noise variance for the gyro bias 860 double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 861 862 double angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix 863 double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix 864 double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 865 866 double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 867 double K[2]; // Kalman gain - This is a 2x1 matrix 868 double y; // Angle difference - 1x1 matrix 869 double S; // Estimate error - 1x1 matrix 870}; 871Kalman kalmanX; // Create the Kalman instances 872Kalman kalmanY; 873 874 875 876/******** 877* SETUP * 878*********/ 879 880void setup() 881{ 882 int error; 883 uint8_t c; 884 accel_t_gyro_union accel_t_gyro; 885 886 887 Serial.begin(19200); 888 889 // Initialize the 'Wire' class for the I2C-bus. 890 Wire.begin(); 891 892 893 // default at power-up: 894 // Gyro at 250 degrees second 895 // Acceleration at 2g 896 // Clock source at internal 8MHz 897 // The device is in sleep mode. 898 // 899 900 error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); 901 902 903 // According to the datasheet, the 'sleep' bit 904 // should read a '1'. But I read a '0'. 905 // That bit has to be cleared, since the sensor 906 // is in sleep mode at power-up. Even if the 907 // bit reads '0'. 908 error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1); 909 910 // Clear the 'sleep' bit to start the sensor. 911 MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); 912 913 //Initialize the angles 914 calibrate_sensors(); 915 set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0, 0, 0, 0); 916 917 918 // Get raw acceleration values 919 //float G_CONVERT = 16384; 920 float accel_x = accel_t_gyro.value.x_accel; 921 float accel_y = accel_t_gyro.value.y_accel; 922 float accel_z = accel_t_gyro.value.z_accel; 923 924 // Get angle values from accelerometer 925 float RADIANS_TO_DEGREES = 180/3.14159; 926// float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); 927 float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 928 float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 929 930 float accel_angle_z = 0; 931 932 kalmanX.setAngle(accel_angle_x); // Set starting angle 933 kalmanY.setAngle(accel_angle_y); 934 935} 936 937 938 939/************ 940* Main loop * 941*************/ 942void loop() 943{ 944 int error; 945 double dT; 946 accel_t_gyro_union accel_t_gyro; 947 948 float kalAngleX, kalAngleY, kalAngleZ; // Calculate the angle using a Kalman filter 949 950 // Read the raw values. 951 error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); 952 953 // Get the time of reading for rotation computations 954 unsigned long t_now = millis(); 955 956 957 // Convert gyro values to degrees/sec 958 float FS_SEL = 131; 959 960 float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL; 961 float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL; 962 float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL; 963 964 965 // Get raw acceleration values 966 //float G_CONVERT = 16384; 967 float accel_x = accel_t_gyro.value.x_accel; 968 float accel_y = accel_t_gyro.value.y_accel; 969 float accel_z = accel_t_gyro.value.z_accel; 970 971 // Get angle values from accelerometer 972 float RADIANS_TO_DEGREES = 180/3.14159; 973// float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); 974 float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 975 float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; 976 977 float accel_angle_z = 0; 978 979 // Compute the (filtered) gyro angles 980 float dt =(t_now - get_last_time())/1000.0; 981 float gyro_angle_x = gyro_x*dt + get_last_x_angle(); 982 float gyro_angle_y = gyro_y*dt + get_last_y_angle(); 983 float gyro_angle_z = gyro_z*dt + get_last_z_angle(); 984 985 // Compute the drifting gyro angles 986 float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); 987 float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); 988 float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); 989 990 // Apply the complementary filter to figure out the change in angle - choice of alpha is 991 // estimated now. Alpha depends on the sampling rate... 992 float alpha = 0.96; 993 float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; 994 float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; 995 float angle_z = gyro_angle_z; //Accelerometer doesn't give z-angle 996 997 998 //KALMAN filter 999 kalAngleX = kalmanX.getAngle(accel_angle_x, gyro_x, dt); 1000 //delay(5); 1001 kalAngleY = kalmanY.getAngle(accel_angle_y, gyro_y, dt); 1002 kalAngleZ = gyro_angle_z; 1003 1004 1005 // Update the saved data with the latest values 1006 set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z, kalAngleX, kalAngleY, kalAngleZ); 1007 1008 //Note added: 10/08/13 pad() creates padding that is required to ensure 128Bytes are sent. May not be needed as new version of LV code maybe able to handle 1009 1010 //Serial.print("START"); 1011 // Send the data to the serial port 1012 Serial.print(F("DEL:")); //Delta T 1013 Serial.print(dt, DEC); 1014 1015 Serial.print(F("#ACC:")); //Accelerometer angle 1016 pad(accel_angle_x); 1017 //Serial.print(accel_angle_x, 2); 1018 Serial.print(F(",")); 1019 pad(accel_angle_y); 1020 //Serial.print(accel_angle_y, 2); 1021 Serial.print(F(",")); 1022 pad(accel_angle_z); 1023 //Serial.print(accel_angle_z, 2); 1024 1025 Serial.print(F("#GYR:")); 1026 pad(unfiltered_gyro_angle_x); 1027 //Serial.print(unfiltered_gyro_angle_x, 2); //Gyroscope angle 1028 Serial.print(F(",")); 1029 pad(unfiltered_gyro_angle_y); 1030 //Serial.print(unfiltered_gyro_angle_y, 2); 1031 Serial.print(F(",")); 1032 pad(unfiltered_gyro_angle_z); 1033 //Serial.print(unfiltered_gyro_angle_z, 2); 1034 1035 Serial.print(F("#FIL:")); //Filtered angle 1036 pad(angle_x); 1037 //Serial.print(angle_x, 2); 1038 Serial.print(F(",")); 1039 pad(angle_y); 1040 //Serial.print(angle_y, 2); 1041 Serial.print(F(",")); 1042 pad(angle_z); 1043 //Serial.print(angle_z, 2); 1044 Serial.print(F(",")); 1045 1046 Serial.print(F("#KAL:")); //Kalman Filtered angle 1047 pad(kalAngleX); 1048 //Serial.print(kalAngleX, 2); 1049 Serial.print(F(",")); 1050 pad (kalAngleY); 1051 //Serial.print(kalAngleY, 2); 1052 Serial.print(F(",")); 1053 pad(kalAngleZ); 1054 //Serial.print(kalAngleZ, 2); 1055 //Serial.print("END"); 1056 Serial.print("\ 1057"); 1058 //Serial.println(F("")); 1059 Serial.flush(); 1060 // Delay so we don't swamp the serial port 1061 delay(5); //so not to swamp the serial port 1062} 1063 1064 1065// -------------------------------------------------------- 1066// MPU6050_read 1067// 1068// This is a common function to read multiple bytes 1069// from an I2C device. 1070// 1071// It uses the boolean parameter for Wire.endTransMission() 1072// to be able to hold or release the I2C-bus. 1073// This is implemented in Arduino 1.0.1. 1074// 1075// Only this function is used to read. 1076// There is no function for a single byte. 1077// 1078int MPU6050_read(int start, uint8_t *buffer, int size) 1079{ 1080 int i, n, error; 1081 1082 Wire.beginTransmission(MPU6050_I2C_ADDRESS); 1083 n = Wire.write(start); 1084 if (n != 1) 1085 return (-10); 1086 1087 n = Wire.endTransmission(false); // hold the I2C-bus 1088 if (n != 0) 1089 return (n); 1090 1091 // Third parameter is true: relase I2C-bus after data is read. 1092 Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true); 1093 i = 0; 1094 while(Wire.available() && i<size) 1095 { 1096 buffer[i++]=Wire.read(); 1097 } 1098 if ( i != size) 1099 return (-11); 1100 1101 return (0); // return : no error 1102} 1103 1104 1105// -------------------------------------------------------- 1106// MPU6050_write 1107// 1108// This is a common function to write multiple bytes to an I2C device. 1109// 1110// If only a single register is written, 1111// use the function MPU_6050_write_reg(). 1112// 1113// Parameters: 1114// start : Start address, use a define for the register 1115// pData : A pointer to the data to write. 1116// size : The number of bytes to write. 1117// 1118// If only a single register is written, a pointer 1119// to the data has to be used, and the size is 1120// a single byte: 1121// int data = 0; // the data to write 1122// MPU6050_write (MPU6050_PWR_MGMT_1, &c, 1); 1123// 1124int MPU6050_write(int start, const uint8_t *pData, int size) 1125{ 1126 int n, error; 1127 1128 Wire.beginTransmission(MPU6050_I2C_ADDRESS); 1129 n = Wire.write(start); // write the start address 1130 if (n != 1) 1131 return (-20); 1132 1133 n = Wire.write(pData, size); // write data bytes 1134 if (n != size) 1135 return (-21); 1136 1137 error = Wire.endTransmission(true); // release the I2C-bus 1138 if (error != 0) 1139 return (error); 1140 1141 return (0); // return : no error 1142} 1143 1144// -------------------------------------------------------- 1145// MPU6050_write_reg 1146// 1147// An extra function to write a single register. 1148// It is just a wrapper around the MPU_6050_write() 1149// function, and it is only a convenient function 1150// to make it easier to write a single register. 1151// 1152int MPU6050_write_reg(int reg, uint8_t data) 1153{ 1154 int error; 1155 1156 error = MPU6050_write(reg, &data, 1); 1157 1158 return (error); 1159} 1160 1161//This function pads the printed value to take up 1162//7 characters Example: **20.16 1163void pad(float PadVal){ 1164 1165 //Positve Value 1166 if (PadVal > 0){ //For positive value 1167 if(PadVal < 10){ //For positive value less than 10 1168 Serial.print("***"); //0-9 ***0-9.00 1169 } 1170 else if (PadVal < 100){ //values less than 100 1171 Serial.print("**"); //10-99 **10-99.00 1172 } 1173 1174 } 1175 else if(PadVal < 0){ //for negative values 1176 if(PadVal > -10){ //for negative value greater than -10 1177 Serial.print("**"); //-9 - 0 **-1 - -9.00 1178 } 1179 else if (PadVal > -100){ //for negative values lesss than -99 1180 Serial.print("*"); //*-99.00 1181 } 1182 } 1183 1184 Serial.print(PadVal, 2); //print the padded value 1185}
Downloadable files
Project Schematic Diagram
Project Schematic Diagram
Comments
Only logged in users can leave comments
tahiryasin5
0 Followers
•0 Projects
Table of contents
Intro
2
0