Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.

MPU9250_SPI/MPU9250_SPI.cpp@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
Parent:
0:0929d3d566cf 
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:0929d3d566cf 1 /*CODED by Qiyong Mu on 21/06/2014
Anaesthetix 0:0929d3d566cf 2 kylongmu@msn.com
Anaesthetix 0:0929d3d566cf 3 */
Anaesthetix 0:0929d3d566cf 4
Anaesthetix 0:0929d3d566cf 5 #include <mbed.h>
Anaesthetix 0:0929d3d566cf 6 #include "MPU9250_SPI.h"
Anaesthetix 0:0929d3d566cf 7
Anaesthetix 0:0929d3d566cf 8 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
Anaesthetix 0:0929d3d566cf 9
Anaesthetix 0:0929d3d566cf 10 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
Anaesthetix 0:0929d3d566cf 11 {
Anaesthetix 0:0929d3d566cf 12 unsigned int temp_val;
Anaesthetix 0:0929d3d566cf 13 select();
Anaesthetix 0:0929d3d566cf 14 spi.write(WriteAddr);
Anaesthetix 0:0929d3d566cf 15 temp_val=spi.write(WriteData);
Anaesthetix 0:0929d3d566cf 16 deselect();
Anaesthetix 0:0929d3d566cf 17 wait_us(5);
Anaesthetix 0:0929d3d566cf 18 return temp_val;
Anaesthetix 0:0929d3d566cf 19 }
Anaesthetix 0:0929d3d566cf 20 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
Anaesthetix 0:0929d3d566cf 21 {
Anaesthetix 0:0929d3d566cf 22 return WriteReg(WriteAddr | READ_FLAG,WriteData);
Anaesthetix 0:0929d3d566cf 23 }
Anaesthetix 0:0929d3d566cf 24 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
Anaesthetix 0:0929d3d566cf 25 {
Anaesthetix 0:0929d3d566cf 26 unsigned int i = 0;
Anaesthetix 0:0929d3d566cf 27
Anaesthetix 0:0929d3d566cf 28 select();
Anaesthetix 0:0929d3d566cf 29 spi.write(ReadAddr | READ_FLAG);
Anaesthetix 0:0929d3d566cf 30 for(i=0; i<Bytes; i++)
Anaesthetix 0:0929d3d566cf 31 ReadBuf[i] = spi.write(0x00);
Anaesthetix 0:0929d3d566cf 32 deselect();
Anaesthetix 0:0929d3d566cf 33 //wait_us(50);
Anaesthetix 0:0929d3d566cf 34 }
Anaesthetix 0:0929d3d566cf 35
Anaesthetix 0:0929d3d566cf 36 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 37 INITIALIZATION
Anaesthetix 0:0929d3d566cf 38 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
Anaesthetix 0:0929d3d566cf 39 low pass filter value; suitable values are:
Anaesthetix 0:0929d3d566cf 40 BITS_DLPF_CFG_256HZ_NOLPF2
Anaesthetix 0:0929d3d566cf 41 BITS_DLPF_CFG_188HZ
Anaesthetix 0:0929d3d566cf 42 BITS_DLPF_CFG_98HZ
Anaesthetix 0:0929d3d566cf 43 BITS_DLPF_CFG_42HZ
Anaesthetix 0:0929d3d566cf 44 BITS_DLPF_CFG_20HZ
Anaesthetix 0:0929d3d566cf 45 BITS_DLPF_CFG_10HZ
Anaesthetix 0:0929d3d566cf 46 BITS_DLPF_CFG_5HZ
Anaesthetix 0:0929d3d566cf 47 BITS_DLPF_CFG_2100HZ_NOLPF
Anaesthetix 0:0929d3d566cf 48 returns 1 if an error occurred
Anaesthetix 0:0929d3d566cf 49 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 50 #define MPU_InitRegNum 17
Anaesthetix 0:0929d3d566cf 51
Anaesthetix 0:0929d3d566cf 52 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
Anaesthetix 0:0929d3d566cf 53 uint8_t i = 0;
Anaesthetix 0:0929d3d566cf 54 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
Anaesthetix 0:0929d3d566cf 55 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
Anaesthetix 0:0929d3d566cf 56 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
Anaesthetix 0:0929d3d566cf 57 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
Anaesthetix 0:0929d3d566cf 58 {0x01, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz //3
Anaesthetix 0:0929d3d566cf 59 {0x08, MPUREG_GYRO_CONFIG}, // +-1000dps = 10 / 500
Anaesthetix 0:0929d3d566cf 60 {0x00, MPUREG_ACCEL_CONFIG}, // +-2G
Anaesthetix 0:0929d3d566cf 61 {0x03, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 41Hz
Anaesthetix 0:0929d3d566cf 62 {0x30, MPUREG_INT_PIN_CFG}, //
Anaesthetix 0:0929d3d566cf 63 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
Anaesthetix 0:0929d3d566cf 64 //{0x20, MPUREG_USER_CTRL}, // Enable AUX
Anaesthetix 0:0929d3d566cf 65 {0x20, MPUREG_USER_CTRL}, // I2C Master mode
Anaesthetix 0:0929d3d566cf 66 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
Anaesthetix 0:0929d3d566cf 67
Anaesthetix 0:0929d3d566cf 68 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
Anaesthetix 0:0929d3d566cf 69 //{0x09, MPUREG_I2C_SLV4_CTRL},
Anaesthetix 0:0929d3d566cf 70 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
Anaesthetix 0:0929d3d566cf 71
Anaesthetix 0:0929d3d566cf 72 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 73 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
Anaesthetix 0:0929d3d566cf 74 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
Anaesthetix 0:0929d3d566cf 75
Anaesthetix 0:0929d3d566cf 76 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 77 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
Anaesthetix 0:0929d3d566cf 78 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
Anaesthetix 0:0929d3d566cf 79
Anaesthetix 0:0929d3d566cf 80 };
Anaesthetix 0:0929d3d566cf 81 spi.format(8,0);
Anaesthetix 0:0929d3d566cf 82 spi.frequency(4000000);
Anaesthetix 0:0929d3d566cf 83
Anaesthetix 0:0929d3d566cf 84 for(i=0; i<MPU_InitRegNum; i++) {
Anaesthetix 0:0929d3d566cf 85 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
Anaesthetix 0:0929d3d566cf 86 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
Anaesthetix 0:0929d3d566cf 87 }
Anaesthetix 0:0929d3d566cf 88
Anaesthetix 0:0929d3d566cf 89 set_acc_scale(BITS_FS_2G);
Anaesthetix 0:0929d3d566cf 90 set_gyro_scale(BITS_FS_1000DPS);
Anaesthetix 0:0929d3d566cf 91
Anaesthetix 0:0929d3d566cf 92 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
Anaesthetix 0:0929d3d566cf 93 return 0;
Anaesthetix 0:0929d3d566cf 94 }
Anaesthetix 0:0929d3d566cf 95
Anaesthetix 0:0929d3d566cf 96 bool mpu9250_spi::init2(int sample_rate_div,int low_pass_filter){
Anaesthetix 0:0929d3d566cf 97 uint8_t i = 0;
Anaesthetix 0:0929d3d566cf 98 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
Anaesthetix 0:0929d3d566cf 99 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
Anaesthetix 0:0929d3d566cf 100 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
Anaesthetix 0:0929d3d566cf 101 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
Anaesthetix 0:0929d3d566cf 102 {0x01, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz //3
Anaesthetix 0:0929d3d566cf 103 {0x08, MPUREG_GYRO_CONFIG}, // +-1000dps = 10 / 500
Anaesthetix 0:0929d3d566cf 104 {0x00, MPUREG_ACCEL_CONFIG}, // +-2G
Anaesthetix 0:0929d3d566cf 105 {0x08, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 41Hz
Anaesthetix 0:0929d3d566cf 106 {0x30, MPUREG_INT_PIN_CFG}, //
Anaesthetix 0:0929d3d566cf 107 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
Anaesthetix 0:0929d3d566cf 108 //{0x20, MPUREG_USER_CTRL}, // Enable AUX
Anaesthetix 0:0929d3d566cf 109 {0x20, MPUREG_USER_CTRL}, // I2C Master mode
Anaesthetix 0:0929d3d566cf 110 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
Anaesthetix 0:0929d3d566cf 111
Anaesthetix 0:0929d3d566cf 112 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
Anaesthetix 0:0929d3d566cf 113 //{0x09, MPUREG_I2C_SLV4_CTRL},
Anaesthetix 0:0929d3d566cf 114 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
Anaesthetix 0:0929d3d566cf 115
Anaesthetix 0:0929d3d566cf 116 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 117 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
Anaesthetix 0:0929d3d566cf 118 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
Anaesthetix 0:0929d3d566cf 119
Anaesthetix 0:0929d3d566cf 120 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 121 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
Anaesthetix 0:0929d3d566cf 122 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
Anaesthetix 0:0929d3d566cf 123
Anaesthetix 0:0929d3d566cf 124 };
Anaesthetix 0:0929d3d566cf 125 spi.format(8,0);
Anaesthetix 0:0929d3d566cf 126 spi.frequency(4000000);
Anaesthetix 0:0929d3d566cf 127
Anaesthetix 0:0929d3d566cf 128 for(i=0; i<MPU_InitRegNum; i++) {
Anaesthetix 0:0929d3d566cf 129 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
Anaesthetix 0:0929d3d566cf 130 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
Anaesthetix 0:0929d3d566cf 131 }
Anaesthetix 0:0929d3d566cf 132
Anaesthetix 0:0929d3d566cf 133 set_acc_scale(BITS_FS_2G);
Anaesthetix 0:0929d3d566cf 134 set_gyro_scale(BITS_FS_1000DPS);
Anaesthetix 0:0929d3d566cf 135
Anaesthetix 0:0929d3d566cf 136 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
Anaesthetix 0:0929d3d566cf 137 return 0;
Anaesthetix 0:0929d3d566cf 138 }
Anaesthetix 0:0929d3d566cf 139 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 140 ACCELEROMETER SCALE
Anaesthetix 0:0929d3d566cf 141 usage: call this function at startup, after initialization, to set the right range for the
Anaesthetix 0:0929d3d566cf 142 accelerometers. Suitable ranges are:
Anaesthetix 0:0929d3d566cf 143 BITS_FS_2G
Anaesthetix 0:0929d3d566cf 144 BITS_FS_4G
Anaesthetix 0:0929d3d566cf 145 BITS_FS_8G
Anaesthetix 0:0929d3d566cf 146 BITS_FS_16G
Anaesthetix 0:0929d3d566cf 147 returns the range set (2,4,8 or 16)
Anaesthetix 0:0929d3d566cf 148 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 149 unsigned int mpu9250_spi::set_acc_scale(int scale){
Anaesthetix 0:0929d3d566cf 150 unsigned int temp_scale;
Anaesthetix 0:0929d3d566cf 151 //WriteReg(MPUREG_ACCEL_CONFIG, scale);
Anaesthetix 0:0929d3d566cf 152
Anaesthetix 0:0929d3d566cf 153 switch (scale){
Anaesthetix 0:0929d3d566cf 154 case BITS_FS_2G:
Anaesthetix 0:0929d3d566cf 155 acc_divider=16384;
Anaesthetix 0:0929d3d566cf 156 break;
Anaesthetix 0:0929d3d566cf 157 case BITS_FS_4G:
Anaesthetix 0:0929d3d566cf 158 acc_divider=8192;
Anaesthetix 0:0929d3d566cf 159 break;
Anaesthetix 0:0929d3d566cf 160 case BITS_FS_8G:
Anaesthetix 0:0929d3d566cf 161 acc_divider=4096;
Anaesthetix 0:0929d3d566cf 162 break;
Anaesthetix 0:0929d3d566cf 163 case BITS_FS_16G:
Anaesthetix 0:0929d3d566cf 164 acc_divider=2048;
Anaesthetix 0:0929d3d566cf 165 break;
Anaesthetix 0:0929d3d566cf 166 }
Anaesthetix 0:0929d3d566cf 167 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
Anaesthetix 0:0929d3d566cf 168
Anaesthetix 0:0929d3d566cf 169 switch (temp_scale){
Anaesthetix 0:0929d3d566cf 170 case BITS_FS_2G:
Anaesthetix 0:0929d3d566cf 171 temp_scale=2;
Anaesthetix 0:0929d3d566cf 172 break;
Anaesthetix 0:0929d3d566cf 173 case BITS_FS_4G:
Anaesthetix 0:0929d3d566cf 174 temp_scale=4;
Anaesthetix 0:0929d3d566cf 175 break;
Anaesthetix 0:0929d3d566cf 176 case BITS_FS_8G:
Anaesthetix 0:0929d3d566cf 177 temp_scale=8;
Anaesthetix 0:0929d3d566cf 178 break;
Anaesthetix 0:0929d3d566cf 179 case BITS_FS_16G:
Anaesthetix 0:0929d3d566cf 180 temp_scale=16;
Anaesthetix 0:0929d3d566cf 181 break;
Anaesthetix 0:0929d3d566cf 182 }
Anaesthetix 0:0929d3d566cf 183 return temp_scale;
Anaesthetix 0:0929d3d566cf 184 }
Anaesthetix 0:0929d3d566cf 185
Anaesthetix 0:0929d3d566cf 186
Anaesthetix 0:0929d3d566cf 187 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 188 GYROSCOPE SCALE
Anaesthetix 0:0929d3d566cf 189 usage: call this function at startup, after initialization, to set the right range for the
Anaesthetix 0:0929d3d566cf 190 gyroscopes. Suitable ranges are:
Anaesthetix 0:0929d3d566cf 191 BITS_FS_250DPS
Anaesthetix 0:0929d3d566cf 192 BITS_FS_500DPS
Anaesthetix 0:0929d3d566cf 193 BITS_FS_1000DPS
Anaesthetix 0:0929d3d566cf 194 BITS_FS_2000DPS
Anaesthetix 0:0929d3d566cf 195 returns the range set (250,500,1000 or 2000)
Anaesthetix 0:0929d3d566cf 196 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 197 unsigned int mpu9250_spi::set_gyro_scale(int scale){
Anaesthetix 0:0929d3d566cf 198 unsigned int temp_scale;
Anaesthetix 0:0929d3d566cf 199 //WriteReg(MPUREG_GYRO_CONFIG, scale);
Anaesthetix 0:0929d3d566cf 200 switch (scale){
Anaesthetix 0:0929d3d566cf 201 case BITS_FS_250DPS:
Anaesthetix 0:0929d3d566cf 202 gyro_divider=131;
Anaesthetix 0:0929d3d566cf 203 break;
Anaesthetix 0:0929d3d566cf 204 case BITS_FS_500DPS:
Anaesthetix 0:0929d3d566cf 205 gyro_divider=65.5;
Anaesthetix 0:0929d3d566cf 206 break;
Anaesthetix 0:0929d3d566cf 207 case BITS_FS_1000DPS:
Anaesthetix 0:0929d3d566cf 208 gyro_divider=32.8;
Anaesthetix 0:0929d3d566cf 209 break;
Anaesthetix 0:0929d3d566cf 210 case BITS_FS_2000DPS:
Anaesthetix 0:0929d3d566cf 211 gyro_divider=16.4;
Anaesthetix 0:0929d3d566cf 212 break;
Anaesthetix 0:0929d3d566cf 213 }
Anaesthetix 0:0929d3d566cf 214 //temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
Anaesthetix 0:0929d3d566cf 215 switch (temp_scale){
Anaesthetix 0:0929d3d566cf 216 case BITS_FS_250DPS:
Anaesthetix 0:0929d3d566cf 217 temp_scale=250;
Anaesthetix 0:0929d3d566cf 218 break;
Anaesthetix 0:0929d3d566cf 219 case BITS_FS_500DPS:
Anaesthetix 0:0929d3d566cf 220 temp_scale=500;
Anaesthetix 0:0929d3d566cf 221 break;
Anaesthetix 0:0929d3d566cf 222 case BITS_FS_1000DPS:
Anaesthetix 0:0929d3d566cf 223 temp_scale=1000;
Anaesthetix 0:0929d3d566cf 224 break;
Anaesthetix 0:0929d3d566cf 225 case BITS_FS_2000DPS:
Anaesthetix 0:0929d3d566cf 226 temp_scale=2000;
Anaesthetix 0:0929d3d566cf 227 break;
Anaesthetix 0:0929d3d566cf 228 }
Anaesthetix 0:0929d3d566cf 229 return temp_scale;
Anaesthetix 0:0929d3d566cf 230 }
Anaesthetix 0:0929d3d566cf 231
Anaesthetix 0:0929d3d566cf 232
Anaesthetix 0:0929d3d566cf 233 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 234 WHO AM I?
Anaesthetix 0:0929d3d566cf 235 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
Anaesthetix 0:0929d3d566cf 236 mpu9250 which should be 104 when in SPI mode.
Anaesthetix 0:0929d3d566cf 237 returns the I2C address (104)
Anaesthetix 0:0929d3d566cf 238 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 239 unsigned int mpu9250_spi::whoami(){
Anaesthetix 0:0929d3d566cf 240 unsigned int response;
Anaesthetix 0:0929d3d566cf 241 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
Anaesthetix 0:0929d3d566cf 242 return response;
Anaesthetix 0:0929d3d566cf 243 }
Anaesthetix 0:0929d3d566cf 244
Anaesthetix 0:0929d3d566cf 245
Anaesthetix 0:0929d3d566cf 246 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 247 READ ACCELEROMETER
Anaesthetix 0:0929d3d566cf 248 usage: call this function to read accelerometer data. Axis represents selected axis:
Anaesthetix 0:0929d3d566cf 249 0 -> X axis
Anaesthetix 0:0929d3d566cf 250 1 -> Y axis
Anaesthetix 0:0929d3d566cf 251 2 -> Z axis
Anaesthetix 0:0929d3d566cf 252 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 253 void mpu9250_spi::read_acc()
Anaesthetix 0:0929d3d566cf 254 {
Anaesthetix 0:0929d3d566cf 255 uint8_t response[6];
Anaesthetix 0:0929d3d566cf 256 int16_t bit_data;
Anaesthetix 0:0929d3d566cf 257 float data;
Anaesthetix 0:0929d3d566cf 258 int i;
Anaesthetix 0:0929d3d566cf 259 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
Anaesthetix 0:0929d3d566cf 260 for(i=0; i<3; i++) {
Anaesthetix 0:0929d3d566cf 261 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
Anaesthetix 0:0929d3d566cf 262 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 263 accelerometer_data[i]=data/acc_divider;
Anaesthetix 0:0929d3d566cf 264 }
Anaesthetix 0:0929d3d566cf 265
Anaesthetix 0:0929d3d566cf 266 }
Anaesthetix 0:0929d3d566cf 267
Anaesthetix 0:0929d3d566cf 268 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 269 READ GYROSCOPE
Anaesthetix 0:0929d3d566cf 270 usage: call this function to read gyroscope data. Axis represents selected axis:
Anaesthetix 0:0929d3d566cf 271 0 -> X axis
Anaesthetix 0:0929d3d566cf 272 1 -> Y axis
Anaesthetix 0:0929d3d566cf 273 2 -> Z axis
Anaesthetix 0:0929d3d566cf 274 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 275 void mpu9250_spi::read_rot()
Anaesthetix 0:0929d3d566cf 276 {
Anaesthetix 0:0929d3d566cf 277 uint8_t response[6];
Anaesthetix 0:0929d3d566cf 278 int16_t bit_data;
Anaesthetix 0:0929d3d566cf 279 float data;
Anaesthetix 0:0929d3d566cf 280 int i;
Anaesthetix 0:0929d3d566cf 281 ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
Anaesthetix 0:0929d3d566cf 282 for(i=0; i<3; i++) {
Anaesthetix 0:0929d3d566cf 283 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
Anaesthetix 0:0929d3d566cf 284 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 285 gyroscope_data[i]=data/gyro_divider;
Anaesthetix 0:0929d3d566cf 286 }
Anaesthetix 0:0929d3d566cf 287
Anaesthetix 0:0929d3d566cf 288 }
Anaesthetix 0:0929d3d566cf 289
Anaesthetix 0:0929d3d566cf 290 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 291 READ TEMPERATURE
Anaesthetix 0:0929d3d566cf 292 usage: call this function to read temperature data.
Anaesthetix 0:0929d3d566cf 293 returns the value in °C
Anaesthetix 0:0929d3d566cf 294 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 295 void mpu9250_spi::read_temp(){
Anaesthetix 0:0929d3d566cf 296 uint8_t response[2];
Anaesthetix 0:0929d3d566cf 297 int16_t bit_data;
Anaesthetix 0:0929d3d566cf 298 float data;
Anaesthetix 0:0929d3d566cf 299 ReadRegs(MPUREG_TEMP_OUT_H,response,2);
Anaesthetix 0:0929d3d566cf 300
Anaesthetix 0:0929d3d566cf 301 bit_data=((int16_t)response[0]<<8)|response[1];
Anaesthetix 0:0929d3d566cf 302 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 303 Temperature=(data/340)+36.53;
Anaesthetix 0:0929d3d566cf 304 deselect();
Anaesthetix 0:0929d3d566cf 305 }
Anaesthetix 0:0929d3d566cf 306
Anaesthetix 0:0929d3d566cf 307 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 308 READ ACCELEROMETER CALIBRATION
Anaesthetix 0:0929d3d566cf 309 usage: call this function to read accelerometer data. Axis represents selected axis:
Anaesthetix 0:0929d3d566cf 310 0 -> X axis
Anaesthetix 0:0929d3d566cf 311 1 -> Y axis
Anaesthetix 0:0929d3d566cf 312 2 -> Z axis
Anaesthetix 0:0929d3d566cf 313 returns Factory Trim value
Anaesthetix 0:0929d3d566cf 314 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 315 void mpu9250_spi::calib_acc()
Anaesthetix 0:0929d3d566cf 316 {
Anaesthetix 0:0929d3d566cf 317 uint8_t response[4];
Anaesthetix 0:0929d3d566cf 318 int temp_scale;
Anaesthetix 0:0929d3d566cf 319 //READ CURRENT ACC SCALE
Anaesthetix 0:0929d3d566cf 320 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
Anaesthetix 0:0929d3d566cf 321 set_acc_scale(BITS_FS_8G);
Anaesthetix 0:0929d3d566cf 322 //ENABLE SELF TEST need modify
Anaesthetix 0:0929d3d566cf 323 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
Anaesthetix 0:0929d3d566cf 324
Anaesthetix 0:0929d3d566cf 325 ReadRegs(MPUREG_SELF_TEST_X,response,4);
Anaesthetix 0:0929d3d566cf 326 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
Anaesthetix 0:0929d3d566cf 327 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
Anaesthetix 0:0929d3d566cf 328 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
Anaesthetix 0:0929d3d566cf 329
Anaesthetix 0:0929d3d566cf 330 set_acc_scale(temp_scale);
Anaesthetix 0:0929d3d566cf 331 }
Anaesthetix 0:0929d3d566cf 332 uint8_t mpu9250_spi::AK8963_whoami(){
Anaesthetix 0:0929d3d566cf 333 uint8_t response;
Anaesthetix 0:0929d3d566cf 334 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
Anaesthetix 0:0929d3d566cf 335 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 336 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
Anaesthetix 0:0929d3d566cf 337
Anaesthetix 0:0929d3d566cf 338 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
Anaesthetix 0:0929d3d566cf 339 wait(0.001);
Anaesthetix 0:0929d3d566cf 340 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
Anaesthetix 0:0929d3d566cf 341 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
Anaesthetix 0:0929d3d566cf 342 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
Anaesthetix 0:0929d3d566cf 343
Anaesthetix 0:0929d3d566cf 344 return response;
Anaesthetix 0:0929d3d566cf 345 }
Anaesthetix 0:0929d3d566cf 346 void mpu9250_spi::AK8963_calib_Magnetometer(){
Anaesthetix 0:0929d3d566cf 347 uint8_t response[3];
Anaesthetix 0:0929d3d566cf 348 float data;
Anaesthetix 0:0929d3d566cf 349 int i;
Anaesthetix 0:0929d3d566cf 350
Anaesthetix 0:0929d3d566cf 351 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
Anaesthetix 0:0929d3d566cf 352 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 353 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
Anaesthetix 0:0929d3d566cf 354
Anaesthetix 0:0929d3d566cf 355 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
Anaesthetix 0:0929d3d566cf 356 wait(0.001);
Anaesthetix 0:0929d3d566cf 357 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
Anaesthetix 0:0929d3d566cf 358 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
Anaesthetix 0:0929d3d566cf 359
Anaesthetix 0:0929d3d566cf 360 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
Anaesthetix 0:0929d3d566cf 361 for(i=0; i<3; i++) {
Anaesthetix 0:0929d3d566cf 362 data=response[i];
Anaesthetix 0:0929d3d566cf 363 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
Anaesthetix 0:0929d3d566cf 364 }
Anaesthetix 0:0929d3d566cf 365 }
Anaesthetix 0:0929d3d566cf 366 void mpu9250_spi::AK8963_read_Magnetometer(){
Anaesthetix 0:0929d3d566cf 367 uint8_t response[7];
Anaesthetix 0:0929d3d566cf 368 int16_t bit_data;
Anaesthetix 0:0929d3d566cf 369 float data;
Anaesthetix 0:0929d3d566cf 370 int i;
Anaesthetix 0:0929d3d566cf 371
Anaesthetix 0:0929d3d566cf 372 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
Anaesthetix 0:0929d3d566cf 373 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 374 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
Anaesthetix 0:0929d3d566cf 375
Anaesthetix 0:0929d3d566cf 376 wait(0.001);
Anaesthetix 0:0929d3d566cf 377 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
Anaesthetix 0:0929d3d566cf 378 //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.
Anaesthetix 0:0929d3d566cf 379 for(i=0; i<3; i++) {
Anaesthetix 0:0929d3d566cf 380 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
Anaesthetix 0:0929d3d566cf 381 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 382 Magnetometer[i]=data*Magnetometer_ASA[i];
Anaesthetix 0:0929d3d566cf 383 }
Anaesthetix 0:0929d3d566cf 384 }
Anaesthetix 0:0929d3d566cf 385 void mpu9250_spi::read_all(){
Anaesthetix 0:0929d3d566cf 386 uint8_t response[21];
Anaesthetix 0:0929d3d566cf 387 int16_t bit_data;
Anaesthetix 0:0929d3d566cf 388 float data;
Anaesthetix 0:0929d3d566cf 389 int i;
Anaesthetix 0:0929d3d566cf 390 spi.format(8,0);
Anaesthetix 0:0929d3d566cf 391 spi.frequency(4000000);
Anaesthetix 0:0929d3d566cf 392 //Send I2C command at first
Anaesthetix 0:0929d3d566cf 393 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
Anaesthetix 0:0929d3d566cf 394 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
Anaesthetix 0:0929d3d566cf 395 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
Anaesthetix 0:0929d3d566cf 396 //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.
Anaesthetix 0:0929d3d566cf 397
Anaesthetix 0:0929d3d566cf 398 //wait(0.001);
Anaesthetix 0:0929d3d566cf 399 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
Anaesthetix 0:0929d3d566cf 400 //Get accelerometer value
Anaesthetix 0:0929d3d566cf 401 for(i=0; i<3; i++) {
Anaesthetix 0:0929d3d566cf 402 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
Anaesthetix 0:0929d3d566cf 403 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 404 accelerometer_data[i]=data/16384;
Anaesthetix 0:0929d3d566cf 405 }
Anaesthetix 0:0929d3d566cf 406 //Get temperature
Anaesthetix 0:0929d3d566cf 407 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
Anaesthetix 0:0929d3d566cf 408 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 409 Temperature=((data-21)/333.87)+21;
Anaesthetix 0:0929d3d566cf 410 //Get gyroscop value
Anaesthetix 0:0929d3d566cf 411 for(i=4; i<7; i++) {
Anaesthetix 0:0929d3d566cf 412 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
Anaesthetix 0:0929d3d566cf 413 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 414 gyroscope_data[i-4]=data/65.5; //32.8
Anaesthetix 0:0929d3d566cf 415 }
Anaesthetix 0:0929d3d566cf 416 //Get Magnetometer value
Anaesthetix 0:0929d3d566cf 417 for(i=7; i<10; i++) {
Anaesthetix 0:0929d3d566cf 418 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
Anaesthetix 0:0929d3d566cf 419 data=(float)bit_data;
Anaesthetix 0:0929d3d566cf 420 Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
Anaesthetix 0:0929d3d566cf 421 }
Anaesthetix 0:0929d3d566cf 422 }
Anaesthetix 0:0929d3d566cf 423
Anaesthetix 0:0929d3d566cf 424 /*-----------------------------------------------------------------------------------------------
Anaesthetix 0:0929d3d566cf 425 SPI SELECT AND DESELECT
Anaesthetix 0:0929d3d566cf 426 usage: enable and disable mpu9250 communication bus
Anaesthetix 0:0929d3d566cf 427 -----------------------------------------------------------------------------------------------*/
Anaesthetix 0:0929d3d566cf 428 void mpu9250_spi::select() {
Anaesthetix 0:0929d3d566cf 429 //Set CS low to start transmission (interrupts conversion)
Anaesthetix 0:0929d3d566cf 430 cs = 0;
Anaesthetix 0:0929d3d566cf 431 }
Anaesthetix 0:0929d3d566cf 432 void mpu9250_spi::deselect() {
Anaesthetix 0:0929d3d566cf 433 //Set CS high to stop transmission (restarts conversion)
Anaesthetix 0:0929d3d566cf 434 cs = 1;
Anaesthetix 0:0929d3d566cf 435 }
Anaesthetix 0:0929d3d566cf 436