大 田澤 / Group_Control_No2_3actions

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MPU92502/MPU9250.cpp@0:65f9be2858c9, 2017-02-22 (annotated)

Committer:
tukudani
Date:
Wed Feb 22 08:59:47 2017 +0000
Revision:
0:65f9be2858c9
Phantom2 GroupControl

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tukudani 0:65f9be2858c9 1 /*CODED by Qiyong Mu on 21/06/2014
tukudani 0:65f9be2858c9 2 kylongmu@msn.com
tukudani 0:65f9be2858c9 3 */
tukudani 0:65f9be2858c9 4
tukudani 0:65f9be2858c9 5 #include <mbed.h>
tukudani 0:65f9be2858c9 6 #include "MPU9250.h"
tukudani 0:65f9be2858c9 7
tukudani 0:65f9be2858c9 8 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs)
tukudani 0:65f9be2858c9 9 {
tukudani 0:65f9be2858c9 10 /* uint8_t offset[3][2] =
tukudani 0:65f9be2858c9 11 {-21 , MPUREG_XG_OFFS_TC};
tukudani 0:65f9be2858c9 12 for(i=0; i<offset; i++) {
tukudani 0:65f9be2858c9 13 WriteReg(offset[i][1], offset[i][0]);
tukudani 0:65f9be2858c9 14 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
tukudani 0:65f9be2858c9 15 }*/
tukudani 0:65f9be2858c9 16 Magnetometer_offset[0]=0;
tukudani 0:65f9be2858c9 17 Magnetometer_offset[1]=0;
tukudani 0:65f9be2858c9 18 Magnetometer_offset[2]=0;
tukudani 0:65f9be2858c9 19 }
tukudani 0:65f9be2858c9 20
tukudani 0:65f9be2858c9 21 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
tukudani 0:65f9be2858c9 22 {
tukudani 0:65f9be2858c9 23 unsigned int temp_val;
tukudani 0:65f9be2858c9 24 select();
tukudani 0:65f9be2858c9 25 spi.write(WriteAddr);
tukudani 0:65f9be2858c9 26 temp_val=spi.write(WriteData);
tukudani 0:65f9be2858c9 27 deselect();
tukudani 0:65f9be2858c9 28 wait_us(50);
tukudani 0:65f9be2858c9 29 return temp_val;
tukudani 0:65f9be2858c9 30 }
tukudani 0:65f9be2858c9 31 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
tukudani 0:65f9be2858c9 32 {
tukudani 0:65f9be2858c9 33 return WriteReg(WriteAddr | READ_FLAG,WriteData);
tukudani 0:65f9be2858c9 34 }
tukudani 0:65f9be2858c9 35 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
tukudani 0:65f9be2858c9 36 {
tukudani 0:65f9be2858c9 37 unsigned int i = 0;
tukudani 0:65f9be2858c9 38
tukudani 0:65f9be2858c9 39 select();
tukudani 0:65f9be2858c9 40 spi.write(ReadAddr | READ_FLAG);
tukudani 0:65f9be2858c9 41 for(i=0; i<Bytes; i++)
tukudani 0:65f9be2858c9 42 ReadBuf[i] = spi.write(0x00);
tukudani 0:65f9be2858c9 43 deselect();
tukudani 0:65f9be2858c9 44 wait_us(50);
tukudani 0:65f9be2858c9 45 }
tukudani 0:65f9be2858c9 46
tukudani 0:65f9be2858c9 47 Timer deg_time;
tukudani 0:65f9be2858c9 48 void mpu9250_spi::Timer_start( void )
tukudani 0:65f9be2858c9 49 {
tukudani 0:65f9be2858c9 50 deg_time.start();
tukudani 0:65f9be2858c9 51 }
tukudani 0:65f9be2858c9 52
tukudani 0:65f9be2858c9 53 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 54 INITIALIZATION
tukudani 0:65f9be2858c9 55 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
tukudani 0:65f9be2858c9 56 low pass filter value; suitable values are:
tukudani 0:65f9be2858c9 57 BITS_DLPF_CFG_256HZ_NOLPF2
tukudani 0:65f9be2858c9 58 BITS_DLPF_CFG_188HZ
tukudani 0:65f9be2858c9 59 BITS_DLPF_CFG_98HZ
tukudani 0:65f9be2858c9 60 BITS_DLPF_CFG_42HZ
tukudani 0:65f9be2858c9 61 BITS_DLPF_CFG_20HZ
tukudani 0:65f9be2858c9 62 BITS_DLPF_CFG_10HZ
tukudani 0:65f9be2858c9 63 BITS_DLPF_CFG_5HZ
tukudani 0:65f9be2858c9 64 BITS_DLPF_CFG_2100HZ_NOLPF
tukudani 0:65f9be2858c9 65 returns 1 if an error occurred
tukudani 0:65f9be2858c9 66 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 67 #define MPU_InitRegNum 17
tukudani 0:65f9be2858c9 68
tukudani 0:65f9be2858c9 69 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter)
tukudani 0:65f9be2858c9 70 {
tukudani 0:65f9be2858c9 71 uint8_t i = 0;
tukudani 0:65f9be2858c9 72 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
tukudani 0:65f9be2858c9 73 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
tukudani 0:65f9be2858c9 74 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
tukudani 0:65f9be2858c9 75 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
tukudani 0:65f9be2858c9 76 {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
tukudani 0:65f9be2858c9 77 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
tukudani 0:65f9be2858c9 78 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G
tukudani 0:65f9be2858c9 79 {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
tukudani 0:65f9be2858c9 80 {0x30, MPUREG_INT_PIN_CFG}, //
tukudani 0:65f9be2858c9 81 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
tukudani 0:65f9be2858c9 82 //{0x20, MPUREG_USER_CTRL}, // Enable AUX
tukudani 0:65f9be2858c9 83 {0x20, MPUREG_USER_CTRL}, // I2C Master mode
tukudani 0:65f9be2858c9 84 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
tukudani 0:65f9be2858c9 85
tukudani 0:65f9be2858c9 86 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
tukudani 0:65f9be2858c9 87 //{0x09, MPUREG_I2C_SLV4_CTRL},
tukudani 0:65f9be2858c9 88 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
tukudani 0:65f9be2858c9 89
tukudani 0:65f9be2858c9 90 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
tukudani 0:65f9be2858c9 91 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
tukudani 0:65f9be2858c9 92 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
tukudani 0:65f9be2858c9 93
tukudani 0:65f9be2858c9 94 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
tukudani 0:65f9be2858c9 95 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
tukudani 0:65f9be2858c9 96 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
tukudani 0:65f9be2858c9 97
tukudani 0:65f9be2858c9 98 };
tukudani 0:65f9be2858c9 99 spi.format(8,0);
tukudani 0:65f9be2858c9 100 spi.frequency(1000000);
tukudani 0:65f9be2858c9 101
tukudani 0:65f9be2858c9 102 for(i=0; i<MPU_InitRegNum; i++) {
tukudani 0:65f9be2858c9 103 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
tukudani 0:65f9be2858c9 104 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
tukudani 0:65f9be2858c9 105 }
tukudani 0:65f9be2858c9 106
tukudani 0:65f9be2858c9 107 set_acc_scale(2);
tukudani 0:65f9be2858c9 108 set_gyro_scale(250);
tukudani 0:65f9be2858c9 109
tukudani 0:65f9be2858c9 110 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
tukudani 0:65f9be2858c9 111 return 0;
tukudani 0:65f9be2858c9 112 }
tukudani 0:65f9be2858c9 113 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 114 ACCELEROMETER SCALE
tukudani 0:65f9be2858c9 115 usage: call this function at startup, after initialization, to set the right range for the
tukudani 0:65f9be2858c9 116 accelerometers. Suitable ranges are:
tukudani 0:65f9be2858c9 117 BITS_FS_2G
tukudani 0:65f9be2858c9 118 BITS_FS_4G
tukudani 0:65f9be2858c9 119 BITS_FS_8G
tukudani 0:65f9be2858c9 120 BITS_FS_16G
tukudani 0:65f9be2858c9 121 returns the range set (2,4,8 or 16)
tukudani 0:65f9be2858c9 122 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 123 unsigned int mpu9250_spi::set_acc_scale(int scale)
tukudani 0:65f9be2858c9 124 {
tukudani 0:65f9be2858c9 125 unsigned int temp_scale;
tukudani 0:65f9be2858c9 126 WriteReg(MPUREG_ACCEL_CONFIG, scale);
tukudani 0:65f9be2858c9 127
tukudani 0:65f9be2858c9 128 switch (scale) {
tukudani 0:65f9be2858c9 129 case BITS_FS_2G:
tukudani 0:65f9be2858c9 130 acc_divider=16384;
tukudani 0:65f9be2858c9 131 break;
tukudani 0:65f9be2858c9 132 case BITS_FS_4G:
tukudani 0:65f9be2858c9 133 acc_divider=8192;
tukudani 0:65f9be2858c9 134 break;
tukudani 0:65f9be2858c9 135 case BITS_FS_8G:
tukudani 0:65f9be2858c9 136 acc_divider=4096;
tukudani 0:65f9be2858c9 137 break;
tukudani 0:65f9be2858c9 138 case BITS_FS_16G:
tukudani 0:65f9be2858c9 139 acc_divider=2048;
tukudani 0:65f9be2858c9 140 break;
tukudani 0:65f9be2858c9 141 }
tukudani 0:65f9be2858c9 142 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
tukudani 0:65f9be2858c9 143
tukudani 0:65f9be2858c9 144 switch (temp_scale) {
tukudani 0:65f9be2858c9 145 case BITS_FS_2G:
tukudani 0:65f9be2858c9 146 temp_scale=2;
tukudani 0:65f9be2858c9 147 break;
tukudani 0:65f9be2858c9 148 case BITS_FS_4G:
tukudani 0:65f9be2858c9 149 temp_scale=4;
tukudani 0:65f9be2858c9 150 break;
tukudani 0:65f9be2858c9 151 case BITS_FS_8G:
tukudani 0:65f9be2858c9 152 temp_scale=8;
tukudani 0:65f9be2858c9 153 break;
tukudani 0:65f9be2858c9 154 case BITS_FS_16G:
tukudani 0:65f9be2858c9 155 temp_scale=16;
tukudani 0:65f9be2858c9 156 break;
tukudani 0:65f9be2858c9 157 }
tukudani 0:65f9be2858c9 158 return temp_scale;
tukudani 0:65f9be2858c9 159 }
tukudani 0:65f9be2858c9 160
tukudani 0:65f9be2858c9 161
tukudani 0:65f9be2858c9 162 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 163 GYROSCOPE SCALE
tukudani 0:65f9be2858c9 164 usage: call this function at startup, after initialization, to set the right range for the
tukudani 0:65f9be2858c9 165 gyroscopes. Suitable ranges are:
tukudani 0:65f9be2858c9 166 BITS_FS_250DPS
tukudani 0:65f9be2858c9 167 BITS_FS_500DPS
tukudani 0:65f9be2858c9 168 BITS_FS_1000DPS
tukudani 0:65f9be2858c9 169 BITS_FS_2000DPS
tukudani 0:65f9be2858c9 170 returns the range set (250,500,1000 or 2000)
tukudani 0:65f9be2858c9 171 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 172 unsigned int mpu9250_spi::set_gyro_scale(int scale)
tukudani 0:65f9be2858c9 173 {
tukudani 0:65f9be2858c9 174 unsigned int temp_scale;
tukudani 0:65f9be2858c9 175 WriteReg(MPUREG_GYRO_CONFIG, scale);
tukudani 0:65f9be2858c9 176 switch (scale) {
tukudani 0:65f9be2858c9 177 case BITS_FS_250DPS:
tukudani 0:65f9be2858c9 178 gyro_divider=131;
tukudani 0:65f9be2858c9 179 break;
tukudani 0:65f9be2858c9 180 case BITS_FS_500DPS:
tukudani 0:65f9be2858c9 181 gyro_divider=65.5;
tukudani 0:65f9be2858c9 182 break;
tukudani 0:65f9be2858c9 183 case BITS_FS_1000DPS:
tukudani 0:65f9be2858c9 184 gyro_divider=32.8;
tukudani 0:65f9be2858c9 185 break;
tukudani 0:65f9be2858c9 186 case BITS_FS_2000DPS:
tukudani 0:65f9be2858c9 187 gyro_divider=16.4;
tukudani 0:65f9be2858c9 188 break;
tukudani 0:65f9be2858c9 189 }
tukudani 0:65f9be2858c9 190 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
tukudani 0:65f9be2858c9 191 switch (temp_scale) {
tukudani 0:65f9be2858c9 192 case BITS_FS_250DPS:
tukudani 0:65f9be2858c9 193 temp_scale=250;
tukudani 0:65f9be2858c9 194 break;
tukudani 0:65f9be2858c9 195 case BITS_FS_500DPS:
tukudani 0:65f9be2858c9 196 temp_scale=500;
tukudani 0:65f9be2858c9 197 break;
tukudani 0:65f9be2858c9 198 case BITS_FS_1000DPS:
tukudani 0:65f9be2858c9 199 temp_scale=1000;
tukudani 0:65f9be2858c9 200 break;
tukudani 0:65f9be2858c9 201 case BITS_FS_2000DPS:
tukudani 0:65f9be2858c9 202 temp_scale=2000;
tukudani 0:65f9be2858c9 203 break;
tukudani 0:65f9be2858c9 204 }
tukudani 0:65f9be2858c9 205 return temp_scale;
tukudani 0:65f9be2858c9 206 }
tukudani 0:65f9be2858c9 207
tukudani 0:65f9be2858c9 208
tukudani 0:65f9be2858c9 209 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 210 WHO AM I?
tukudani 0:65f9be2858c9 211 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
tukudani 0:65f9be2858c9 212 mpu9250 which should be 104 when in SPI mode.
tukudani 0:65f9be2858c9 213 returns the I2C address (104)
tukudani 0:65f9be2858c9 214 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 215 unsigned int mpu9250_spi::whoami()
tukudani 0:65f9be2858c9 216 {
tukudani 0:65f9be2858c9 217 unsigned int response;
tukudani 0:65f9be2858c9 218 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
tukudani 0:65f9be2858c9 219 return response;
tukudani 0:65f9be2858c9 220 }
tukudani 0:65f9be2858c9 221
tukudani 0:65f9be2858c9 222
tukudani 0:65f9be2858c9 223 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 224 READ ACCELEROMETER
tukudani 0:65f9be2858c9 225 usage: call this function to read accelerometer data. Axis represents selected axis:
tukudani 0:65f9be2858c9 226 0 -> X axis
tukudani 0:65f9be2858c9 227 1 -> Y axis
tukudani 0:65f9be2858c9 228 2 -> Z axis
tukudani 0:65f9be2858c9 229 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 230 void mpu9250_spi::read_acc()
tukudani 0:65f9be2858c9 231 {
tukudani 0:65f9be2858c9 232 uint8_t response[6];
tukudani 0:65f9be2858c9 233 int16_t bit_data;
tukudani 0:65f9be2858c9 234 float data;
tukudani 0:65f9be2858c9 235 int i;
tukudani 0:65f9be2858c9 236 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
tukudani 0:65f9be2858c9 237 for(i=0; i<3; i++) {
tukudani 0:65f9be2858c9 238 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 239 data=(float)bit_data;
tukudani 0:65f9be2858c9 240 accelerometer_data[i]=data/acc_divider;
tukudani 0:65f9be2858c9 241 /*accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i];
tukudani 0:65f9be2858c9 242 accelerometer_data_prev[i]=accelerometer_data[i];*/
tukudani 0:65f9be2858c9 243 }
tukudani 0:65f9be2858c9 244
tukudani 0:65f9be2858c9 245 }
tukudani 0:65f9be2858c9 246
tukudani 0:65f9be2858c9 247 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 248 READ GYROSCOPE
tukudani 0:65f9be2858c9 249 usage: call this function to read gyroscope data. Axis represents selected axis:
tukudani 0:65f9be2858c9 250 0 -> X axis
tukudani 0:65f9be2858c9 251 1 -> Y axis
tukudani 0:65f9be2858c9 252 2 -> Z axis
tukudani 0:65f9be2858c9 253 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 254 void mpu9250_spi::read_rot()
tukudani 0:65f9be2858c9 255 {
tukudani 0:65f9be2858c9 256 uint8_t response[6];
tukudani 0:65f9be2858c9 257 int16_t bit_data;
tukudani 0:65f9be2858c9 258 float data;
tukudani 0:65f9be2858c9 259 int i;
tukudani 0:65f9be2858c9 260 ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
tukudani 0:65f9be2858c9 261 for(i=0; i<3; i++) {
tukudani 0:65f9be2858c9 262 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 263 data=(float)bit_data;
tukudani 0:65f9be2858c9 264 gyroscope_data[i]=data;
tukudani 0:65f9be2858c9 265 }
tukudani 0:65f9be2858c9 266
tukudani 0:65f9be2858c9 267 }
tukudani 0:65f9be2858c9 268
tukudani 0:65f9be2858c9 269 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 270 READ TEMPERATURE
tukudani 0:65f9be2858c9 271 usage: call this function to read temperature data.
tukudani 0:65f9be2858c9 272 returns the value in °C
tukudani 0:65f9be2858c9 273 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 274 void mpu9250_spi::read_temp()
tukudani 0:65f9be2858c9 275 {
tukudani 0:65f9be2858c9 276 uint8_t response[2];
tukudani 0:65f9be2858c9 277 int16_t bit_data;
tukudani 0:65f9be2858c9 278 float data;
tukudani 0:65f9be2858c9 279 ReadRegs(MPUREG_TEMP_OUT_H,response,2);
tukudani 0:65f9be2858c9 280
tukudani 0:65f9be2858c9 281 bit_data=((int16_t)response[0]<<8)|response[1];
tukudani 0:65f9be2858c9 282 data=(float)bit_data;
tukudani 0:65f9be2858c9 283 Temperature=(data/340)+36.53;
tukudani 0:65f9be2858c9 284 deselect();
tukudani 0:65f9be2858c9 285 }
tukudani 0:65f9be2858c9 286
tukudani 0:65f9be2858c9 287 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 288 READ ACCELEROMETER CALIBRATION
tukudani 0:65f9be2858c9 289 usage: call this function to read accelerometer data. Axis represents selected axis:
tukudani 0:65f9be2858c9 290 0 -> X axis
tukudani 0:65f9be2858c9 291 1 -> Y axis
tukudani 0:65f9be2858c9 292 2 -> Z axis
tukudani 0:65f9be2858c9 293 returns Factory Trim value
tukudani 0:65f9be2858c9 294 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 295 void mpu9250_spi::calib_acc()
tukudani 0:65f9be2858c9 296 {
tukudani 0:65f9be2858c9 297 uint8_t response[4];
tukudani 0:65f9be2858c9 298 int temp_scale;
tukudani 0:65f9be2858c9 299 //READ CURRENT ACC SCALE
tukudani 0:65f9be2858c9 300 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
tukudani 0:65f9be2858c9 301 set_acc_scale(BITS_FS_8G);
tukudani 0:65f9be2858c9 302 //ENABLE SELF TEST need modify
tukudani 0:65f9be2858c9 303 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
tukudani 0:65f9be2858c9 304
tukudani 0:65f9be2858c9 305 ReadRegs(MPUREG_SELF_TEST_X,response,4);
tukudani 0:65f9be2858c9 306 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
tukudani 0:65f9be2858c9 307 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
tukudani 0:65f9be2858c9 308 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
tukudani 0:65f9be2858c9 309
tukudani 0:65f9be2858c9 310 set_acc_scale(temp_scale);
tukudani 0:65f9be2858c9 311 }
tukudani 0:65f9be2858c9 312 uint8_t mpu9250_spi::AK8963_whoami()
tukudani 0:65f9be2858c9 313 {
tukudani 0:65f9be2858c9 314 uint8_t response;
tukudani 0:65f9be2858c9 315 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
tukudani 0:65f9be2858c9 316 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
tukudani 0:65f9be2858c9 317 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
tukudani 0:65f9be2858c9 318
tukudani 0:65f9be2858c9 319 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
tukudani 0:65f9be2858c9 320 wait(0.001);
tukudani 0:65f9be2858c9 321 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
tukudani 0:65f9be2858c9 322 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
tukudani 0:65f9be2858c9 323 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
tukudani 0:65f9be2858c9 324
tukudani 0:65f9be2858c9 325 return response;
tukudani 0:65f9be2858c9 326 }
tukudani 0:65f9be2858c9 327 void/*double**/ mpu9250_spi::AK8936_read_Orientation(/*double *getdata*/)
tukudani 0:65f9be2858c9 328 {
tukudani 0:65f9be2858c9 329 float angles[2];
tukudani 0:65f9be2858c9 330 float acc_angle[2];
tukudani 0:65f9be2858c9 331 float gyro_angle[2];
tukudani 0:65f9be2858c9 332 int i;
tukudani 0:65f9be2858c9 333
tukudani 0:65f9be2858c9 334 deg_time.stop();
tukudani 0:65f9be2858c9 335 read_all();
tukudani 0:65f9be2858c9 336 double kx = gyroscope_data[0];
tukudani 0:65f9be2858c9 337 double ky = gyroscope_data[1];
tukudani 0:65f9be2858c9 338 double kz = gyroscope_data[2];
tukudani 0:65f9be2858c9 339 double gx = accelerometer_data[0];
tukudani 0:65f9be2858c9 340 double gy = accelerometer_data[1];
tukudani 0:65f9be2858c9 341 double gz = accelerometer_data[2];
tukudani 0:65f9be2858c9 342 double mx = Magnetometer[0];//-centorx;
tukudani 0:65f9be2858c9 343 double my = Magnetometer[1];//-centory;
tukudani 0:65f9be2858c9 344 double mz = Magnetometer[2];//-centorz;
tukudani 0:65f9be2858c9 345 /*double g0x=0.01,g0y=0.001,g0z=0.99;
tukudani 0:65f9be2858c9 346 double Gproduct = gx*g0x+gy*g0y+gz*g0z;
tukudani 0:65f9be2858c9 347 double Glengh = sqrt(gx*gx+gy*gy+gz*gz);
tukudani 0:65f9be2858c9 348 double G0lengh =sqrt(g0x*g0x+g0y*g0y+g0z*g0z);
tukudani 0:65f9be2858c9 349 double nx = (gy * g0z - gz * g0y);
tukudani 0:65f9be2858c9 350 double ny = (gz * g0x - gx * g0z);
tukudani 0:65f9be2858c9 351 double nz = (gx * g0y - gy * g0x);
tukudani 0:65f9be2858c9 352 double GPlengh = sqrt(nx*nx+ny*ny+nz*nz);
tukudani 0:65f9be2858c9 353 nx/=GPlengh;
tukudani 0:65f9be2858c9 354 ny/=GPlengh;
tukudani 0:65f9be2858c9 355 nz/=GPlengh;
tukudani 0:65f9be2858c9 356 double accang = acos((Gproduct)/(fabs(Glengh)*fabs(G0lengh)));
tukudani 0:65f9be2858c9 357 //double hx=(nx*nx*(1-cos(accang))+cos(accang))*mx + (nx*ny*(1-cos(accang))+nz*sin(accang))*my + (nz*nx*(1-cos(accang))+ny*sin(accang))*mz;
tukudani 0:65f9be2858c9 358 //double hy=(nx*ny*(1-cos(accang))+nz*sin(accang))*mx + (ny*ny*(1-cos(accang))+cos(accang))*my + (ny*nz*(1-cos(accang))+nx*sin(accang))*mz;
tukudani 0:65f9be2858c9 359 double Mrot[3][3] = {
tukudani 0:65f9be2858c9 360 {nx*nx*(1-cos(accang))+cos(accang) , nx*ny*(1-cos(accang))+nz*sin(accang) ,nz*nx*(1-cos(accang))+ny*sin(accang)},
tukudani 0:65f9be2858c9 361 {nx*ny*(1-cos(accang))+nz*sin(accang) , ny*ny*(1-cos(accang))+cos(accang) ,ny*nz*(1-cos(accang))+nx*sin(accang)},
tukudani 0:65f9be2858c9 362 {nz*nx*(1-cos(accang))+ny*sin(accang) , ny*nz*(1-cos(accang))+nx*sin(accang) ,nz*nz*(1-cos(accang))+cos(accang)}
tukudani 0:65f9be2858c9 363 };
tukudani 0:65f9be2858c9 364 double MrotD[3][3];
tukudani 0:65f9be2858c9 365 double temp[3][3];
tukudani 0:65f9be2858c9 366 for(int i=0; i<3; i++)
tukudani 0:65f9be2858c9 367 for(int j=0; j<3; j++) {
tukudani 0:65f9be2858c9 368 temp[j][i]=Mrot[i][j];
tukudani 0:65f9be2858c9 369 MrotD[j][i]=temp[j][i];
tukudani 0:65f9be2858c9 370 }
tukudani 0:65f9be2858c9 371 double hx = MrotD[0][0]*mx+MrotD[0][1]*my+MrotD[0][2]*mz;
tukudani 0:65f9be2858c9 372 double hy = MrotD[1][0]*mx+MrotD[1][1]*my+MrotD[1][2]*mz;
tukudani 0:65f9be2858c9 373 double hz = MrotD[2][0]*mx+MrotD[2][1]*my+MrotD[2][2]*mz;
tukudani 0:65f9be2858c9 374 double heading = atan2(hy,hx)*180/3.14;*/
tukudani 0:65f9be2858c9 375
tukudani 0:65f9be2858c9 376 gyro_angle[0] = ky * deg_time.read() * (3.1415 / 180.0);
tukudani 0:65f9be2858c9 377 gyro_angle[1] = kx * deg_time.read() * (3.1415 / 180.0);
tukudani 0:65f9be2858c9 378 acc_angle[0] = atan2(-gx,gz);
tukudani 0:65f9be2858c9 379 acc_angle[1] = atan2(gy,-gx*sin(acc_angle[0])+gz*cos(acc_angle[0]));
tukudani 0:65f9be2858c9 380 for(i = 0; i < 2; i++){
tukudani 0:65f9be2858c9 381 angles[i] = 0.50 * (deg_before[i] + gyro_angle[i]) + 0.50 * acc_angle[i];
tukudani 0:65f9be2858c9 382 deg_before[i] = angles[i];
tukudani 0:65f9be2858c9 383 if(angles[i] > 3.1415){
tukudani 0:65f9be2858c9 384 angles[i] -= 6.2830;
tukudani 0:65f9be2858c9 385 }else if(angles[i] < -3.1415){
tukudani 0:65f9be2858c9 386 angles[i] += 6.2830;
tukudani 0:65f9be2858c9 387 }else;
tukudani 0:65f9be2858c9 388 }
tukudani 0:65f9be2858c9 389 double roll = angles[0];
tukudani 0:65f9be2858c9 390 double pitch = angles[1];
tukudani 0:65f9be2858c9 391 double hy = mz*sin(roll)-my*cos(roll);
tukudani 0:65f9be2858c9 392 double hx = mx*cos(pitch)+my*sin(pitch)*sin(roll)+mz*sin(pitch)*cos(roll);
tukudani 0:65f9be2858c9 393 double heading = -atan2(hy,hx)*(180/3.1415);
tukudani 0:65f9be2858c9 394
tukudani 0:65f9be2858c9 395 if (heading > 0)
tukudani 0:65f9be2858c9 396 heading = 360 - heading;
tukudani 0:65f9be2858c9 397 else
tukudani 0:65f9be2858c9 398 heading = -heading;
tukudani 0:65f9be2858c9 399 /*float pitch = atan(-hx/sqrt(hy*hy+hz*hz))*180/3.14; //invert gx because +pitch is up. range is +/-90 degrees
tukudani 0:65f9be2858c9 400 float roll = atan(hy/sqrt(hx*hx+hz*hz))*180/3.14; // right side down is +roll
tukudani 0:65f9be2858c9 401 if (gz > 0)
tukudani 0:65f9be2858c9 402 {
tukudani 0:65f9be2858c9 403 if ( roll > 0)
tukudani 0:65f9be2858c9 404 roll = 180 - roll;
tukudani 0:65f9be2858c9 405 else
tukudani 0:65f9be2858c9 406 roll = -180 - roll;
tukudani 0:65f9be2858c9 407 }*/
tukudani 0:65f9be2858c9 408
tukudani 0:65f9be2858c9 409 deg_time.reset();
tukudani 0:65f9be2858c9 410 deg_time.start();
tukudani 0:65f9be2858c9 411
tukudani 0:65f9be2858c9 412 getdata[0]=heading;
tukudani 0:65f9be2858c9 413 getdata[1]=pitch;
tukudani 0:65f9be2858c9 414 getdata[2]=roll;
tukudani 0:65f9be2858c9 415 //return getdata;
tukudani 0:65f9be2858c9 416 }
tukudani 0:65f9be2858c9 417 void mpu9250_spi::AK8963_setoffset(int x,double offset)
tukudani 0:65f9be2858c9 418 {
tukudani 0:65f9be2858c9 419 if((x<0)||(x>3))return;
tukudani 0:65f9be2858c9 420 Magnetometer_offset[x]+=offset;
tukudani 0:65f9be2858c9 421 }
tukudani 0:65f9be2858c9 422 void mpu9250_spi::AK8963_calib_Magnetometer()
tukudani 0:65f9be2858c9 423 {
tukudani 0:65f9be2858c9 424 uint8_t response[3];
tukudani 0:65f9be2858c9 425 float data;
tukudani 0:65f9be2858c9 426 int i;
tukudani 0:65f9be2858c9 427
tukudani 0:65f9be2858c9 428 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
tukudani 0:65f9be2858c9 429 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
tukudani 0:65f9be2858c9 430 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
tukudani 0:65f9be2858c9 431
tukudani 0:65f9be2858c9 432 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
tukudani 0:65f9be2858c9 433 wait(0.001);
tukudani 0:65f9be2858c9 434 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
tukudani 0:65f9be2858c9 435 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
tukudani 0:65f9be2858c9 436
tukudani 0:65f9be2858c9 437 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
tukudani 0:65f9be2858c9 438 for(i=0; i<3; i++) {
tukudani 0:65f9be2858c9 439 data=response[i];
tukudani 0:65f9be2858c9 440 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
tukudani 0:65f9be2858c9 441 }
tukudani 0:65f9be2858c9 442 }
tukudani 0:65f9be2858c9 443 void mpu9250_spi::AK8963_read_Magnetometer()
tukudani 0:65f9be2858c9 444 {
tukudani 0:65f9be2858c9 445 uint8_t response[7];
tukudani 0:65f9be2858c9 446 int16_t bit_data;
tukudani 0:65f9be2858c9 447 float data;
tukudani 0:65f9be2858c9 448 int i;
tukudani 0:65f9be2858c9 449
tukudani 0:65f9be2858c9 450 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
tukudani 0:65f9be2858c9 451 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
tukudani 0:65f9be2858c9 452 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
tukudani 0:65f9be2858c9 453
tukudani 0:65f9be2858c9 454 wait(0.001);
tukudani 0:65f9be2858c9 455 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
tukudani 0:65f9be2858c9 456 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
tukudani 0:65f9be2858c9 457 for(i=0; i<3; i++) {
tukudani 0:65f9be2858c9 458 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
tukudani 0:65f9be2858c9 459 data=(float)bit_data;
tukudani 0:65f9be2858c9 460 Magnetometer[i]=data*Magnetometer_ASA[i]-Magnetometer_offset[i];
tukudani 0:65f9be2858c9 461 }
tukudani 0:65f9be2858c9 462 }
tukudani 0:65f9be2858c9 463 void mpu9250_spi::read_all()
tukudani 0:65f9be2858c9 464 {
tukudani 0:65f9be2858c9 465 uint8_t response[21];
tukudani 0:65f9be2858c9 466 int16_t bit_data;
tukudani 0:65f9be2858c9 467 float data;
tukudani 0:65f9be2858c9 468 int i;
tukudani 0:65f9be2858c9 469
tukudani 0:65f9be2858c9 470 //Send I2C command at first
tukudani 0:65f9be2858c9 471 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
tukudani 0:65f9be2858c9 472 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
tukudani 0:65f9be2858c9 473 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
tukudani 0:65f9be2858c9 474 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
tukudani 0:65f9be2858c9 475
tukudani 0:65f9be2858c9 476 //wait(0.001);
tukudani 0:65f9be2858c9 477 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
tukudani 0:65f9be2858c9 478 //Get accelerometer value
tukudani 0:65f9be2858c9 479 for(i=0; i<3; i++) {
tukudani 0:65f9be2858c9 480 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 481 data=(float)bit_data;
tukudani 0:65f9be2858c9 482 accelerometer_data[i]=data/acc_divider;
tukudani 0:65f9be2858c9 483 //accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i];
tukudani 0:65f9be2858c9 484 //accelerometer_data_prev[i]=accelerometer_data[i];
tukudani 0:65f9be2858c9 485 }
tukudani 0:65f9be2858c9 486 //Get temperature
tukudani 0:65f9be2858c9 487 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 488 data=(float)bit_data;
tukudani 0:65f9be2858c9 489 Temperature=((data-21)/333.87)+21;//(data/340)+36.53;
tukudani 0:65f9be2858c9 490 //Get gyroscop value
tukudani 0:65f9be2858c9 491 for(i=3; i<7; i++) {
tukudani 0:65f9be2858c9 492 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 493 data=(float)bit_data;
tukudani 0:65f9be2858c9 494 gyroscope_data[i-4]=data/gyro_divider;
tukudani 0:65f9be2858c9 495 }
tukudani 0:65f9be2858c9 496 //Get Magnetometer value
tukudani 0:65f9be2858c9 497 for(i=7; i<10; i++) {
tukudani 0:65f9be2858c9 498 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
tukudani 0:65f9be2858c9 499 data=(float)bit_data;
tukudani 0:65f9be2858c9 500 Magnetometer[i-7]=data*Magnetometer_ASA[i-7]-Magnetometer_offset[i-7];
tukudani 0:65f9be2858c9 501 }
tukudani 0:65f9be2858c9 502 }
tukudani 0:65f9be2858c9 503
tukudani 0:65f9be2858c9 504
tukudani 0:65f9be2858c9 505
tukudani 0:65f9be2858c9 506 void mpu9250_spi::SpeedSet()
tukudani 0:65f9be2858c9 507 {
tukudani 0:65f9be2858c9 508 get_speedrate();
tukudani 0:65f9be2858c9 509 read_acc();
tukudani 0:65f9be2858c9 510 /*for(int i=0;i<3;i++)
tukudani 0:65f9be2858c9 511 ratespeed[i]=acc[i];*/
tukudani 0:65f9be2858c9 512 double S[3];
tukudani 0:65f9be2858c9 513 speedT.stop();
tukudani 0:65f9be2858c9 514 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 515
tukudani 0:65f9be2858c9 516 S[i] =((double)(ratespeed[i]+prev_ratespeed[i])*speedT.read()/2);
tukudani 0:65f9be2858c9 517 speed[i]+=(double)S[i];
tukudani 0:65f9be2858c9 518 speed[i]=floor(speed[i]*100.0)/100.0;
tukudani 0:65f9be2858c9 519 Synthesis_speed[i]+=(double)S[i];
tukudani 0:65f9be2858c9 520 Synthesis_speed[i]=0.9*(double)(Synthesis_speed[i]+(double)ratespeed[i]/100.5)+0.1*ratespeed[i];
tukudani 0:65f9be2858c9 521 kalman_speed[i]=kalmaspeed[i].getAngle((double)kalman_speed[i], (double)ratespeed[i], (double)angleT.read()*1000);
tukudani 0:65f9be2858c9 522 comp_speed[i]=kalman_speed[i]*0.01+Synthesis_speed[i]*0.01+comp_speed[i]*0.98;
tukudani 0:65f9be2858c9 523 prev_ratespeed[i]=ratespeed[i];
tukudani 0:65f9be2858c9 524 }
tukudani 0:65f9be2858c9 525 speedT.reset();
tukudani 0:65f9be2858c9 526 speedT.start();
tukudani 0:65f9be2858c9 527 }
tukudani 0:65f9be2858c9 528 void mpu9250_spi::MeterSet()
tukudani 0:65f9be2858c9 529 {
tukudani 0:65f9be2858c9 530 double S[3];
tukudani 0:65f9be2858c9 531 for(int i = 0 ; i<3 ; i++)
tukudani 0:65f9be2858c9 532 ratemeter[i]=comp_speed[i]*1000;
tukudani 0:65f9be2858c9 533 meterT.stop();
tukudani 0:65f9be2858c9 534 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 535
tukudani 0:65f9be2858c9 536 S[i] =((double)(ratemeter[i]+prev_ratemeter[i])*meterT.read()/2);
tukudani 0:65f9be2858c9 537 meter[i]+=(double)S[i];
tukudani 0:65f9be2858c9 538 Synthesis_meter[i]+=(double)S[i];
tukudani 0:65f9be2858c9 539 Synthesis_meter[i]=0.9*(double)(Synthesis_meter[i]+(double)ratemeter[i]/100.5)+0.1*meter[i];
tukudani 0:65f9be2858c9 540 kalman_meter[i]=kalmameter[i].getAngle((double)kalman_meter[i], (double)ratemeter[i], (double)angleT.read()*1000);
tukudani 0:65f9be2858c9 541 comp_meter[i]=kalman_meter[i]*0.01+Synthesis_meter[i]*0.01+comp_meter[i]*0.98;
tukudani 0:65f9be2858c9 542 prev_ratemeter[i]=ratemeter[i];
tukudani 0:65f9be2858c9 543 }
tukudani 0:65f9be2858c9 544 meterT.reset();
tukudani 0:65f9be2858c9 545 meterT.start();
tukudani 0:65f9be2858c9 546 }
tukudani 0:65f9be2858c9 547
tukudani 0:65f9be2858c9 548
tukudani 0:65f9be2858c9 549
tukudani 0:65f9be2858c9 550 void mpu9250_spi::MPU_setup()
tukudani 0:65f9be2858c9 551 {
tukudani 0:65f9be2858c9 552 z_offset=2;
tukudani 0:65f9be2858c9 553 x_offset=0;
tukudani 0:65f9be2858c9 554 y_offset=0;
tukudani 0:65f9be2858c9 555 uint8_t buffer[6];
tukudani 0:65f9be2858c9 556
tukudani 0:65f9be2858c9 557 for(int i=0; i<sampleNum; i++) {
tukudani 0:65f9be2858c9 558 ReadRegs(MPUREG_ACCEL_XOUT_H,buffer,6);
tukudani 0:65f9be2858c9 559 int Xacc = (int)buffer[1] << 8 | (int)buffer[0];
tukudani 0:65f9be2858c9 560 int Yacc = (int)buffer[3] << 8 | (int)buffer[2];
tukudani 0:65f9be2858c9 561 int Zacc = (int)buffer[5] << 8 | (int)buffer[4]-255;
tukudani 0:65f9be2858c9 562 if((int)Xacc-x_offset>xnoise)
tukudani 0:65f9be2858c9 563 xnoise=(int)Xacc-x_offset;
tukudani 0:65f9be2858c9 564 else if((int)Xacc-x_offset<-xnoise)
tukudani 0:65f9be2858c9 565 xnoise=-(int)Xacc-x_offset;
tukudani 0:65f9be2858c9 566
tukudani 0:65f9be2858c9 567 if((int)Yacc-y_offset>ynoise)
tukudani 0:65f9be2858c9 568 ynoise=(int)Yacc-y_offset;
tukudani 0:65f9be2858c9 569 else if((int)Yacc-y_offset<-ynoise)
tukudani 0:65f9be2858c9 570 ynoise=-(int)Yacc-y_offset;
tukudani 0:65f9be2858c9 571
tukudani 0:65f9be2858c9 572 if((int)Zacc-z_offset>znoise)
tukudani 0:65f9be2858c9 573 znoise=(int)Zacc-z_offset;
tukudani 0:65f9be2858c9 574 else if((int)Zacc-z_offset<-znoise)
tukudani 0:65f9be2858c9 575 znoise=-(int)Zacc-z_offset;
tukudani 0:65f9be2858c9 576 }
tukudani 0:65f9be2858c9 577
tukudani 0:65f9be2858c9 578
tukudani 0:65f9be2858c9 579 }
tukudani 0:65f9be2858c9 580 void mpu9250_spi::MPU_setnum(int Num,float time,double rate)
tukudani 0:65f9be2858c9 581 {
tukudani 0:65f9be2858c9 582 sampleNum=Num;
tukudani 0:65f9be2858c9 583 sampleTime=time;
tukudani 0:65f9be2858c9 584 Rate=rate;
tukudani 0:65f9be2858c9 585 }
tukudani 0:65f9be2858c9 586
tukudani 0:65f9be2858c9 587
tukudani 0:65f9be2858c9 588 void mpu9250_spi::get_angle_acc()
tukudani 0:65f9be2858c9 589 {
tukudani 0:65f9be2858c9 590 //short data[3];
tukudani 0:65f9be2858c9 591 //get_axis_acc(data);
tukudani 0:65f9be2858c9 592 float R = sqrt(pow((float)accelerometer_data[0],2) + pow((float)accelerometer_data[1],2) + pow((float)accelerometer_data[2],2));
tukudani 0:65f9be2858c9 593 angle_acc[0]=-atan2(accelerometer_data[0],accelerometer_data[2])*(180 / 3.1415);
tukudani 0:65f9be2858c9 594 angle_acc[1]=atan2(accelerometer_data[1],accelerometer_data[2])*(180 / 3.1415);
tukudani 0:65f9be2858c9 595 angle_acc[2]=atan2(accelerometer_data[2],accelerometer_data[0])*(180 / 3.1415);
tukudani 0:65f9be2858c9 596 /*angle_acc[1] = -((180 / 3.1415) * acos((float)accelerometer_data[1] / R)-90); // roll - angle of magnetic field vector in x direction
tukudani 0:65f9be2858c9 597 angle_acc[0] = (180 / 3.1415) * acos((float)accelerometer_data[0] / R)-90; // pitch - angle of magnetic field vector in y direction
tukudani 0:65f9be2858c9 598 angle_acc[2] = (180 / 3.1415) * acos((float)accelerometer_data[2] / R); */
tukudani 0:65f9be2858c9 599 /*
tukudani 0:65f9be2858c9 600 angle_acc[0] = atan2(accelerometer_data[0],sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
tukudani 0:65f9be2858c9 601 angle_acc[1] = atan2(accelerometer_data[1],sqrt(pow((float)accelerometer_data[0],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
tukudani 0:65f9be2858c9 602 angle_acc[2] = atan2(sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)),accelerometer_data[2])*180/3.1415;
tukudani 0:65f9be2858c9 603 */
tukudani 0:65f9be2858c9 604
tukudani 0:65f9be2858c9 605 }
tukudani 0:65f9be2858c9 606 void mpu9250_spi::get_rate()
tukudani 0:65f9be2858c9 607 {
tukudani 0:65f9be2858c9 608 uint8_t response[6];
tukudani 0:65f9be2858c9 609 short offset_t[3]= {0,0,0};
tukudani 0:65f9be2858c9 610 ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
tukudani 0:65f9be2858c9 611 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 612 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 613 rate[i]=((double)(bit_data*0.01)-offset_t[i])/gyro_divider;;
tukudani 0:65f9be2858c9 614 //printf("%d",rate[i]);
tukudani 0:65f9be2858c9 615 }
tukudani 0:65f9be2858c9 616 }
tukudani 0:65f9be2858c9 617
tukudani 0:65f9be2858c9 618 void mpu9250_spi::get_speedrate()
tukudani 0:65f9be2858c9 619 {
tukudani 0:65f9be2858c9 620 uint8_t response[6];
tukudani 0:65f9be2858c9 621 short offset_t[3]= {0,0,0};
tukudani 0:65f9be2858c9 622 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
tukudani 0:65f9be2858c9 623 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 624 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 625 ratespeed[i]=(short)(bit_data*0.01)-offset_t[i];
tukudani 0:65f9be2858c9 626 }
tukudani 0:65f9be2858c9 627 }
tukudani 0:65f9be2858c9 628 void mpu9250_spi::get_angle(double *x,double *y,double *z)
tukudani 0:65f9be2858c9 629 {
tukudani 0:65f9be2858c9 630 *x=(floor(angle[0]*100.0)/100.0);
tukudani 0:65f9be2858c9 631 *y=(floor(angle[1]*100.0)/100.0);
tukudani 0:65f9be2858c9 632 *z=(floor(angle[2]*100.0)/100.0);
tukudani 0:65f9be2858c9 633 }
tukudani 0:65f9be2858c9 634 void mpu9250_spi::set_angle()
tukudani 0:65f9be2858c9 635 {
tukudani 0:65f9be2858c9 636 get_rate();
tukudani 0:65f9be2858c9 637 read_acc();
tukudani 0:65f9be2858c9 638 get_angle_acc();
tukudani 0:65f9be2858c9 639 double S[3];
tukudani 0:65f9be2858c9 640 angleT.stop();
tukudani 0:65f9be2858c9 641 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 642 //rate[i]=gyroscope_data[i]*0.01;
tukudani 0:65f9be2858c9 643 double angA=angle_acc[i];
tukudani 0:65f9be2858c9 644 S[i] =((double)(rate[i]+prev_rate[i])*angleT.read()/2);
tukudani 0:65f9be2858c9 645 S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
tukudani 0:65f9be2858c9 646 if(i == 2) angle[i] += S[1]*gyro_divider;
tukudani 0:65f9be2858c9 647 else angle[i] += S[i]*gyro_divider;;//(floor(S[i]*10000.0)/10000.0);//-offset_angle[i];
tukudani 0:65f9be2858c9 648 //angle[i]+=(double)S[i];
tukudani 0:65f9be2858c9 649 Synthesis_angle[i]+=(double)angle[i];
tukudani 0:65f9be2858c9 650 Synthesis_angle[i]=0.92*(double)(Synthesis_angle[i]+(double)rate[i])+0.08*angA;
tukudani 0:65f9be2858c9 651 kalman_angle[i]=kalma[i].getAngle((double)angA, (double)rate[i], (double)angleT.read_ms());
tukudani 0:65f9be2858c9 652 comp_angle[i]=kalman_angle[i];//Synthesis_angle[i]*0.5+kalman_angle[i]*0+angle[i]*0.3+angle_acc[i]*0.2;
tukudani 0:65f9be2858c9 653 comp_angle[i]=kalman_angle[i]*0.4+Synthesis_angle[i]*0.4+comp_angle[i]*0.2;
tukudani 0:65f9be2858c9 654 prev_rate[i]=rate[i];
tukudani 0:65f9be2858c9 655 }
tukudani 0:65f9be2858c9 656 angleT.reset();
tukudani 0:65f9be2858c9 657 angleT.start();
tukudani 0:65f9be2858c9 658 }
tukudani 0:65f9be2858c9 659 void mpu9250_spi::set_angleoffset()
tukudani 0:65f9be2858c9 660 {
tukudani 0:65f9be2858c9 661 double axis[3],offseta[3];
tukudani 0:65f9be2858c9 662 offseta[0]=offseta[1]=offseta[2]=0;
tukudani 0:65f9be2858c9 663 offset_angle[0]=0;
tukudani 0:65f9be2858c9 664 offset_angle[1]=0;
tukudani 0:65f9be2858c9 665 offset_angle[2]=0;
tukudani 0:65f9be2858c9 666 for(int i=0; i<sampleNum; i++) {
tukudani 0:65f9be2858c9 667 offseta[0]+=rate[0];
tukudani 0:65f9be2858c9 668 offseta[1]+=rate[1];
tukudani 0:65f9be2858c9 669 offseta[2]+=rate[2];
tukudani 0:65f9be2858c9 670 }
tukudani 0:65f9be2858c9 671 offset_angle[0]=offseta[0]/sampleNum;
tukudani 0:65f9be2858c9 672 offset_angle[1]=offseta[1]/sampleNum;
tukudani 0:65f9be2858c9 673 offset_angle[2]=offseta[2]/sampleNum;
tukudani 0:65f9be2858c9 674 offset_angle[0]=(floor(offset_angle[0]*100.0)/100.0);
tukudani 0:65f9be2858c9 675 offset_angle[1]=(floor(offset_angle[1]*100.0)/100.0);
tukudani 0:65f9be2858c9 676 offset_angle[2]=(floor(offset_angle[2]*100.0)/100.0);
tukudani 0:65f9be2858c9 677 angle[0]=0;
tukudani 0:65f9be2858c9 678 angle[1]=0;
tukudani 0:65f9be2858c9 679 angle[2]=0;
tukudani 0:65f9be2858c9 680 }
tukudani 0:65f9be2858c9 681 void mpu9250_spi::set_noise()
tukudani 0:65f9be2858c9 682 {
tukudani 0:65f9be2858c9 683 short gyro[3];
tukudani 0:65f9be2858c9 684 noise[0]=noise[1]=noise[2]=0;
tukudani 0:65f9be2858c9 685 uint8_t response[6];
tukudani 0:65f9be2858c9 686 for(int i=0; i<sampleNum; i++) {
tukudani 0:65f9be2858c9 687
tukudani 0:65f9be2858c9 688 for(int t=0; t<3; t++) {
tukudani 0:65f9be2858c9 689 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 690 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 691 gyro[i]=(short)bit_data;
tukudani 0:65f9be2858c9 692 }
tukudani 0:65f9be2858c9 693 if((int)gyro[t]>noise[t])
tukudani 0:65f9be2858c9 694 noise[t]=(int)gyro[t];
tukudani 0:65f9be2858c9 695 else if((int)gyro[t]<-noise[t])
tukudani 0:65f9be2858c9 696 noise[t]=-(int)gyro[t];
tukudani 0:65f9be2858c9 697 }
tukudani 0:65f9be2858c9 698 }
tukudani 0:65f9be2858c9 699 noise[0]*=sampleTime;
tukudani 0:65f9be2858c9 700 noise[1]*=sampleTime;
tukudani 0:65f9be2858c9 701 noise[2]*=sampleTime;
tukudani 0:65f9be2858c9 702 }
tukudani 0:65f9be2858c9 703 void mpu9250_spi::set_offset()
tukudani 0:65f9be2858c9 704 {
tukudani 0:65f9be2858c9 705 short axis[3],offseta[3];
tukudani 0:65f9be2858c9 706 uint8_t response[6];
tukudani 0:65f9be2858c9 707 offseta[0]=0;
tukudani 0:65f9be2858c9 708 offseta[1]=0;
tukudani 0:65f9be2858c9 709 offseta[2]=0;
tukudani 0:65f9be2858c9 710 for(int i=0; i<sampleNum; i++) {
tukudani 0:65f9be2858c9 711 for(int i=0; i<3; i++) {
tukudani 0:65f9be2858c9 712 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
tukudani 0:65f9be2858c9 713 axis[i]=(short)bit_data;
tukudani 0:65f9be2858c9 714 }
tukudani 0:65f9be2858c9 715 offseta[0]+=axis[0];
tukudani 0:65f9be2858c9 716 offseta[1]+=axis[1];
tukudani 0:65f9be2858c9 717 offseta[2]+=axis[2];
tukudani 0:65f9be2858c9 718 }
tukudani 0:65f9be2858c9 719 offset[0]=offseta[0]/sampleNum;
tukudani 0:65f9be2858c9 720 offset[1]=offseta[1]/sampleNum;
tukudani 0:65f9be2858c9 721 offset[2]=offseta[2]/sampleNum;
tukudani 0:65f9be2858c9 722 }
tukudani 0:65f9be2858c9 723 mpu9250_spi::kalman::kalman(void)
tukudani 0:65f9be2858c9 724 {
tukudani 0:65f9be2858c9 725 P[0][0] = 0;
tukudani 0:65f9be2858c9 726 P[0][1] = 0;
tukudani 0:65f9be2858c9 727 P[1][0] = 0;
tukudani 0:65f9be2858c9 728 P[1][1] = 0;
tukudani 0:65f9be2858c9 729
tukudani 0:65f9be2858c9 730 angle = 0;
tukudani 0:65f9be2858c9 731 bias = 0;
tukudani 0:65f9be2858c9 732
tukudani 0:65f9be2858c9 733 Q_angle = 0.001;
tukudani 0:65f9be2858c9 734 Q_gyroBias = 0.003;
tukudani 0:65f9be2858c9 735 R_angle = 0.03;
tukudani 0:65f9be2858c9 736 }
tukudani 0:65f9be2858c9 737
tukudani 0:65f9be2858c9 738 double mpu9250_spi::kalman::getAngle(double newAngle, double newRate, double dt)
tukudani 0:65f9be2858c9 739 {
tukudani 0:65f9be2858c9 740 //predict the angle according to the gyroscope
tukudani 0:65f9be2858c9 741 rate = newRate - bias;
tukudani 0:65f9be2858c9 742 angle = dt * rate;
tukudani 0:65f9be2858c9 743 //update the error covariance
tukudani 0:65f9be2858c9 744 P[0][0] += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
tukudani 0:65f9be2858c9 745 P[0][1] -= dt * P[1][1];
tukudani 0:65f9be2858c9 746 P[1][0] -= dt * P[1][1];
tukudani 0:65f9be2858c9 747 P[1][1] += dt * Q_gyroBias;
tukudani 0:65f9be2858c9 748 //difference between the predicted angle and the accelerometer angle
tukudani 0:65f9be2858c9 749 y = newAngle - angle;
tukudani 0:65f9be2858c9 750 //estimation error
tukudani 0:65f9be2858c9 751 S = P[0][0] + R_angle;
tukudani 0:65f9be2858c9 752 //determine the kalman gain according to the error covariance and the distrust
tukudani 0:65f9be2858c9 753 K[0] = P[0][0]/S;
tukudani 0:65f9be2858c9 754 K[1] = P[1][0]/S;
tukudani 0:65f9be2858c9 755 //adjust the angle according to the kalman gain and the difference with the measurement
tukudani 0:65f9be2858c9 756 angle += K[0] * y;
tukudani 0:65f9be2858c9 757 bias += K[1] * y;
tukudani 0:65f9be2858c9 758 //update the error covariance
tukudani 0:65f9be2858c9 759 P[0][0] -= K[0] * P[0][0];
tukudani 0:65f9be2858c9 760 P[0][1] -= K[0] * P[0][1];
tukudani 0:65f9be2858c9 761 P[1][0] -= K[1] * P[0][0];
tukudani 0:65f9be2858c9 762 P[1][1] -= K[1] * P[0][1];
tukudani 0:65f9be2858c9 763
tukudani 0:65f9be2858c9 764 return angle;
tukudani 0:65f9be2858c9 765 }
tukudani 0:65f9be2858c9 766 void mpu9250_spi::kalman::setAngle(double newAngle)
tukudani 0:65f9be2858c9 767 {
tukudani 0:65f9be2858c9 768 angle = newAngle;
tukudani 0:65f9be2858c9 769 };
tukudani 0:65f9be2858c9 770 void mpu9250_spi::kalman::setQangle(double newQ_angle)
tukudani 0:65f9be2858c9 771 {
tukudani 0:65f9be2858c9 772 Q_angle = newQ_angle;
tukudani 0:65f9be2858c9 773 };
tukudani 0:65f9be2858c9 774 void mpu9250_spi::kalman::setQgyroBias(double newQ_gyroBias)
tukudani 0:65f9be2858c9 775 {
tukudani 0:65f9be2858c9 776 Q_gyroBias = newQ_gyroBias;
tukudani 0:65f9be2858c9 777 };
tukudani 0:65f9be2858c9 778 void mpu9250_spi::kalman::setRangle(double newR_angle)
tukudani 0:65f9be2858c9 779 {
tukudani 0:65f9be2858c9 780 R_angle = newR_angle;
tukudani 0:65f9be2858c9 781 };
tukudani 0:65f9be2858c9 782
tukudani 0:65f9be2858c9 783 double mpu9250_spi::kalman::getRate(void)
tukudani 0:65f9be2858c9 784 {
tukudani 0:65f9be2858c9 785 return rate;
tukudani 0:65f9be2858c9 786 };
tukudani 0:65f9be2858c9 787 double mpu9250_spi::kalman::getQangle(void)
tukudani 0:65f9be2858c9 788 {
tukudani 0:65f9be2858c9 789 return Q_angle;
tukudani 0:65f9be2858c9 790 };
tukudani 0:65f9be2858c9 791 double mpu9250_spi::kalman::getQbias(void)
tukudani 0:65f9be2858c9 792 {
tukudani 0:65f9be2858c9 793 return Q_gyroBias;
tukudani 0:65f9be2858c9 794 };
tukudani 0:65f9be2858c9 795 double mpu9250_spi::kalman::getRangle(void)
tukudani 0:65f9be2858c9 796 {
tukudani 0:65f9be2858c9 797 return R_angle;
tukudani 0:65f9be2858c9 798 };
tukudani 0:65f9be2858c9 799
tukudani 0:65f9be2858c9 800 /*-----------------------------------------------------------------------------------------------
tukudani 0:65f9be2858c9 801 SPI SELECT AND DESELECT
tukudani 0:65f9be2858c9 802 usage: enable and disable mpu9250 communication bus
tukudani 0:65f9be2858c9 803 -----------------------------------------------------------------------------------------------*/
tukudani 0:65f9be2858c9 804 void mpu9250_spi::select()
tukudani 0:65f9be2858c9 805 {
tukudani 0:65f9be2858c9 806 //Set CS low to start transmission (interrupts conversion)
tukudani 0:65f9be2858c9 807 cs = 0;
tukudani 0:65f9be2858c9 808 }
tukudani 0:65f9be2858c9 809 void mpu9250_spi::deselect()
tukudani 0:65f9be2858c9 810 {
tukudani 0:65f9be2858c9 811 //Set CS high to stop transmission (restarts conversion)
tukudani 0:65f9be2858c9 812 cs = 1;
tukudani 0:65f9be2858c9 813 }