#include "I2Cdev.h" #include <EEPROM.h> #include "Adafruit_NeoPixel.h" #include "MPU6050_6Axis_MotionApps20.h" /* Change this in MPU6050.h line 66 for correct calibration registers // for 6050 #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS #define MPU6050_RA_XA_OFFS_L_TC 0x07 #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS #define MPU6050_RA_YA_OFFS_L_TC 0x09 #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS #define MPU6050_RA_ZA_OFFS_L_TC 0x0B // for 9250 accelerometer offsets #define MPU6050_RA_XA_OFFS_H 0x77 //[15:0] XA_OFFS #define MPU6050_RA_XA_OFFS_L_TC 0x78 #define MPU6050_RA_YA_OFFS_H 0x7A //[15:0] YA_OFFS #define MPU6050_RA_YA_OFFS_L_TC 0x7B #define MPU6050_RA_ZA_OFFS_H 0x7D //[15:0] ZA_OFFS #define MPU6050_RA_ZA_OFFS_L_TC 0x7E and line 75 in MPU6050.cpp return getDeviceID() == 0x38; */ //#include "MPU6050_9Axis_MotionApps41.h" // not 9250's magnetometer, its a different one //#include "MPU6050.h" // not necessary if using MotionApps include file // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif //#define USE_ULTRASOUND //#define Calibrationmode //#define Mode2 #define Magnetometer const float DEG2RAD = 3.14159265354/180; const float RAD2DEG = 180.0f/3.14159265354f; //#define OUTPUT_READABLE_YAWPITCHROLL // rotation angle of the sensor unsigned long last_read_time; float last_x_angle; // These are the filtered angles float last_y_angle; float last_z_angle; //float last_gyro_x_angle; // Store the gyro angles to compare drift //float last_gyro_y_angle; //float last_gyro_z_angle; MPU6050 mpu; byte state[8]; byte state0 = 0; unsigned long risetime[8]; int speedtime[8]; int fspeedtime[4]; unsigned long currenttime; //int val = 0; unsigned long lastdisplay = 0; const int inputports = 1<<PORTL0 | 1<<PORTL1 | 1<<PORTL2 | 1<<PORTL3; int lastread = 0; unsigned long loopcount = 0; float thr = 0,fb = 0,lr = 0,rr = 0; float prevafb = 0.0,prevalr = 0.0, prevarr = 0.0; float prevrafb = 0.0,prevralr = 0.0,prevrarr = 0.0; float afbav = 0,alrav = 0; float afbavav = 0,alravav = 0; float afb2av = 0,alr2av = 0; //unsigned int pules = 0; int st = 0; const float kp = 13.0,ki = 0.00,kd = 0.0, ka = 0.0, kii = 0.001;//0.1 002 const float kp2 = 0.28,ki2 = 0.002,kd2 = 0.040;//002 const float yawkp = 2.0,yawkd = 1.0; const float cgafb = 0.8,cgalr = 0.4,cgarr = 0.0; float yawcentre = 0.0; unsigned long pulsecount = 0; //unsigned long jitter[8]; /* static float cosar[] = {1,0.999847695156396,0.999390827019115,0.998629534754617,0.997564050259901,0.996194698091866,0.994521895368447,0.992546151641558,0.990268068741878,0.987688340595527, 0.984807753012688,0.981627183448245,0.978147600734496,0.974370064786044,0.970295726276933,0.965925826290142,0.961261695939539,0.95630475596441,0.951056516296692,0.945518575601028, 0.939692620787801,0.933580426499284,0.927183854569067,0.920504853454926,0.913545457645301,0.906307787039573,0.89879404630232,0.891006524191759,0.882947592862563,0.874619707143285, 0.866025403788588,0.857167300706529,0.848048096161117,0.838670567950396,0.829037572560301,0.819152044294545,0.809016994380801,0.798635510053453,0.788010753613194,0.77714596146376, 0.766044443126091,0.754709580230213,0.743144825485168,0.731353701627283,0.719339800347106,0.70710678119535,0.694658370468151,0.681998360072007,0.669130606368726,0.656059029000737, 0.642787609697135,0.629320391060802,0.615661475336994,0.601815023163757,0.587785252304558,0.573576436363509,0.55919290348359,0.544639035028251,0.529919264246811,0.515038074924044, 0.500000000014374,0.484809620261096,0.469471562801034,0.453990499755075,0.43837114680499,0.422618261756996,0.406736643092479,0.390731128506335,0.374606593433353,0.35836794956312, 0.342020143343865,0.325568154475727,0.30901699439389,0.292371704742048,0.275637355836677,0.258819045122561,0.241921895620067,0.22495105436462,0.207911690838865,0.190808995397997, 0.173648177688725,0.156434465062362,0.139173100982528,0.121869343427937,0.104528463290763,0.0871557427710821,0.0697564737678574,0.0523359562669775,0.0348994967268294,0.0174524064618999,0}; */ bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector //////// magnetometer stuff int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output //float magCalibration[3] = { 0, 0, 0} , magbias[3] = { 0, 0, 0}; // Factory mag calibration and mag bias float magXmin = 32768,magXmax = -32768; float magYmin = 32768,magYmax = -32768; float magZmin = 32768,magZmax = -32768; float magcx = 0.0f,magcy = 0.0f,magcz = 0.0f; float magnormal = 1.0f; float magangle = 0.0f; float magcorrection = 0.0f; Adafruit_NeoPixel pixels = Adafruit_NeoPixel(8, 52, NEO_GRB + NEO_KHZ800); void setupmpu() { // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz) #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialize serial communication // (115200 chosen because it is required for Teapot Demo output, but it's // really up to you depending on your project) // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to // the baud timing being too misaligned with processor ticks. You must use // 38400 or slower in these cases, or use some kind of external separate // crystal solution for the UART timer. // initialize device mpu.initialize(); // verify connection mpu.testConnection(); // load and configure the DMP devStatus = mpu.dmpInitialize(); // supply your own gyro offsets here, scaled for min sensitivity /* mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // 1688 factory default for my test chip*/ // 9250 settings // -7482 6212 8157 79 -26 10 mpu.setXAccelOffset(-7600); // -522 -382 -475 -7482 mpu.setYAccelOffset(6212); // 1131 1175 mpu.setZAccelOffset(8157); // 1273 1289 mpu.setXGyroOffset(79); // 44 42 mpu.setYGyroOffset(-26); // -28 -21 mpu.setZGyroOffset(10); // 17 :30: // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready //Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // enable Arduino interrupt detection //Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); //attachInterrupt(0, dmpDataReady, RISING); //mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it //Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) //Serial.print(F("DMP Initialization failed (code ")); //Serial.print(devStatus); //Serial.println(F(")")); } } void readmpu() { if (fifoCount<packetSize) fifoCount = mpu.getFIFOCount(); //Serial1.print(fifoCount);Serial1.print(" "); if (fifoCount<packetSize) return; // 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; if (fifoCount>packetSize)// if 2 packets then read the second one instantly (to ensure we are keeping up) { mpu.getFIFOBytes(fifoBuffer, packetSize); fifoCount -= packetSize; } mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); last_x_angle = ypr[2] * 180/M_PI; last_y_angle = ypr[1] * 180/M_PI; last_z_angle = ypr[0] * 180/M_PI; #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print("quat\t"); Serial.print(q.w); Serial.print("\t"); Serial.print(q.x); Serial.print("\t"); Serial.print(q.y); Serial.print("\t"); Serial.println(q.z); #endif #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); Serial.print("euler\t"); Serial.print(euler[0] * 180/M_PI); Serial.print("\t"); Serial.print(euler[1] * 180/M_PI); Serial.print("\t"); Serial.println(euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); Serial.print("ypr\t"); Serial.print(ypr[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr[1] * 180/M_PI); Serial.print("\t"); Serial.println(ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print("areal\t"); Serial.print(aaReal.x); Serial.print("\t"); Serial.print(aaReal.y); Serial.print("\t"); Serial.println(aaReal.z); #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.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print("aworld\t"); Serial.print(aaWorld.x); Serial.print("\t"); Serial.print(aaWorld.y); Serial.print("\t"); Serial.println(aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: teapotPacket[2] = fifoBuffer[0]; teapotPacket[3] = fifoBuffer[1]; teapotPacket[4] = fifoBuffer[4]; teapotPacket[5] = fifoBuffer[5]; teapotPacket[6] = fifoBuffer[8]; teapotPacket[7] = fifoBuffer[9]; teapotPacket[8] = fifoBuffer[12]; teapotPacket[9] = fifoBuffer[13]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif } #ifdef USE_ULTRASOUND const int us1trig = 30; const int us1echo = 31; unsigned long us1pulsestart = 0; long us1pulse = 0; int us1mode = 0; int tempv = 0; unsigned long df = 0; void SetupUS() { pinMode(us1trig,OUTPUT); pinMode(us1echo,INPUT); digitalWrite(us1trig,LOW);// make sure its initialised low } #endif #ifdef USE_ULTRASOUND boolean fmode = false; int timeoutloop = 0; long pus1pulse = 0; unsigned long pus1pulsetime = 0,ttime = 0; float ustemp = 0; long pdf = 0; long CalcRotation(long pulse) {// using z as forwards, x as left right, y as up/down if ((last_x_angle>89) || (last_x_angle<-89)) return 0;// wont cope if upside-down if ((last_y_angle>89) || (last_y_angle<-89)) return 0;// wont cope if upside-down float ox = 0,oy = pulse,oz = 0; float x,y,z; /* x = ox; y = cos(last_x_angle*DEG2RAD)*oy-sin(last_x_angle*DEG2RAD)*oz; z = sin(last_x_angle*DEG2RAD)*oy+cos(last_x_angle*DEG2RAD)*oz; y = sin(last_y_angle*DEG2RAD)*x+cos(last_y_angle*DEG2RAD)*y; // dont need x or z*/ y = cosar[abs((int)last_x_angle)]*oy; y = cosar[abs((int)last_y_angle)]*y; return y; } void CheckUltraSound(unsigned long mcros) { if (us1mode==0) {// pulse the trigger and go to mode 1 PORTC|=_BV(PORTC7); //digitalWrite(us1trig,HIGH); us1mode = 1; us1pulsestart = mcros; //delayMicroseconds(2); } if (us1mode==1) { // wait 10 micros and pull pin down if (mcros-us1pulsestart>=10) { //digitalWrite(us1trig,LOW); PORTC &= ~_BV(PORTC7); us1pulsestart = mcros; timeoutloop = 0; us1mode = 2; } } if (us1mode==2) {// might need a timeout as it doesnt seem totally reliable timeoutloop++; if (timeoutloop>100) { // micros takes a little while to run so dont do it every time timeoutloop = 0; df = mcros-us1pulsestart; if (df>1000) { us1mode = 0;// restart } } if (us1mode==2)// if not timed out then check the pin { //tempv = digitalRead(us1echo); tempv = PINC & _BV(PORTC6); if (tempv!=0) {// mode 2 - pin up us1pulsestart = mcros; us1mode = 3; } } } if (us1mode==3) { // detect LOW and return to mode 0 (perhaps a delay before doing that) //tempv = digitalRead(us1echo); tempv = PINC & _BV(PORTC6); if (tempv==0) { ttime = mcros; us1pulse = ttime-us1pulsestart; // us1pulse = CalcRotation(us1pulse); if (pus1pulsetime>0) { ustemp = 1000000.0; ustemp = ustemp/(float)(ttime-pus1pulsetime); pdf = (pdf*10+(us1pulse-pus1pulse)*ustemp)/11;// ttime-pus1pulsetime is microseconds since last pulse } if (us1pulse>20000) { // if higher than around 4m then ultrasound wont work so 0 the movement pdf = 0; } pus1pulse = us1pulse; pus1pulsetime = ttime; us1mode = 4; us1pulsestart = mcros+(10000-us1pulse); /* if (fmode) { digitalWrite(13,HIGH); fmode = false; } else { digitalWrite(13,LOW); fmode = true; }*/ } } if (us1mode==4) {// simple timeout if (mcros>us1pulsestart) { us1mode = 0; } } } #endif unsigned long lasteepromupdate = 0; unsigned long lastradiopulse = 0; void setup() { pixels.begin(); digitalWrite(13,LOW); delay(10000); lasteepromupdate = micros(); pinMode(13, OUTPUT); pinMode(6, OUTPUT); pinMode(7, OUTPUT); pinMode(8, OUTPUT); pinMode(9, OUTPUT); pinMode(46, INPUT); // dont really need to explicitly define these as inputs but doing it anyway pinMode(47, INPUT); pinMode(48, INPUT); pinMode(49, INPUT); #ifdef USE_ULTRASOUND SetupUS(); #endif boolean epromvalid = false; /* if ((EEPROM.read(100)==9) && (EEPROM.read(101)==52)) { afbavav = EEPROM.read(102)*256; afbavav += EEPROM.read(103); alravav = EEPROM.read(104)*256; alravav += EEPROM.read(105); } else { EEPROM.write(100,0); EEPROM.write(101,0); EEPROM.write(102,0); EEPROM.write(103,0); EEPROM.write(104,0); EEPROM.write(105,0); EEPROM.write(106,0); EEPROM.write(107,0); }*/ speedtime[0] = 1000; speedtime[1] = 1000; speedtime[2] = 1000; speedtime[3] = 1000; speedtime[4] = 1000; speedtime[5] = 1000; speedtime[6] = 1000; speedtime[7] = 1000; fspeedtime[0] = 1000; fspeedtime[1] = 1000; fspeedtime[2] = 1000; fspeedtime[3] = 1000; /* jitter[0] = 0; jitter[1] = 0; jitter[2] = 0; jitter[3] = 0;*/ #ifndef Calibrationmode Serial1.begin(115200);// remove for calibration Serial.begin(115200); setupmpu(); // for calibration #endif #ifdef Magnetometer initAK8963();//magCalibration); #endif lastradiopulse = micros(); } void ReadAccelerometer() { readmpu(); } float t0d = 0,t1d = 0,t2d = 0,t3d = 0; const int pwmprecision = 20; void sendpulse() { int tmp = fspeedtime[0]/pwmprecision; t0d+=(fspeedtime[0]-(tmp*pwmprecision)); speedtime[0] = tmp*pwmprecision; if (t0d>pwmprecision) { speedtime[0]+=pwmprecision; if (speedtime[0]>2000) speedtime[0] = 2000; t0d-=pwmprecision; } tmp = fspeedtime[1]/pwmprecision; t1d+=(fspeedtime[1]-(tmp*pwmprecision)); speedtime[1] = tmp*pwmprecision; if (t1d>pwmprecision) { speedtime[1]+=pwmprecision; if (speedtime[1]>2000) speedtime[1] = 2000; t1d-=pwmprecision; } tmp = fspeedtime[2]/pwmprecision; t2d+=(fspeedtime[2]-(tmp*pwmprecision)); speedtime[2] = tmp*pwmprecision; if (t2d>pwmprecision) { speedtime[2]+=pwmprecision; if (speedtime[2]>2000) speedtime[2] = 2000; t2d-=pwmprecision; } tmp = fspeedtime[3]/pwmprecision; t3d+=(fspeedtime[3]-(tmp*pwmprecision)); speedtime[3] = tmp*pwmprecision; if (t3d>pwmprecision) { speedtime[3]+=pwmprecision; if (speedtime[3]>2000) speedtime[3] = 2000; t3d-=pwmprecision; } pulsecount++; int count = 0; boolean done0 = false; boolean done1 = false; boolean done2 = false; boolean done3 = false; unsigned long currenttime2 = 0; unsigned long timediff = 0; // unsigned int td = 0; unsigned long risetimel = micros(); PORTH |= _BV(PORTH6) | _BV(PORTH5) | _BV(PORTH4) | _BV(PORTH3); // loop until all done while(count<4) { currenttime2 = micros(); #ifdef USE_ULTRASOUND if (thr>30)// only because the esc's dont initialise correctly with this enabled - only really needed for initialisation CheckUltraSound(currenttime2); #endif timediff = currenttime2-risetimel; if (timediff>=speedtime[2]) // 1000 to 2000 { if (!done2) { // td = timediff-speedtime[2]; // if (td>jitter[2]) // jitter[2] = td; //jitter[2] = (timediff-speedtime[2]); PORTH &= ~_BV(PORTH4); count++; done2 = true; } } if (timediff>=speedtime[3]) // 1000 to 2000 { if (!done3) { // td = timediff-speedtime[3]; // if (td>jitter[3]) // jitter[3] = td; //jitter[3] = (timediff-speedtime[3]); PORTH &= ~_BV(PORTH3); count++; done3 = true; } } if (timediff>=speedtime[0]) // 1000 to 2000 { if (!done0) { // td = timediff-speedtime[0]; // if (td>jitter[0]) // jitter[0] = td; //jitter[0] = (timediff-speedtime[0]); PORTH &= ~_BV(PORTH6); count++; done0 = true; } } if (timediff>=speedtime[1]) // 1000 to 2000 { if (!done1) { // td = timediff-speedtime[1]; // if (td>jitter[1]) // jitter[1] = td; //jitter[1] = (timediff-speedtime[1]); PORTH &= ~_BV(PORTH5); count++; done1 = true; } } } } float afb = 0; float alr = 0; float arr = 0; float athr = 0; float ptafb = 0; float ptalr = 0; float pptafb = 0; float pptalr = 0; unsigned long thrdbg = 0; void SerialWriteInt(int i) { /* byte b1,b2; b1 = i >> 8; b2 = i-(b1<<8); Serial1.write(b1); Serial1.write(b2); */ } void EEPROMWriteInt(int pos,int i) { byte b1,b2; b1 = i >> 8; b2 = i-(b1<<8); EEPROM.write(pos,b1); EEPROM.write(pos+1,b2); } float prevafb2 = 0; float prevalr2 = 0; float prevxangle = 0; float prevyangle = 0; float prevzangle = 0; int stablecount = 0; boolean armed = false; #ifdef Mode2 float cax = 0.0f,cay = 0.0f,caz = 0.0f; long ptime = 0; float thri = 0.0f; #endif int serialout = 0; const int minthrottle = 8; const float acsense = 500; int pixelupdate = 0; int pixelpos = 0; //int pixeldir = 1; void UpdatePIDA() { pixelupdate++; if (pixelupdate==10) { float tmagangle = magangle; if (tmagangle<0) tmagangle = -tmagangle; pixelpos = (int)tmagangle/23; for(int i=0;i<8;i++){ // pixels.Color takes RGB values, from 0,0,0 up to 255,255,255 pixels.setPixelColor(i, pixels.Color(10,10,10)); if (i==pixelpos) { if (magangle<0) { pixels.setPixelColor(i, pixels.Color(0,0,((int)tmagangle))); } else { pixels.setPixelColor(i, pixels.Color(0,((int)tmagangle),0)); } } } pixels.show(); // This sends the updated pixel color to the hardware. pixelupdate = 0; /* pixelpos+=pixeldir; if ((pixelpos==7) || (pixelpos==0)) pixeldir = -pixeldir;*/ } #ifndef Calibrationmode ReadAccelerometer();// remove for calibration serialout++; if (serialout>5) { /* if (readMagData(magCount)==true) { Serial.print(magCount[0]); Serial.print(" "); Serial.print(magCount[1]); Serial.print(" "); Serial.print(magCount[2]); Serial.print(" "); } Serial.print(last_x_angle); Serial.print(" "); Serial.print(last_y_angle); Serial.print(" "); Serial.print(last_z_angle); Serial.println(" ");*/ #ifdef Magnetometer ReadMagnetometer(); #endif serialout = 0; } if (!armed) { float xdiff = abs(last_x_angle-prevxangle); float ydiff = abs(last_y_angle-prevyangle); float zdiff = abs(last_z_angle-prevzangle); prevxangle = last_x_angle; prevyangle = last_y_angle; prevzangle = last_z_angle; float t = xdiff+ydiff+zdiff; if (t<5) { stablecount++; } else { stablecount = 0; } if (stablecount>200) { armed = true; digitalWrite(13,HIGH); } else { return; } } #endif // are the values stable? if not then dont arm and dont send the pulse, could screw up the calibration of the ESC's float lxa = last_x_angle; // IMU values float lya = last_y_angle; float lza = last_z_angle+magcorrection; #ifdef USE_ULTRASOUND CheckUltraSound(micros()); #endif if (thr>=20) { afb = fb/2-lxa;//+6; // actual pitch error alr = lr/2+lya;//+11; // actual roll error arr = rr; // Yaw dont work, might need triming on the radio } else { afb = -lxa;//+6; // actual pitch error alr = +lya;//+11; // actual roll error arr = 0; // Yaw dont work, might need triming on the radio } //if ((afb<0.5) && (afb>-0.5)) afb = 0; //if ((alr<0.5) && (alr>-0.5)) alr = 0; //if ((arr<0.5) && (arr>-0.5)) arr = 0; float rafb = afb-prevafb; // calculate pitch/roll/yar roll rates (difference between last run and current run) float ralr = alr-prevalr; float rarr = arr-prevarr; /*if (rafb>2) { Serial.print(afb,DEC);Serial.print(" ");Serial.println(prevafb,DEC); }*/ //Serial.print(rafb,DEC);Serial.print(" ");Serial.println(ralr,DEC); float aafb = rafb-prevrafb; // calculate pitch/roll instantaneous acceleration float aalr = ralr-prevralr; prevrafb = rafb; prevralr = ralr; prevrarr = rarr; prevafb = afb; prevalr = alr; prevarr = arr; //afb = constrain(afb,-20,20); //alr = constrain(alr,-20,20); if (thr>40) {// dont accumalate i when not flying afbav = afbav+afb*0.02; // create integration of angular error afbav = constrain(afbav,-1000,1000); alrav = alrav+alr*0.02; alrav = constrain(alrav,-1000,1000); if (afb>1) { afbavav += 1; // create integration of angular error } if (afb<-1) { afbavav -= 1; // create integration of angular error } if (alr>1) { alravav += 1; } if (alr<-1) { alravav -= 1; } afbavav = constrain(afbavav,-10000,10000); alravav = constrain(afbavav,-10000,10000); } else if (thr<minthrottle) { afbav = 0; alrav = 0; afbavav = 0; alravav = 0; unsigned long tme = micros(); if (tme-lasteepromupdate>120000)// 2 mins { //EEPROMWriteInt(102,(int)afbavav); //EEPROMWriteInt(104,(int)alravav); } } int kpafb = (int)constrain((kp*afb*100),-5,5); int kdafb = (int)constrain((kd*rafb*50*100),-5,5); int kiafb = (int)(ki*afbav*100); int kpalr = (int)constrain((kp*alr*100),-5,5); int kdalr = (int)constrain((kd*ralr*50*100),-5,5); int kialr = (int)(ki*alrav*100); afb = constrain(kp*afb,-200,200)+ki*afbav+kii*afbavav+constrain(kd*rafb*50,-20,20)-ka*aafb; // calc the pida values alr = constrain(kp*alr,-200,200)+ki*alrav+kii*alravav+constrain(kd*ralr*50,-20,20)-ka*aalr; //afb = 0; //alr = 0; //arr = yawkp*rr+yawkd*rarr*50;// add some p to control it int acx = 0,acy = 0,acz = 0,gyx = 0,gyy = 0,gyz = 0; #ifndef Calibrationmode mpu.getMotion6(&acx, &acy, &acz, &gyx, &gyy, &gyz);// get actual gyro data remove for calibration #endif #ifdef USE_ULTRASOUND CheckUltraSound(micros()); #endif #ifdef Mode2 // Stabilisation mode 2 (not dmp) long ctime= micros(); float gscale = (ctime-ptime); gscale = gscale/1000000.0f; float gscale1 = DEG2RAD*gscale/100.0f; ptime = ctime; // RotateVectorNT(zrot,cax,cay,caz); float tcax = cos(gyz*gscale1)*cax-sin(gyz*gscale1)*cay; float tcay = sin(gyz*gscale1)*cax+cos(gyz*gscale1)*cay; float tcaz = caz; cax = tcax; cay = tcay; caz = tcaz; // RotateVectorNT(yrot,cax,cay,caz); tcax = cos(gyy*gscale1)*cax-sin(gyy*gscale1)*caz; tcay = cay; tcaz = sin(gyy*gscale1)*cax+cos(gyy*gscale1)*caz; cax = tcax; cay = tcay; caz = tcaz; // RotateVectorNT(xrot,cax,cay,caz); tcax = cax; tcay = cos(gyx*gscale1)*cay-sin(gyx*gscale1)*caz; tcaz = sin(gyx*gscale1)*cay+cos(gyx*gscale1)*caz; cax = tcax; cay = tcay; caz = tcaz; cax = cax*(1-gscale)+acx*gscale; cay = cay*(1-gscale)-acy*gscale; caz = caz*(1-gscale)+acz*gscale; float acnormal = cax*cax+cay*cay+caz*caz; acnormal = sqrt(acnormal); float cax2 = cax; cax2 = cax2/acnormal; float cay2 = cay; cay2 = cay2/acnormal; float caz2 = caz; caz2 = caz2/acnormal; // acnormal-16384 = gravity offset if (caz2<0) { // now use cax/cay/caz to adjust afb & alr if (cay2>0) { afb = constrain((caz2-1.0)*-400,-200,200); } else { afb = constrain((caz2-1.0)*400,-200,200); } alr = constrain((cax2)*800,-200,200); }// only use this if upside-down, doesnt seem very stable (perhaps with more work) float facx = acx,facy = acy,facz = acz; acnormal = facx*facx+facy*facy+facz*facz; acnormal = sqrt(acnormal); #endif float fgyx = gyx; float fgyy = gyy; float fgyz = -gyz; fgyx = fgyx/1000.0; fgyy = fgyy/1000.0; fgyz = fgyz/1000.0; arr = yawkp*rr+yawkd*fgyz*50+constrain(yawkp*(lza-yawcentre),-20,20);// add some p to control it yawcentre-=0.00; float reafb = afb-fgyx*50;//gyx//rafb float realr = alr-fgyy*50;//gyy//ralr if (thr>40) { afb2av+=reafb*0.02; alr2av+=realr*0.02; afb2av = constrain(afb2av,-3000,3000); alr2av = constrain(alr2av,-3000,3000); } else if (thr<minthrottle) { afb2av = 0; alr2av = 0; yawcentre = lza; } float rafb2 = (reafb-prevafb2)*50; float ralr2 = (realr-prevalr2)*50; prevafb2 = reafb; prevalr2 = realr; // second PID float afb2 = constrain(kp2*reafb,-15,15)+constrain(ki2*afb2av,-5,5)+kd2*rafb2; float alr2 = constrain(kp2*realr,-15,15)+constrain(ki2*alr2av,-5,5)+kd2*ralr2; // afb = fb; // alr = lr; //afb = constrain(afb,-20,20); // constraints just to make sure, can probably reduce these //alr = constrain(alr,-20,20); afb2 = constrain(afb2,-20,20); alr2 = constrain(alr2,-20,20); arr = constrain(arr,-20,20); //float arr = constrain(rr,-25,25); /* float afbc = cosar[abs((int)afb2)]; float alrc = cosar[abs((int)alr2)]; athr = thr*(2-afbc); athr = athr*(2-alrc);*/ athr = (((thr*1.1)-50)/5); athr = athr*athr*athr/20+50; float ang = abs(lxa)+abs(lya); float adj = 1; if ((ang>0) && (ang<45)) adj = constrain(90/(90-ang),1.0,1.3); athr = athr*adj; #ifdef USE_ULTRASOUND // check us1pulse and use the D to stabalise the height // now use the difference to adjust the throttle (make it small for now and slowely increase it athr = athr-constrain(pdf*0.0005,-6,6); #endif #ifdef Mode2 if ((acnormal<(16384+acsense)) && (acnormal>(16384-acsense))) acnormal = 16384; thri = constrain(thri+(acnormal-16384),-20000,20000)*0.999; // up increases, down decreases // if (thr<minthrottle) // thri = 0.0f; // reset when 0 throttle //Serial1.println(thri); athr = athr-(constrain(thri*0.00005,-10,10));//constrain((acnormal-16384)*0.002,-10,10)+ #endif athr = constrain(athr,0,100); //alr = 0; /* afb2 = 0;// use this code to set centre of gravity alr2 = 0; arr = 0; athr = thr;*/ afb2+=cgafb; alr2+=cgalr; arr+=cgarr; float thrscale = athr/100; fspeedtime[0] = constrain(athr+afb2*thrscale+alr2*thrscale+arr*thrscale,0,100)*10+1000; // front left fspeedtime[1] = constrain(athr+afb2*thrscale-alr2*thrscale-arr*thrscale,0,100)*10+1000; // front right fspeedtime[2] = constrain(athr-afb2*thrscale-alr2*thrscale+arr*thrscale,0,100)*10+1000; // back right fspeedtime[3] = constrain(athr-afb2*thrscale+alr2*thrscale-arr*thrscale,0,100)*10+1000; // back left /* speedtime[0] = constrain(thr,0,90)*10+1000; speedtime[1] = constrain(thr,0,90)*10+1000; speedtime[2] = constrain(thr,0,90)*10+1000; speedtime[3] = constrain(thr,0,90)*10+1000;*/ if (thr<minthrottle) { // if throttle less than 10% then zero the motors fspeedtime[0] = 1000; fspeedtime[1] = 1000; fspeedtime[2] = 1000; fspeedtime[3] = 1000; } #ifdef Calibrationmode if (thr>50) {// for calibration speedtime[0] = 2000; speedtime[1] = 2000; speedtime[2] = 2000; speedtime[3] = 2000; } #endif sendpulse(); // send the pwm to the motors (all at once) //Serial.println(speedtime[1],DEC); //Serial.println(thrdbg,DEC);// 1600 //Serial.println(speedtime[0],DEC);// 1600 //Serial.print(thr);Serial.print(" ");Serial.println(athr); //Serial.print(speedtime[0],DEC);Serial.print(" ");Serial.print(speedtime[1],DEC);Serial.print(" ");Serial.print(speedtime[2],DEC);Serial.print(" ");Serial.println(speedtime[3],DEC); //Serial.print(last_x_angle,DEC);Serial.print(" ");Serial.print(last_y_angle,DEC);Serial.print(" ");Serial.println(last_z_angle,DEC); //Serial.print(afb,DEC);Serial.print(" ");Serial.println(alr,DEC); //Serial.println(rafb,DEC); //Serial.print(rafb,DEC);Serial.print(" ");Serial.println(kd*rafb*50,DEC); //Serial.print(fb,DEC);Serial.print(" ");Serial.print(lr,DEC);Serial.print(" ");Serial.println(rr,DEC); //Serial1.print(last_x_angle,DEC);Serial1.print(" ");Serial.println(afb,DEC); #ifndef Calibrationmode if (thr>=minthrottle) { //SerialWriteInt(1);SerialWriteInt((int)(prevalr*100));SerialWriteInt((int)(alr*100));SerialWriteInt(kpalr);SerialWriteInt(kdalr);SerialWriteInt(kialr); //SerialWriteInt(1);SerialWriteInt((int)(fgyx*100));SerialWriteInt((int)(afb*100));SerialWriteInt(fgyy*100);SerialWriteInt(ralr*100);SerialWriteInt(kialr); //SerialWriteInt(1);SerialWriteInt((int)(fspeedtime[0]));SerialWriteInt((int)(fspeedtime[1]));SerialWriteInt((int)(fspeedtime[2]));SerialWriteInt((int)(fspeedtime[3]));SerialWriteInt((int)(yawcentre*100)); //SerialWriteInt(1);SerialWriteInt((int)(afb*100));SerialWriteInt((int)(fgyx*5000));SerialWriteInt((int)(fb));SerialWriteInt((int)(afb));SerialWriteInt(alr); } #endif } void receivepulse() {// receive a pulse from 4 channels } boolean radio0ok = false; boolean radio1ok = false; boolean radio2ok = false; boolean radio3ok = false; unsigned long timeout = 0; const long maxtimeout = 22000; void loop() { for (int k=0;k<1000;k++) { currenttime = micros(); timeout = currenttime-lastradiopulse; #ifdef USE_ULTRASOUND CheckUltraSound(currenttime); #endif int r = PINL & inputports; if ((r^lastread!=0) || (timeout>maxtimeout)) // radio pin has changed { int val = (PINL & (1<<PORTL3)); if ((state[4]==0) && (timeout<maxtimeout)) { // find up if (val!=0) { risetime[4] = currenttime; state[4] = 1; } } else if ((state[4]==1) || (timeout>maxtimeout)) {// wait for LOW if ((val==0) || (timeout>maxtimeout)) { if (timeout<maxtimeout) { st = currenttime-risetime[4]; if ((st>=900) && (st<=2100)) radio3ok = true; if (st<900) st = 1500; if (st>2100) st = 1500; // or should this be 1000? (must test the responses from the radio with the transmitter turned off) speedtime[4] = (speedtime[4]*5+st)/6; //speedtime[4] = st; state[4] = 0;// not interested in working out the full pulse time (actually it might be useful to determine the RC is connected) rr = speedtime[4]; // check the values we are actually getting from the receiver rr = -(rr-1280)/10; } //if ((rr>=-5) && (rr<=5)) rr = 0; // UpdateTimings(); /* speedtime[0] = constrain(thr+fb+lr+rr,0,100)*10+1000; speedtime[1] = constrain(thr+fb-lr-rr,0,100)*10+1000; speedtime[2] = constrain(thr-fb-lr+rr,0,100)*10+1000; speedtime[3] = constrain(thr-fb+lr-rr,0,100)*10+1000;*/ if (((radio0ok) && (radio1ok) && (radio2ok) && (radio3ok)) || (timeout>maxtimeout)) // timeout is purely for internal testing so remove it once sorted { if (timeout>maxtimeout) { thr = 0; rr = 0; lr = 0; fb = 0; } UpdatePIDA(); } lastradiopulse = currenttime; radio0ok = false; radio1ok = false; radio2ok = false; radio3ok = false; } } val = (PINL & (1<<PORTD2)); if (state[5]==0) { // find up if (val!=0) { risetime[5] = currenttime; state[5] = 1; } } else if (state[5]==1) {// wait for LOW if (val==0) { st = currenttime-risetime[5]; thrdbg = st; if ((st>=900) && (st<=2100)) radio2ok = true; if (st<1000) st = 1000; if (st>2100) st = 1000; // or should this be 1000? (must test the responses from the radio with the transmitter turned off) speedtime[5] = (speedtime[5]*5+st)/6; //if ((st<1100) && (st>1000)) speedtime[5] = 1000;// if throttle set to <1100 and we still have a signal set it to 1000 // speedtime[5] = st; state[5] = 0;// not interested in working out the full pulse time (actually it might be useful to determine the RC is connected) thr = speedtime[5]; if (thr<1150) thr = 1000; thr = constrain((thr-1050)/10,0,100); //UpdateTimings(); /* speedtime[0] = constrain(thr+fb+lr+rr,0,100)*10+1000; speedtime[1] = constrain(thr+fb-lr-rr,0,100)*10+1000; speedtime[2] = constrain(thr-fb-lr+rr,0,100)*10+1000; speedtime[3] = constrain(thr-fb+lr-rr,0,100)*10+1000;*/ } } val = (PINL & (1<<PORTL1)); if (state[6]==0) { // find up if (val!=0) { risetime[6] = currenttime; state[6] = 1; } } else if (state[6]==1) {// wait for LOW if (val==0) { st = currenttime-risetime[6]; if ((st>=900) && (st<=2100)) radio1ok = true; if (st<900) st = 1360; if (st>2100) st = 1360; // or should this be 1000? (must test the responses from the radio with the transmitter turned off) speedtime[6] = (speedtime[6]*14+st)/15; // speedtime[6] = st; state[6] = 0;// not interested in working out the full pulse time (actually it might be useful to determine the RC is connected) fb = speedtime[6]; // check the values we are actually getting from the receiver fb = -(fb-1474)/16;// 1390 //if ((fb>=-7) && (fb<=7)) fb = 0; //UpdateTimings(); /* speedtime[0] = constrain(thr+fb+lr+rr,0,100)*10+1000; speedtime[1] = constrain(thr+fb-lr-rr,0,100)*10+1000; speedtime[2] = constrain(thr-fb-lr+rr,0,100)*10+1000; speedtime[3] = constrain(thr-fb+lr-rr,0,100)*10+1000;*/ } } val = (PINL & (1<<PORTL0)); if (state[7]==0) { // find up if (val!=0) { risetime[7] = currenttime; state[7] = 1; } } else if (state[7]==1) {// wait for LOW if (val==0) { st = currenttime-risetime[7]; if ((st>=900) && (st<=2100)) radio0ok = true; if (st<1000) st = 1600; if (st>2100) st = 1600; speedtime[7] = (speedtime[7]*14+st)/15; // speedtime[7] = st; state[7] = 0;// not interested in working out the full pulse time (actually it might be useful to determine the RC is connected) lr = speedtime[7]; // check the values we are actually getting from the receiver lr = (lr-1465)/16; //if ((lr>=-7) && (lr<=7)) lr = 0; //UpdateTimings(); } } }// end of something changed lastread = r; loopcount++; }// end of k loop if (currenttime-lastdisplay>1000000) // every 30 seconds { //Serial.begin(9600); //Serial.println(loopcount/1,DEC); //Serial.println(pulsecount,DEC); //pulsecount = 0; //Serial.print(last_x_angle,DEC);Serial.print(" ");Serial.println(last_y_angle,DEC); //Serial.println(athr,DEC); //Serial.println(cos(afb*3.14159265354/180),DEC); //Serial.print(lr,DEC);Serial.print(" "); //Serial.println(alr,DEC); //Serial.print(thr,DEC);Serial.print(" "); //Serial.print(fb,DEC);Serial.print(" "); //Serial.println(afb,DEC); //Serial.println(rr,DEC); //Serial.println(speedtime[0],DEC);// 1510 //Serial.println(speedtime[1],DEC);// 1063 //Serial.println(speedtime[2],DEC);// 1384 //Serial.println(speedtime[3],DEC);// 1616 //Serial.println(jitter[0],DEC);// 1510 //Serial.println(jitter[1],DEC);// 1063 //Serial.println(jitter[2],DEC);// 1384 //Serial.println(jitter[3],DEC);// 1616 //Serial.println(speedtime[4],DEC);// 1500 //Serial.println(speedtime[5],DEC);// 1063 //Serial.println(speedtime[6],DEC);// 1390 //Serial.println(speedtime[7],DEC);// 1600 //Serial.println(risetime[4],DEC);// 1510 //Serial.println(risetime[5],DEC);// 1063 //Serial.println(risetime[6],DEC);// 1384 //Serial.println(risetime[7],DEC);// 1616 //unsigned long r = PIND & inputports; //Serial.println(r,DEC); //Serial.println(lastread,DEC); //Serial.println(r^lastread,DEC); //Serial.flush(); //Serial.end(); loopcount = 0; lastdisplay = currenttime; } } // new magnetometer stuff #define AK8963_ADDRESS 0x0C #define WHO_AM_I_AK8963 0x00 // should return 0x48 #define INFO 0x01 #define AK8963_ST1 0x02 // data ready status bit 0 #define AK8963_XOUT_L 0x03 // data #define AK8963_XOUT_H 0x04 #define AK8963_YOUT_L 0x05 #define AK8963_YOUT_H 0x06 #define AK8963_ZOUT_L 0x07 #define AK8963_ZOUT_H 0x08 #define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 #define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 #define AK8963_ASTC 0x0C // Self test control #define AK8963_I2CDIS 0x0F // I2C disable #define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value #define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value #define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value enum Mscale { MFS_14BITS = 0, // 0.6 mG per LSB MFS_16BITS // 0.15 mG per LSB }; uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution uint8_t Mmode = 0x06; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition void initAK8963()//float * destination) { //writeByte(0x68, 0x37, 0x02 | readByte(0x68,0x37)); //writeByte(0x68, 106, 32); mpu.setI2CBypassEnabled(true); delay(10); // First extract the factory calibration for each magnetometer axis // uint8_t rawData[3]; // x/y/z gyro calibration data stored here writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer delay(10); writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode delay(10); readBytesi(AK8963_ADDRESS, AK8963_ASAX, 3, rawData); // Read the x-, y-, and z-axis calibration values //destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. //destination[1] = (float)(rawData[1] - 128)/256. + 1.; //destination[2] = (float)(rawData[2] - 128)/256. + 1.; writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer delay(10); // Configure the magnetometer for continuous read and highest resolution // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates writeByte(AK8963_ADDRESS, AK8963_CNTL, 1 << 4 | Mmode); // Set magnetometer data resolution and sample ODR delay(10); /* mpu.setI2CBypassEnabled(false); writeByte(mpu.devAddr,0x25, 0x80 | AK8963_ADDRESS); //Set i2c address at slave0 at 0x0C writeByte(mpu.devAddr,0x26, AK8963_ST1); //Set where reading at slave 0 starts writeByte(mpu.devAddr,0x27, 0x80 | 7); //set offset at start reading and enable*/ } void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) { I2Cdev::writeByte(address, subAddress, data); /* Wire.beginTransmission(address); // Initialize the Tx buffer Wire.write(subAddress); // Put slave register address in Tx buffer Wire.write(data); // Put data in Tx buffer Wire.endTransmission(); // Send the Tx buffer */ } uint8_t readByte(uint8_t address, uint8_t subAddress) { uint8_t data; // `data` will store the register data uint8_t buffer[1]; uint8_t c = I2Cdev::readByte(address, subAddress, buffer); //Serial.println(c); data = buffer[0]; /* Wire.beginTransmission(address); // Initialize the Tx buffer Wire.write(subAddress); // Put slave register address in Tx buffer Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address data = Wire.read(); // Fill Rx buffer with result*/ return data; // Return data read from slave register } void readBytesi(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) { I2Cdev::readBytes(address, subAddress, count, dest); /* Wire.beginTransmission(address); // Initialize the Tx buffer Wire.write(subAddress); // Put slave register address in Tx buffer Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive uint8_t i = 0; Wire.requestFrom(address, count); // Read bytes from slave register address while (Wire.available()) { dest[i++] = Wire.read(); } // Put read results in the Rx buffer*/ } boolean readMagData(int16_t * destination) { // mpu.setI2CBypassEnabled(true); /* readBytesi(mpu.devAddr, 73, 7, rawData); // Read the six raw data and ST2 registers sequentially into data array uint8_t c = rawData[6]; // End data read by reading ST2 register if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data //destination[0] = rawData[1]; destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; // mpu.setI2CBypassEnabled(false); return true; } return false;*/ uint8_t t = readByte(AK8963_ADDRESS, AK8963_ST1); if(t & 0x01) { // wait for magnetometer data ready bit to be set readBytesi(AK8963_ADDRESS, AK8963_XOUT_L, 7, rawData); // Read the six raw data and ST2 registers sequentially into data array uint8_t c = rawData[6]; // End data read by reading ST2 register if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data //destination[0] = rawData[1]; destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; // mpu.setI2CBypassEnabled(false); return true; } } // mpu.setI2CBypassEnabled(false); return false; //destination[0] = readByte(AK8963_ADDRESS, 0); } void ReadMagnetometer() { magCount[0] = 0; magCount[1] = 0; magCount[2] = 0; if (readMagData(magCount)==true) { /* Serial.print(magCount[0]); Serial.print(" "); Serial.print(magCount[1]); Serial.print(" "); Serial.print(magCount[2]); Serial.println(" ");*/ if (magCount[0]<magXmin) magXmin = magCount[0]; if (magCount[0]>magXmax) magXmax = magCount[0]; if (magCount[1]<magYmin) magYmin = magCount[1]; if (magCount[1]>magYmax) magYmax = magCount[1]; if (magCount[2]<magZmin) magZmin = magCount[2]; if (magCount[2]>magZmax) magZmax = magCount[2]; if ((magXmax-magXmin)>1) { magcx = ((magCount[0]-(magXmax+magXmin)/2))/((magXmax-magXmin)/2); } if ((magYmax-magYmin)>1) { magcy = ((magCount[1]-(magYmax+magYmin)/2)/((magYmax-magYmin)/2)); } if ((magZmax-magZmin)>1) { magcz = ((magCount[2]-(magZmax+magZmin)/2)/((magZmax-magZmin)/2)); // was magYmax-magYmin)/2 } magnormal = magcx*magcx+magcy*magcy+magcz*magcz; magnormal = sqrt(magnormal); if (magnormal<=0.01f) magnormal = 0.01f; magcx = magcx/magnormal; magcy = magcy/magnormal; magcz = magcz/magnormal; //float sx = magcy, sy = magcx, sz = magcz; float tmx = cos(-last_y_angle * DEG2RAD) * magcy - sin(-last_y_angle * DEG2RAD) * magcz;// roll float tmy = magcx; float tmz = sin(-last_y_angle * DEG2RAD) * magcy + cos(-last_y_angle * DEG2RAD) * magcz; magcx = tmx; magcy = cos(-last_x_angle * DEG2RAD) * tmy - sin(-last_x_angle * DEG2RAD) * tmz;// pitch magcz = sin(-last_x_angle * DEG2RAD) * tmy + cos(-last_x_angle * DEG2RAD) * tmz; if (magcy>0) { if ((magcy>-0.01) && (magcy<0.01)) { } else { if (magcx<0) magangle = -atan((-magcx)/magcy)*RAD2DEG; else magangle = atan(magcx/magcy)*RAD2DEG; } magcorrection = -magangle-last_z_angle;// might have to negate magangle } else { if ((magcx>-0.01) && (magcx<0.01)) { } else { if (magcx<0) magangle = -90-atan((-magcy)/(-magcx))*RAD2DEG; else magangle = 90+atan((-magcy)/(magcx))*RAD2DEG; magcorrection = -magangle-last_z_angle;// might have to negate magangle } } // nmx and nmy point to north (magnetic) // work out a yaw correction (assume yaw=0 is north) // as a test lets face it north always, I think nmy=1.0, nmx=0.0 is north //Serial.print(magcx/magnormal); //Serial.print(" "); //Serial.print(magcy/magnormal); //Serial.print(" "); //Serial.print(magcz/magnormal); } } </plaintext>