void setup() {
Wire.begin();
if (debugSerial){
Serial.begin(115200);
Serial.println(F("=================== SETUP ================="));
}
// initialize device
if (debugSerial && debugMPU6050) Serial.println(F("Initializing I2C devices..."));
accelgyro.initialize();
// verify connection
if (debugSerial && debugMPU6050) {
Serial.println("Testing device connections...");
boolean OK = accelgyro.testConnection() ;
( OK )?
Serial.println(F("MPU6050 connection successful")):
Serial.println(F("MPU6050 connection failed"));
}
if (debugSerial){
Serial.println(F("=============== FIM SETUP ================="));
}
}
bool i2cSetupGyro()
{
//wake up gyro
//return I2Cdev::writeBit( gyroAddr, 0x6b, 6, 0b0);
gyroscope.initialize();
return gyroscope.testConnection();
}
void setup() {
Wire.begin();
Serial.begin(57600);
I2Cdev::writeByte(0x68, 0x6B, 0x01); //PWR_MGMT1 for clock source as X-gyro
uint8_t temp[8] = {8, 0};//GYRO range:250, ACCEL range:2g | Refer the Datasheet if you want to change these.
I2Cdev::writeBytes(0x68, 0x1B, 2, temp); //GYRO_CFG and ACCEL_CFG
accelgyro.setRate(4);
accelgyro.setDLPFMode(0x03);
accelgyro.setFIFOEnabled(true);
I2Cdev::writeByte(0x68, 0x23, 0x78); //FIFO_EN for ACCEL,GYRO
accelgyro.setDMPEnabled(false);
I2Cdev::writeByte(0x68, 0x38, 0x11); //INT_EN
attachInterrupt(0, mpu_interrupt, RISING);
attachInterrupt(1, compass_interrupt, FALLING);
//Put the HMC5883 IC into the correct operating mode
Wire.beginTransmission(0x1E); //open communication with HMC5883
Wire.write(0x00); //select Config_Register_A:
Wire.write(0x58); //4-point avg. and 75Hz rate
Wire.write(0x02); //In Config_Register_B: +-1.9G (820LSB/G)
Wire.write(0x00); //In Mode_Register: continuous measurement mode
Wire.endTransmission();
pinMode(13, INPUT);
#ifdef CAL_DEBUG
Serial.print("Calibrating Gyros and Accel! Hold Still and Level!");
#endif
calibrate_gyros();
calibrate_accel();
accelgyro.resetFIFO();
}
// FUNCION MAIN
int main () {
// Parar con control-c
signal(SIGINT, my_function);
// Declaramos las dos estructuras y la clase acelerometro para su control
Espacio aceleracion, gyroscocion;
MPU6050 acelerometro;
// Conectamos con el acelerometro y pedimos datos
acelerometro.conectamos_acelerometro();
while (true){
if(ctrlc){
whiler = false;
}
aceleracion = acelerometro.get_aceleraciones();
gyroscocion = acelerometro.get_giroscopio();
printf("Aceleracion: [%d, %d, %d]\n", aceleracion.x, aceleracion.y, aceleracion.z);
printf("Giroscopo: [%d, %d, %d]\n", gyroscocion.x, gyroscocion.y, gyroscocion.z);
}
}
int ImuTester::run()
{
// Default is fail unless pass critera met
m_pass = TEST_FAIL;
// Register the driver
int ret = m_sensor.init();
// Open the IMU sensor
DevHandle h;
DevMgr::getHandle(IMU_DEVICE_PATH, h);
if (!h.isValid()) {
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
IMU_DEVICE_PATH, h.getError());
m_done = true;
} else {
m_done = false;
}
while (!m_done) {
++m_read_attempts;
struct imu_sensor_data data;
ret = ImuSensor::getSensorData(h, data, true);
if (ret == 0) {
uint32_t count = data.read_counter;
DF_LOG_INFO("count: %d", count);
if (m_read_counter != count) {
m_read_counter = count;
ImuSensor::printImuValues(h, data);
}
} else {
DF_LOG_INFO("error: unable to read the IMU sensor device.");
}
if (m_read_counter >= num_read_attempts) {
// Done test - PASSED
m_pass = TEST_PASS;
m_done = true;
} else if (m_read_attempts > num_read_attempts) {
DF_LOG_INFO("error: unable to read the IMU sensor device.");
m_done = true;
}
}
DevMgr::releaseHandle(h);
DF_LOG_INFO("Closing IMU sensor");
m_sensor.stop();
return m_pass;
}
void setup() {
// initialize device
printf("Initializing I2C devices...\n");
accelgyro.initialize();
gettimeofday(&tv0, NULL);
// verify connection
printf("Testing device connections...\n");
printf(accelgyro.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n");
}
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
//Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
Serial.begin(38400);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
send_status(MPU_INITIALIZE, STATUS_OK);
// verify connection
send_status(MPU_CONNECTION, mpu.testConnection() ? STATUS_OK : STATUS_FAIL);
// load and configure the DMP
// 0 = DMP OK
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
ua_dev_status = mpu.dmpInitialize();
send_status(DMP_INITIALIZE, ua_dev_status);
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(120);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-185);
mpu.setZAccelOffset(1688); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (ua_dev_status == 0)
{
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachPinChangeInterrupt(INTERRUPT_PIN, dmpDataReady, RISING);
ua_mpu_interrupt_status = mpu.getIntStatus();
send_status(DMP_INTERRUPT, ua_mpu_interrupt_status);
b_dmp_ready = true;
// get expected DMP packet size for later comparison
uh_packet_size = mpu.dmpGetFIFOPacketSize();
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
void setup() {
Serial.begin(38400);
Serial.print("Device type: 0x");Serial.println(HW_TYPE,HEX,8);
Serial.print("Device serial: 0x");Serial.println(HW_SERIAL,HEX,8);
//Dump source package to serial
int len=source_end-source_start;
char* base=source_start;
Serial.print("Source package length: ");
Serial.println(len,DEC);
Serial.print("Source package start: 0x");
Serial.println((int)base,DEC,8);
d.begin();
while(len>0) {
d.line(base,0,len>120?120:len);
base+=120;
len-=120;
}
d.end();
Wire1.begin();
SPI1.begin(1000000,1,1);
bool worked=sd.begin();
Serial.print("sd"); Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(sd.errnum);
worked=p.begin(1);
Serial.print("p"); Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(p.errnum);
worked=fs.begin();
Serial.print("fs"); Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(fs.errnum);
sector.begin();
fs.print(Serial,sector);
mpu6050.begin();
Serial.print("MPU6050 identifier (should be 0x68): 0x");
Serial.println(mpu6050.whoami(),HEX);
hmc5883.begin();
char HMCid[4];
hmc5883.whoami(HMCid);
Serial.print("HMC5883L identifier (should be 'H43'): ");
Serial.println(HMCid);
worked=ad799x.begin(0xB); //Turn on channels 0, 1, and 3
Serial.print("ad799x");Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(0);
worked=bmp180.begin();
Serial.print("bmp180");Serial.print(".begin ");Serial.print(worked?"Worked":"didn't work");Serial.print(". Status code ");Serial.println(0);
Serial.print("BMP180 identifier (should be 0x55): 0x");
Serial.println(bmp180.whoami(),HEX);
bmp180.printCalibration(&Serial);
}
/* Initialize the IMU and I2C communication */
void IMUInit(){
/* Intialize communication I2C */
Wire.begin();
/* Initialize IMU. Default resolution is set to minimum, no offsets */
IMU.initialize();
/* Get first values for accel and gyro */
IMU.getMotion6(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]);
}
void setup_IMU()
{
I2CInitialize();
mpu.initialize();
devStatus = mpu.dmpInitialize();
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
}
void update_IMU()
{
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) mpu.resetFIFO();
// otherwise, check for DMP data ready interrupt (this should happen frequently)
else if (mpuIntStatus & 0x02)
{
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
mpu.dmpGetGyro(gyroRate,fifoBuffer);
gyro_rate_float[0] = (float)gyroRate[0]/2147483648*2000*0.41;
gyro_rate_float[1] = (float)gyroRate[1]/2147483648*2000*0.41;
gyro_rate_float[2] = (float)gyroRate[2]/2147483648*2000*0.41;
}
}
void compute_imu(float _looptime) {
looptime = _looptime;
// get sensor readings
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
gx -= imu_offset[3];
gy -= imu_offset[4];
gz -= imu_offset[5];
mx = mpu.getExternalSensorWord(0);
my = mpu.getExternalSensorWord(2);
mz = mpu.getExternalSensorWord(4);
/* computations */
compute_state_imu();
}
void setup_IMU()
{
tmObjectInit(&lagedatalogsync_tmup);
I2CInitialize();
mpu.initialize();
devStatus = mpu.dmpInitialize();
if (devStatus == 0)
{
//chThdCreateStatic(LageSyncThreadWorkingArea, sizeof(LageSyncThreadWorkingArea), NORMALPRIO, LageSyncthread, NULL);
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
}
void loop() {
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&accData[0], &accData[1], &accData[2], &gyrData[0], &gyrData[1], &gyrData[2]);
CalculatedT();
ComplementaryFilter(accData, gyrData, &final_pitch, &final_roll);
printf("pitch = %6f roll %6f dt = %lu\n", final_pitch, final_roll, elapsed);
}
/* Get readings from IMU and compute them to get the angles */
void computeIMU(){
/* Get Accel and Gyro readings */
IMU.getMotion6(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]);
/* Apply offsets */
for( int i=0 ; i<3 ; i++ ){
accel[i] += accelOffsets[i];
gyro[i] += gyroOffsets[i];
}
/* Refresh time variables */
delta = micros() - auxtime;
auxtime = delta + auxtime;
if( delta > maxdelta ) maxdelta = delta;
compute += delta;
times++;
if( times == 100 ){
avgDelta = compute/100;
times = 0;
compute = 0;
}
/* Compute offsets if needed */
if( !gyroOffReady[0] || !gyroOffReady[1] || !gyroOffReady[2] )
computeGyroOffsets();
if( !accelOffReady[0] || !accelOffReady[1] || !accelOffReady[2] )
computeAccelOffsets();
}
void print_calculated_angular(){
Serial.print("gyro_angle:"); Serial.print(gyro_angle); Serial.print(" ");
Serial.print("range: ");Serial.print(mpu.getFullScaleGyroRange());
Serial.print("mag_angle:"); Serial.print(mag_angle); Serial.print(" ");
Serial.print("mag_angle_2:"); Serial.print(mag_angle_2); Serial.print(" ");
// Serial.print("current_angle:"); Serial.print(current_angle); Serial.print(" ");
}
void CalculateOffsets(uint8_t gyroSamplingRate)
{
int numReadings = 0;
long base_x_gyro = 0;
long base_y_gyro = 0;
long base_z_gyro = 0;
long base_x_accel = 0;
long base_y_accel = 0;
long base_z_accel = 0;
int16_t ax, ay, az, gx, gy, gz;
delay(100);
// Discard the first few reading
unsigned long before = millis();
unsigned long now;
do
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
now = millis();
}
while(now - before < 500/(gyroSamplingRate+1)); // ignore 500 frames of data
before = millis();
do
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// PrintMotion6Data(ax, ay, az, gx, gy, gz);
base_x_gyro += gx;
base_y_gyro += gy;
base_z_gyro += gz;
base_x_accel += ax;
base_y_accel += ay;
base_z_accel += az;
numReadings++;
now = millis();
}
while(now - before < 1000/(gyroSamplingRate+1)); // collect average over 1000 frames of data
base_x_gyro /= numReadings; gXOffset = base_x_gyro;
base_y_gyro /= numReadings; gYOffset = base_y_gyro;
base_z_gyro /= numReadings; gZOffset = base_z_gyro;
base_x_accel /= numReadings; aXOffset = base_x_accel;
base_y_accel /= numReadings; aYOffset = base_y_accel;
base_z_accel /= numReadings; aZOffset = base_z_accel;
}
bool GetMotion6(float angles[])
{
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); //400µs
angles[0] = accX; angles[1] = accY; angles[2] = accZ; angles[3] = gyroX; angles[4] = gyroY; angles[5] = gyroZ;
return true;
}
int DMP::getAttitude()
{
if (!dmpReady) return -1;
// wait for FIFO count > 42 bits
do {
fifoCount = mpu.getFIFOCount();
}while (fifoCount<42);
if (fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
return -1;
// otherwise, check for DMP data ready interrupt
//(this should happen frequently)
} else {
//read packet from fifo
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
for (int i=0;i<DIM;i++){
//offset removal
ypr[i]-=m_ypr_off[i];
//scaling for output in degrees
ypr[i]*=180/M_PI;
}
//printf(" %7.2f %7.2f %7.2f\n",ypr[0],ypr[1],ypr[2]);
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of ROLL, MPU is attached upside down
ypr[2]*=-1.0;
mpu.dmpGetGyro(g, fifoBuffer);
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
}
// printf("gyro %7.2f %7.2f %7.2f \n", (float)g[0]/131.0,
// (float)g[1]/131.0,
// (float)g[2]/131.0);
return 0;
}
}
void Gyro_update(void)
{
/* reset FIFO buffer and wait for first data */
mpu.resetFIFO();
mpu_interrupt = false;
do {
while (!mpu_interrupt) {
;
}
mpu_interrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
} while (!(mpuIntStatus & 0x02));
/* reading data */
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
/* converting to degrees and using offsets */
for (uint8_t i = 0; i < 3; i++) {
ypr[i] = (ypr[i] * 180 / M_PI) + ypr_offsets[i];
}
}
// setup AVR I2C
void userSetup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
}
// Read fifo
static void readFIFO()
{
int pkts = 0;
// Get data from FIFO
fifoCount = mpu.getFIFOCount();
if (fifoCount > 900) {
// Full is 1024, so 900 probably means things have gone bad
printf("Oops, DMP FIFO has %d bytes, aborting\n", fifoCount);
exit(1);
}
while ((fifoCount = mpu.getFIFOCount()) >= 42) {
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
pkts++;
}
if (pkts > 5)
printf("Found %d packets, running slowly\n", pkts);
}
void accelgyro_handler_planB()
{
timeout_watchdog = 0;
accelGyroIntStatus = accelgyro.getIntStatus();
if(accelGyroIntStatus & (0x1 << MPU6050_INTERRUPT_ZMOT_BIT)) // Zero motion detected
{
contZeroMoveDetect++;
DEBUG_PRINTF(V_MESSAGE, "Zero Motion Detected!\n");
if((accelGyroState == ACCELGYRO_STOP) && (accelGyroWaitEvent == ACCELGYRO_WAIT_MOVE))
{
DEBUG_PRINTF(V_MESSAGE, "Zero Motion detected but was expecting Move Detection! Number Zero Move Detections = %d.\n", contZeroMoveDetect);
}
else if((accelGyroState == ACCELGYRO_MOVE) && (accelGyroWaitEvent == ACCELGYRO_WAIT_STOP))
{
accelGyroState = ACCELGYRO_STOP;
accelGyroWaitEvent = ACCELGYRO_WAIT_MOVE;
if(operation_state == START_PHOTO_CAPTURE)
{
shouldStopPhotoTimer = true;
}
else
{
operation_state = IDLE;
}
contZeroMoveDetect = 0;
}
else
{
DEBUG_PRINTF(V_MESSAGE, "Zero Motion detected and wrong accelGyroState and accelGyroWaitEvent values!\n");
}
}
if(accelGyroIntStatus & (0x1 << MPU6050_INTERRUPT_MOT_BIT)) // Motion detected
{
contMoveDetect++;
DEBUG_PRINTF(V_MESSAGE, "Motion Detected!\n");
if((accelGyroState == ACCELGYRO_MOVE) && (accelGyroWaitEvent == ACCELGYRO_WAIT_STOP))
{
DEBUG_PRINTF(V_MESSAGE, "Motion detected but was expecting Move Detection! Number Move Detections = %d.\n", contMoveDetect);
}
if((accelGyroState == ACCELGYRO_STOP) && (accelGyroWaitEvent == ACCELGYRO_WAIT_MOVE))
{
stop_accelgyro_timer();
accelGyroState = ACCELGYRO_MOVE;
accelGyroWaitEvent = ACCELGYRO_WAIT_STOP;
operation_state = START_TEMPORIZATION;
operation_event_flag = 1;
contMoveDetect = 0;
}
else
{
DEBUG_PRINTF(V_MESSAGE, "Motion detected and wrong accelGyroState and accelGyroWaitEvent values!\n");
}
}
return;
}
void loop() {
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// these methods (and a few others) are also available
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);
// display accel/gyro x/y/z values
printf("a/g: %6hd %6hd %6hd %6hd %6hd %6hd\n",ax,ay,az,gx,gy,gz);
}
/*
* Updates function
*
*/
extern "C" void sf_MPU6050_2xDriver_GxAyz_Update_wrapper(const int16_T *x_vel,
const int16_T *y_acc,
const int16_T *z_acc,
const int16_T *x_vel_2,
const int16_T *y_acc_2,
const int16_T *z_acc_2,
real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */
if(xD[0] != 1){
# ifndef MATLAB_MEX_FILE
Wire.begin();
accelgyro.initialize();
accelgyro.setDLPFMode(MPU6050_DLPF_BW_42);
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
accelgyro2.initialize();
accelgyro2.setDLPFMode(MPU6050_DLPF_BW_42);
accelgyro2.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
#endif
//done with initialization
xD[0] = 1;
}
/* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
* Output functions
*
*/
extern "C" void sf_MPU6050_2xDriver_GxAyz_Outputs_wrapper(int16_T *x_vel,
int16_T *y_acc,
int16_T *z_acc,
int16_T *x_vel_2,
int16_T *y_acc_2,
int16_T *z_acc_2,
const real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
y0[0] = u0[0];
For complex signals use: y0[0].re = u0[0].re;
y0[0].im = u0[0].im;
y1[0].re = u1[0].re;
y1[0].im = u1[0].im;
*/
if(xD[0] == 1)
{
#ifndef MATLAB_MEX_FILE
x_vel[0]=accelgyro.getRotationX();
y_acc[0]=accelgyro.getAccelerationY();
z_acc[0]=accelgyro.getAccelerationZ();
x_vel_2[0]=accelgyro2.getRotationX();
y_acc_2[0]=accelgyro2.getAccelerationY();
z_acc_2[0]=accelgyro2.getAccelerationZ();
#endif
}
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}
void read_FIFO(){
uint8_t buffer[2];
int16_t temp = 0;
int samplz = 0;
samplz = accelgyro.getFIFOCount() / 2;
//SERIAL_OUT.println("FIFO_COUNTH : ");
//SERIAL_OUT.println(samplz,DEC);
for(int i=0; i < samplz; i++){
accelgyro.getFIFOBytes(buffer, 2);
temp = ((((int16_t)buffer[0]) << 8) | buffer[1]);
if (abs(temp) > 32765) gyro_error = true;
accum += temp*10 - gyro_null;
//accum = temp;
gyro_count++;
if((accum > GYRO_CAL) && (!cal_flag)) accum -= GYRO_CAL*2; //if we are calculating null, don't roll-over
if((accum < -GYRO_CAL) && (!cal_flag)) accum += GYRO_CAL*2;
}
angle = (float)accum/(float)GYRO_CAL * -3.14159; //change sign of PI for flipped gyro
//angle = (float)accum/GYRO_CAL * -180; //using degrees *10, negative for flipped gyro.
return ;
}
static void initMPU(void)
{
uint8_t error, c;
Wire.begin();
c = mpu.readWho(&error);
Serial.print("WHO_AM_I : ");
Serial.print(c,HEX);
Serial.print(", error = ");
Serial.println(error,DEC);
c = mpu.readSleepBit(&error);
Serial.print("PWR_MGMT_1 : ");
Serial.print(c,HEX);
Serial.print(", error = ");
Serial.println(error,DEC);
mpu.writeAccelFullScaleRange(16, NULL);
mpu.writeSleepBit(0, &error);
Serial.print("PWR_MGMT_1 write : error = ");
Serial.println(error,DEC);
}
void calibrate_gyros(){
byte i=0;
while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
accelgyro.getRotation(&gx, &gy, &gz);
if (count == SAMPLE_COUNT){
xoff += int(gxm/-3);
yoff += int(gym/-3);
zoff += int(gzm/-3);
accelgyro.setXGyroOffset(xoff);
accelgyro.setYGyroOffset(yoff);
accelgyro.setZGyroOffset(zoff);
#ifdef CAL_DEBUG
Serial.print(gxm); Serial.print(" ");
Serial.print(gym); Serial.print(" ");
Serial.println(gzm);
Serial.print(xoff); Serial.print(" ");
Serial.print(yoff); Serial.print(" ");
Serial.println(zoff);
Serial.println("*********************");
#endif
count = 0;
i++; //iteration++
#ifdef CAL_DEBUG
Serial.print(".");
#endif
}
else{
gxm = (gxm*count + gx)/(count+1.0);
gym = (gym*count + gy)/(count+1.0);
gzm = (gzm*count + gz)/(count+1.0);
count++;
}
}
#ifdef CAL_DEBUG
Serial.println(" Done.");
#endif
}
void gyro_acc()
{
// if programming failed, don't try to do anything
if (!dmpReady) return;
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if (fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (fifoCount >= 42) {
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
printf("ypr %7.2f %7.2f %7.2f t:%d ", ypr[0] * 180/M_PI, ypr[1] * 180/M_PI, ypr[2] * 180/M_PI,count_time);
count_time++;
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
printf("aworld %6d %6d %6d ", aaWorld.x, aaWorld.y, aaWorld.z);
#endif
}
