void setup() {
Serial.begin(9600);
esc.attach(ESC_PIN);
esc2.attach(ESC_PIN2);
esc3.attach(ESC_PIN3);
esc4.attach(ESC_PIN4);
int error;
uint8_t c;
Wire.begin();
error = MPU6050_read(MPU6050_WHO_AM_I, &c, 1);
error = MPU6050_read(MPU6050_PWR_MGMT_1, &c, 1);
MPU6050_write_reg(MPU6050_PWR_MGMT_1, 0);
}
void AccelReading::Initialize(){
int error;
uint8_t c;
//pinMode(led1, OUTPUT);
//pinMode(led2, OUTPUT);
Serial.begin(9600);
Serial.println(F("InvenSense MPU-6050"));
Serial.println(F("June 2012"));
// Initialize the 'Wire' class for the I2C-bus.
Wire.begin();
// default at power-up:
// Gyro at 250 degrees second
// Acceleration at 2g
// Clock source at internal 8MHz
// The device is in sleep mode.
//
error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
Serial.print(F("WHO_AM_I : "));
Serial.print(c,HEX);
Serial.print(F(", error = "));
Serial.println(error,DEC);
// According to the datasheet, the 'sleep' bit
// should read a '1'.
// That bit has to be cleared, since the sensor
// is in sleep mode at power-up.
error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1);
Serial.print(F("PWR_MGMT_1 : "));
Serial.print(c,HEX);
Serial.print(F(", error = "));
Serial.println(error,DEC);
// initialize all the readings to 0:
for (int thisReading = 0; thisReading < numReadings; thisReading++)
{
accelx[thisReading] = 0;
accely[thisReading] = 0;
accelz[thisReading] = 0;
}
// Clear the 'sleep' bit to start the sensor.
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
}
static int read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {
// Read the raw values.
// Read 14 bytes at once,
// containing acceleration, temperature and gyro.
// With the default settings of the MPU-6050,
// there is no filter enabled, and the values
// are not very stable. Returns the error value
accel_t_gyro_union* accel_t_gyro = (accel_t_gyro_union *) accel_t_gyro_ptr;
int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) accel_t_gyro, sizeof(*accel_t_gyro));
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
SWAP ((*accel_t_gyro).reg.x_accel_h, (*accel_t_gyro).reg.x_accel_l);
SWAP ((*accel_t_gyro).reg.y_accel_h, (*accel_t_gyro).reg.y_accel_l);
SWAP ((*accel_t_gyro).reg.z_accel_h, (*accel_t_gyro).reg.z_accel_l);
SWAP ((*accel_t_gyro).reg.t_h, (*accel_t_gyro).reg.t_l);
SWAP ((*accel_t_gyro).reg.x_gyro_h, (*accel_t_gyro).reg.x_gyro_l);
SWAP ((*accel_t_gyro).reg.y_gyro_h, (*accel_t_gyro).reg.y_gyro_l);
SWAP ((*accel_t_gyro).reg.z_gyro_h, (*accel_t_gyro).reg.z_gyro_l);
return error;
}
void findDrift(void){
drift = 0;
int i;
for(i = 0; i < DRIFT_CYCLE_LENGTH; i++){
MPU6050_read(MPU6050_ACCEL_XOUT_H, sizeof(ga_data));
SWAP (ga_data.reg.z_gyro_h, ga_data.reg.z_gyro_l);
drift += (int)(ga_data.value.z_gyro)/1000;
}
}
uint8_t AccelGyro::init() {
Wire.setModule(0);
Wire.begin();
uint8_t c;
uint8_t error;
Serial1.println("Reading who I am bit");
error = MPU6050_read(MPU6050_WHO_AM_I, &c, 1);
Serial1.print("Success. I am : ");
Serial1.println(c, HEX);
// Clear the 'sleep' bit to start the sensor.
MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
Serial1.println("Sensor started!");
return error;
}
static void __attribute__((__interrupt__(TIMER0_A0_VECTOR))) timer0_a0_isr(void)
#endif
{
MPU6050_read(MPU6050_ACCEL_XOUT_H, sizeof(ga_data));
// Reverse byte ordering of accel/gyro data
SWAP (ga_data.reg.x_accel_h, ga_data.reg.x_accel_l);
SWAP (ga_data.reg.y_accel_h, ga_data.reg.y_accel_l);
SWAP (ga_data.reg.z_accel_h, ga_data.reg.z_accel_l);
SWAP (ga_data.reg.x_gyro_h, ga_data.reg.x_gyro_l);
SWAP (ga_data.reg.y_gyro_h, ga_data.reg.y_gyro_l);
SWAP (ga_data.reg.z_gyro_h, ga_data.reg.z_gyro_l);
process_data();
}
void Read_MPU6050AG(MPU6050value_typedef *pAg_value)
{
unsigned char tmp[14]={0};
MPU6050_read(ACCEL_XOUT_H,tmp,14);
pAg_value->AccelX=((tmp[0]<<8)|tmp[1]) - offset_acc_x;
pAg_value->AccelY=((tmp[2]<<8)|tmp[3]) - offset_acc_y;
pAg_value->AccelZ=((tmp[4]<<8)|tmp[5]) - offset_acc_z;
pAg_value->TEMPER=(tmp[6]<<8)|tmp[7];
pAg_value->GYROX=((tmp[8]<<8)|tmp[9]) - offset_gyro_x;
pAg_value->GYROY=((tmp[10]<<8)|tmp[11])- offset_gyro_y;
pAg_value->GYROZ=((tmp[12]<<8)|tmp[13])- offset_gyro_z;
memcpy(&Value_buf,pAg_value,sizeof(MPU6050value_typedef));
}
int AccelGyro::read_gyro_accel_vals(uint8_t* accel_t_gyro_ptr) {
// Read the raw values.
// Read 14 bytes at once,
// containing acceleration, temperature and gyro.
// With the default settings of the MPU-6050,
// there is no filter enabled, and the values
// are not very stable. Returns the error value
// Serial1.print("Size of accel_t_gyto : ");
// Serial1.println(sizeof(accel_t_gyro));
// Serial1.print("Pointer for accel_t_gyro : ");
// Serial1.println((int) &accel_t_gyro, HEX);
// Serial1.print("x_accel_h reg value: ");
// Serial1.println(accel_t_gyro.reg.x_accel_h, DEC);
int error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
// Serial1.print("Error reading: ");
// Serial1.println(error, DEC);
// Serial1.print("x_accel_h reg value: ");
// Serial1.println(accel_t_gyro.reg.x_accel_h, DEC);
// Serial1.print("x_accel_l reg value: ");
// Serial1.println(accel_t_gyro.reg.x_accel_l, DEC);
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
swap_endians((uint8_t*) &accel_t_gyro.reg.x_accel_h, (uint8_t*) &accel_t_gyro.reg.x_accel_l);
swap_endians((uint8_t*) &accel_t_gyro.reg.y_accel_h, (uint8_t*) &accel_t_gyro.reg.y_accel_l);
swap_endians((uint8_t*) &accel_t_gyro.reg.z_accel_h, (uint8_t*) &accel_t_gyro.reg.z_accel_l);
swap_endians((uint8_t*) &accel_t_gyro.reg.t_h, (uint8_t*) &accel_t_gyro.reg.t_l);
swap_endians((uint8_t*) &accel_t_gyro.reg.x_gyro_h, (uint8_t*) &accel_t_gyro.reg.x_gyro_l);
swap_endians((uint8_t*) &accel_t_gyro.reg.y_gyro_h, (uint8_t*) &accel_t_gyro.reg.y_gyro_l);
swap_endians((uint8_t*) &accel_t_gyro.reg.z_gyro_h, (uint8_t*) &accel_t_gyro.reg.z_gyro_l);
// Serial1.println("After swap");
// Serial1.print("x_accel_h reg value: ");
// Serial1.println(accel_t_gyro.reg.x_accel_h, DEC);
// Serial1.print("x_accel_l reg value: ");
// Serial1.println(accel_t_gyro.reg.x_accel_l, DEC);
// Serial1.println("Read successful");
return error;
}
void SimpleMPU6050::initiateRead()
{
// Read all raw values at once (accelerometer, gyroscope, temperature)
MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
}
void AccelReading::Read(){
int error;
double dT;
accel_t_gyro_union accel_t_gyro;
error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
UpdateAverageX(accel_t_gyro.value.x_accel);
UpdateAverageY(accel_t_gyro.value.y_accel);
UpdateAverageZ(accel_t_gyro.value.z_accel);
}
void rd_simpu(void)
{
MPU6050value_typedef Senser_Data={0};
unsigned char tmp[6]={0};
MPU6050_read(ACCEL_XOUT_H,&tmp[0],1);
MPU6050_read(ACCEL_XOUT_L,&tmp[1],1);
MPU6050_read(ACCEL_YOUT_H,&tmp[2],1);
MPU6050_read(ACCEL_YOUT_L,&tmp[3],1);
MPU6050_read(ACCEL_ZOUT_H,&tmp[4],1);
MPU6050_read(ACCEL_ZOUT_L,&tmp[5],1);
Senser_Data.AccelX=(tmp[0]<<8)|tmp[1];
Senser_Data.AccelY=(tmp[2]<<8)|tmp[3];
Senser_Data.AccelZ=(tmp[4]<<8)|tmp[5];
rt_kprintf("Accel_X :%d ,Accel_Y :%d ,Accel_Z :%d\n",Senser_Data.AccelX,Senser_Data.AccelY,Senser_Data.AccelZ);
}
unsigned char getDeviceID(void)
{
unsigned char tmp=0x00;
MPU6050_read(WHO,&tmp,1);
return tmp;
}
void loop() {
int throttleInput = analogRead(7);
//Serial.print(throttleInput);
//Serial.print(", ");
//Serial.println((int) throttle);
long elapsedTime = millis() - lastInput;
//Serial.println(elapsedTime);
if (elapsedTime > 50) {
lastInput = millis();
if (throttleInput < 400 && throttle > 0) {
throttle--;
} else if (throttleInput > 600 && throttle < 179) {
throttle++;
}
}
int error;
double dT;
accel_t_gyro_union accel_t_gyro;
// Read the raw values.
// Read 14 bytes at once,
// containing acceleration, temperature and gyro.
// With the default settings of the MPU-6050,
// there is no filter enabled, and the values
// are not very stable.
error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
//Serial.print(F("Read accel, temp and gyro, error = "));
//Serial.println(error,DEC);
// Swap all high and low bytes.
// After this, the registers values are swapped,
// so the structure name like x_accel_l does no
// longer contain the lower byte.
uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
// Print the raw acceleration values
Serial.print(accel_t_gyro.value.x_accel, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.y_accel, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.z_accel, DEC);
Serial.println(F(""));
int16_t xAccel = accel_t_gyro.value.x_accel;
int16_t yAccel = accel_t_gyro.value.y_accel;
int16_t zAccel = accel_t_gyro.value.z_accel;
int throttle1 = throttle;
int throttle2 = throttle - 18;
int throttle3 = throttle;
int throttle4 = throttle;
if (xAccel < 0) {
throttle3 += 10;
throttle4 += 10;
} else if (xAccel > 0) {
throttle3 -= 10;
throttle4 -= 10;
}
if (yAccel < 0) {
throttle2 += 10;
throttle3 += 10;
} else if (yAccel > 0) {
throttle2 -= 10;
throttle3 -= 10;
}
esc.write(throttle1);
esc2.write(throttle2);
esc3.write(throttle3);
esc4.write(throttle4);
// The temperature sensor is -40 to +85 degrees Celsius.
// It is a signed integer.
// According to the datasheet:
// 340 per degrees Celsius, -512 at 35 degrees.
// At 0 degrees: -512 - (340 * 35) = -12412
/**
Serial.print(F("temperature: "));
dT = ( (double) accel_t_gyro.value.temperature + 12412.0) / 340.0;
Serial.print(dT, 3);
Serial.print(F(" degrees Celsius"));
Serial.println(F(""));
// Print the raw gyro values.
Serial.print(F("gyro x,y,z : "));
Serial.print(accel_t_gyro.value.x_gyro, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.y_gyro, DEC);
Serial.print(F(", "));
Serial.print(accel_t_gyro.value.z_gyro, DEC);
Serial.print(F(", "));
Serial.println(F(""));
**/
}