/* * gyro.c * * Created on: 16 sep. 2016 * Author: rsd12002 */ #include #include "drivers/spi.h" #include "utilities.h" #include "math.h" spi_profile mpu6000_spi_profile; uint8_t num_failed_receive = 0; /*********************************************************************** * BRIEF: SPI1_Init initializes the SPI1 instance with predefined values* * INFORMATION * ***********************************************************************/ bool spi1_init() { return spi_init(SPI1, &mpu6000_spi_profile, MPU6000_NSS_PIN, MPU6000_NSS_PORT); } /*********************************************************************** * BRIEF: mpu6000_Transmit transmits the specified register and data * * to the mpu6000 * * INFORMATION: * * data[0] = register * * data[1] = command * ***********************************************************************/ bool mpu6000_transmit(uint8_t* data, uint8_t length) { return spi_transmit(&mpu6000_spi_profile, data, length, 10); } /*********************************************************************** * BRIEF: mpu6000_TransmitReceive is used to request data from the * * mpu6000 which it stores in data * * INFORMATION: * ***********************************************************************/ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32_t timeout_ms) { return spi_receive_reg_value(&mpu6000_spi_profile, reg, data, length, timeout_ms); } /*********************************************************************** * BRIEF: mpu6000_read_offset reads and returns the offset of the * * gyroscope * * INFORMATION: * * Is automatically called when mpu6000_init is called * * The flight controller needs to be stationary when this function is * * called * * When the UAV is finished this data could be saved so that the * * offset doesn't need to be read every time * ***********************************************************************/ HAL_StatusTypeDef mpu6000_read_gyro_offset(gyro_t* gyro) { uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */ int16_t dataGCheck[6] = {0}; /* checkval */ uint8_t maxDrift = 5; bool calibrate_go = true; bool calibrate_ok = false; int countCalibrate = 0; //small delay so that we know the gyro should give some values HAL_Delay(100); for (int i = 0; i < 100; i++) { //assume that we should calibrate at the start of each loop calibrate_go = true; //get new values if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10)) return HAL_ERROR; //cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still for(int j = 0; j < 6; j ++) { int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]); if (!(currentDrift < maxDrift)) calibrate_go = false; dataGCheck[j] = dataG[j]; } //if true, we have good values exit loop if (calibrate_go) { countCalibrate++; } if (countCalibrate > 4) { calibrate_ok = true; break; } //wait for a little bit so the checks are not to fast HAL_Delay(200); } #ifdef YAW_ROT_0 gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); #elif defined(YAW_ROT_90) gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); #elif defined(YAW_ROT_180) gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); #elif defined(YAW_ROT_270) gyro->offsetX = (((int16_t)dataG[2] << 8) | dataG[3]); gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]); gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); #endif return HAL_OK; } /*********************************************************************** * BRIEF: mpu6000_read_offset reads and returns the offset of the * * accelerometer * * INFORMATION: * * Is automatically called when mpu6000_init is called * * The flight controller needs to be stationary when this function is * * called * * When the UAV is finished this data could be saved so that the * * offset doesn't need to be read every time * ***********************************************************************/ HAL_StatusTypeDef mpu6000_read_acc_offset(accel_t* accel) { uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10)) return HAL_ERROR; #ifdef YAW_ROT_0 accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_90) accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_180) accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #elif defined(YAW_ROT_270) accel->offsetX = ((int16_t)dataA[2] << 8) | dataA[3]; accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1]; accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); #endif return HAL_OK; } /*********************************************************************** * BRIEF: mpu6000_Init initializes the gyroscope and accelerometer * * INFORMATION: * * Utilizing the GyroZ clock * * I2C bus disabled * * Sample rate division = 0 * * 256Hz Digital Low Pass Filter (DLPF) * * Full scale range of the gyroscope = ± 2000°/s * ***********************************************************************/ bool mpu6000_init(gyro_t* gyro, accel_t* accel) { spi1_init(); uint8_t reg[2]; /* Register address and bit selection */ // Reset device reg[0] = MPU_RA_PWR_MGMT_1; reg[1] = BIT_H_RESET; if(!mpu6000_transmit(reg, 2)) return false; HAL_Delay(10); // Reset Signal Path reg[0] = MPU_RA_SIGNAL_PATH_RESET; reg[1] = BIT_GYRO | BIT_ACC; HAL_Delay(150); if(!mpu6000_transmit(reg, 2)) return false; // Wake up device and select GyroZ clock (better performance) reg[0] = MPU_RA_PWR_MGMT_1; reg[1] = 0b00001000 | MPU_CLK_SEL_PLLGYROZ; if(!mpu6000_transmit(reg, 2)) return false; // Disable I2C bus reg[0] = MPU_RA_USER_CTRL; reg[1] = 0x50; if(!mpu6000_transmit(reg, 2)) return false; // No standby mode reg[0] = MPU_RA_PWR_MGMT_2; reg[1] = 0x00; if(!mpu6000_transmit(reg, 2)) return false; // Sample rate reg[0] = MPU_RA_SMPLRT_DIV; reg[1] = 0x00; if(!mpu6000_transmit(reg, 2)) return false; // Digital Low Pass Filter (DLPF) reg[0] = MPU_RA_CONFIG; reg[1] = BITS_DLPF_CFG_256HZ; if(!mpu6000_transmit(reg, 2)) return false; // FIFO reg[0] = MPU_RA_FIFO_EN; // Temperature GyroX GyroY GyroZ Accel Slave Slave Slave reg[1] = 0 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0; if(!mpu6000_transmit(reg, 2)) return false; // Gyroscope 2000DPS reg[0] = MPU_RA_GYRO_CONFIG; reg[1] = BITS_FS_2000DPS; gyro->scale = (1.0f / 16.4f); if(!mpu6000_transmit(reg, 2)) return false; // Accelerometer 16±G reg[0] = MPU_RA_ACCEL_CONFIG; reg[1] = BITS_FS_16G; accel->accel1G = 2048; // (32768/16)/G if(!mpu6000_transmit(reg, 2)) return false; mpu6000_read_gyro_offset(gyro); //mpu6000_read_acc_offset(accel); HAL_Delay(60); return true; } /*********************************************************************** * BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and * * stores the data, in °/s format, in the gyro struct * * INFORMATION: * ***********************************************************************/ bool mpu6000_read_gyro(gyro_t* gyro) { uint8_t data[6]; /* Temporary data variable used to receive gyroscope data */ HAL_StatusTypeDef err; /* SPI transmission status variable */ if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, data, 6, 10)) return false; #ifdef YAW_ROT_0 gyro->gyroX = -(((int16_t)data[0] << 8) | data[1]); gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; gyro->gyroY = -(((int16_t)data[2] << 8) | data[3]); gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; #elif defined(YAW_ROT_90) gyro->gyroX = -(((int16_t)data[2] << 8) | data[3]); gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; gyro->gyroY = (((int16_t)data[0] << 8) | data[1]); gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; #elif defined(YAW_ROT_180) gyro->gyroX = (((int16_t)data[0] << 8) | data[1]); gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; gyro->gyroY = (((int16_t)data[2] << 8) | data[3]); gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; #elif defined(YAW_ROT_270) gyro->gyroX = (((int16_t)data[2] << 8) | data[3]); gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; gyro->gyroY = -(((int16_t)data[0] << 8) | data[1]); gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; #else No Yaw Direction Defined #endif return true; } /*********************************************************************** * BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer * * INFORMATION: * * The data is both saved in raw format and in converted into the * * number of G (9.82 m/s^2) the accelerometer is sensing * ***********************************************************************/ bool mpu6000_read_accel(accel_t* accel) { uint8_t data[6]; /* Temporary data variable used to receive accelerometer data */ if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, data, 6, 10)) return false; #ifdef YAW_ROT_0 accel->accelXraw = -((int16_t)data[0] << 8) | data[1]; accel->accelXraw = accel->accelXraw - accel->offsetX; accel->accelYraw = -((int16_t)data[2] << 8) | data[3]; accel->accelYraw = accel->accelYraw - accel->offsetY; accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; accel->accelZraw = accel->accelZraw + accel->offsetZ; accel->accelXconv = ((float)accel->accelXraw / accel->accel1G); accel->accelYconv = ((float)accel->accelYraw / accel->accel1G); accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); #elif defined(YAW_ROT_90) accel->accelXraw = -((int16_t)data[2] << 8) | data[3]; accel->accelXraw = accel->accelXraw - accel->offsetX; accel->accelYraw = ((int16_t)data[0] << 8) | data[1]; accel->accelYraw = accel->accelYraw - accel->offsetY; accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; accel->accelZraw = accel->accelZraw + accel->offsetZ; accel->accelXconv = (float)(accel->accelYraw / accel->accel1G); accel->accelYconv = (float)(accel->accelXraw / accel->accel1G); accel->accelZconv = (float)(accel->accelZraw / accel->accel1G); #elif defined(YAW_ROT_180) accel->accelXraw = ((int16_t)data[0] << 8) | data[1]; accel->accelXraw = accel->accelXraw - accel->offsetX; accel->accelYraw = ((int16_t)data[2] << 8) | data[3]; accel->accelYraw = accel->accelYraw - accel->offsetY; accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; accel->accelZraw = accel->accelZraw + accel->offsetZ; accel->accelXconv = ((float)accel->accelXraw / accel->accel1G); accel->accelYconv = ((float)accel->accelYraw / accel->accel1G); accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); #elif defined(YAW_ROT_270) accel->accelXraw = ((int16_t)data[2] << 8) | data[3]; accel->accelXraw = accel->accelXraw - accel->offsetX; accel->accelYraw = -((int16_t)data[0] << 8) | data[1]; accel->accelYraw = accel->accelYraw - accel->offsetY; accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; accel->accelZraw = accel->accelZraw + accel->offsetZ; accel->accelXconv = ((float)accel->accelYraw / accel->accel1G); accel->accelYconv = ((float)accel->accelXraw / accel->accel1G); accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); #endif return true; } /*********************************************************************** * BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyroscope axis from the * * FIFO queue * * INFORMATION: * * Reads every complete set of gyro data (6 bytes) from the queue and * * stores it it data_out * * returns: * * -1 if SPI transmission error occurs * * -2 if FIFO queue overflow * * -3 if FIFO queue doesn't contain any complete set of gyro data * * else the number of bytes read from the FIFO queue * ***********************************************************************/ int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out) { uint16_t fifoCount = 0; /* Number of bytes in the FIFO queue */ uint8_t countH = 0; /* Bits 8-16 of the number of bytes in the FIFO queue */ uint8_t countL = 0; /* Bits 0-7 of the number of bytes in the FIFO queue */ uint16_t bytesRead = 0; /* Number of bytes actually read from the FIFO queue */ uint8_t reg[2]; /* Register address and bit selection */ if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTH, &countH, 1, 10)) return -1; if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTL, &countL, 1, 10)) return -1; fifoCount = (uint16_t)((countH << 8) | countL); if (fifoCount == 1024 || num_failed_receive > 20) { reg[0] = MPU_RA_USER_CTRL; reg[1] = BIT_FIFO_RESET; mpu6000_transmit(reg, 2); reg[0] = MPU_RA_USER_CTRL; reg[1] = 1<<6; mpu6000_transmit(reg, 2); return -2; } if (fifoCount < 6) { num_failed_receive++; return -3; } else num_failed_receive = 0; /* Make sure that only complete sets of gyro data are read */ bytesRead = (fifoCount/6)*6; uint8_t fifobuffer[bytesRead+1]; if(!mpu6000_transmit_receive(MPU_RA_FIFO_R_W, fifobuffer, bytesRead, 20)) return false; uint8_t xL, xH, yL, yH, zL, zH; for(int j = 0; 6+((j-1)*6) < bytesRead; j++) { xH = fifobuffer[0+((j-1)*6)]; xL = fifobuffer[1+((j-1)*6)]; yH = fifobuffer[2+((j-1)*6)]; yL = fifobuffer[3+((j-1)*6)]; zH = fifobuffer[4+((j-1)*6)]; zL = fifobuffer[5+((j-1)*6)]; #ifdef YAW_ROT_0 data_out[0+((j-1)*3)] = -(((int16_t)xH << 8) | xL); // X data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; data_out[1+((j-1)*3)] = -(((int16_t)yH << 8) | yL); // Y data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; #elif defined(YAW_ROT_90) data_out[0+((j-1)*3)] = -(((int16_t)yH << 8) | yL); // X data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; data_out[1+((j-1)*3)] = (((int16_t)xH << 8) | xL); // Y data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; #elif defined(YAW_ROT_180) data_out[0+((j-1)*3)] = (((int16_t)xH << 8) | xL); // X data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; data_out[1+((j-1)*3)] = (((int16_t)yH << 8) | yL); // Y data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; #elif defined(YAW_ROT_270) data_out[0+((j-1)*3)] = (((int16_t)yH << 8) | yL); // X data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; data_out[1+((j-1)*3)] = -(((int16_t)xH << 8) | xL); // Y data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; #endif } return bytesRead; } /*********************************************************************** * BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to * * confirm device * * INFORMATION: * * returns true if correct device and revision if found * ***********************************************************************/ bool mpu6000_who_am_i() { uint8_t data = 0; /* Received data is placed in this variable */ if(!mpu6000_transmit_receive(MPU_RA_WHO_AM_I, &data, 1, 10)) return false; if (data != MPU6000_WHO_AM_I_CONST) { return false; } /* look for a product ID we recognize */ if(!mpu6000_transmit_receive(MPU_RA_PRODUCT_ID, &data, 1, 10)) return false; // verify product revision switch (data) { case MPU6000ES_REV_C4: case MPU6000ES_REV_C5: case MPU6000_REV_C4: case MPU6000_REV_C5: case MPU6000ES_REV_D6: case MPU6000ES_REV_D7: case MPU6000ES_REV_D8: case MPU6000_REV_D6: case MPU6000_REV_D7: case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: return true; } return false; } /* Returns the angle. If magnitude of acc is out of range we calculate on integrated gyro data */ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro) { static uint32_t last_micros = 0; // Static stores micros measured from last iteration uint32_t current_micros = clock_get_us(); uint32_t delta_t = current_micros - last_micros; last_micros = current_micros; static float lpf_Acc[3] = {0}; static float smooth[3] = {0}; float sign[3] = {0}; /* We read the accelerometer to get fresh data */ mpu6000_read_accel(accel); /* Filter part */ for (int i = 0; i < 3; i ++) { smooth[i] = lpf_Acc[i] / 16; } sign[0] = (accel->accelXconv< 0) ? -1 : 1; lpf_Acc[0] += sign[0]*sqrtf(ABS_FLOAT(accel->accelXconv)) - smooth[0]; sign[1] = (accel->accelYconv< 0) ? -1 : 1; lpf_Acc[1] += sign[1]*sqrtf(ABS_FLOAT(accel->accelYconv)) - smooth[1]; sign[2] = (accel->accelZconv< 0) ? -1 : 1; lpf_Acc[2] += sign[2]*sqrtf(ABS_FLOAT(accel->accelZconv)) - smooth[2]; accel->accelXconv = smooth[0] * smooth[0] * sign[0]; accel->accelYconv = smooth[1] * smooth[1] * sign[1]; accel->accelZconv = smooth[2] * smooth[2] * sign[2]; /* The total magnitude of the acceleration */ float magnitude = sqrtf( \ ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \ ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \ ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \ ); /* Everything is normal. No outer forces */ if (0.85 < magnitude && magnitude < 1.15) { // Roll accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI; // Pitch accel->pitchAngle = atan2( accel->accelYconv, sqrt(accel->accelZconv*accel->accelZconv + accel->accelXconv*accel->accelXconv))*180/M_PI; } /* Too big forces to calculate on ACC data. Fallback on gyro integration */ else { accel->rollAngle += (float)(delta_t * gyro->gyroY) / 1000000.0; accel->pitchAngle += (float)(delta_t * gyro->gyroX) / 1000000.0; accel->rollAngle = constrainf(accel->rollAngle, -90,90); accel->pitchAngle = constrainf(accel->pitchAngle, -90,90); } }