Revision 12:67a06c9b69d5, committed 2012-10-20

Comitter:

maetugr

Date:

Sat Oct 20 17:28:28 2012 +0000

Parent:

11:9bf69bc6df45

Child:

13:4737ee9ebfee

Commit message:

mit unterbrochenem RC signal, kompass fertig mit speicherung!

Changed in this revision

--- a/LED/LED.cpp	Thu Oct 18 20:04:16 2012 +0000
+++ b/LED/LED.cpp	Sat Oct 20 17:28:28 2012 +0000
@@ -40,7 +40,19 @@
 }
 
 void LED::tilt(int index) {
-    Led = Led^(1 << (index-1));
+    Led = Led ^ (1 << (index-1)); //XOR
+}
+
+void LED::set(int index) {
+    Led = Led | (1 << (index-1)); //OR
+}
+
+void LED::reset(int index) {
+    Led = Led & ~(1 << (index-1)); //OR
+}
+
+int LED::check(int index) {
+    return Led & (1 << (index-1));
 }
 
 void LED::operator=(int value) {

--- a/LED/LED.h	Thu Oct 18 20:04:16 2012 +0000
+++ b/LED/LED.h	Sat Oct 20 17:28:28 2012 +0000
@@ -14,6 +14,9 @@
         void roll(int times);
         void rollnext();
         void tilt(int index);
+        void set(int index);
+        void reset(int index);
+        int check(int index);
         void operator=(int value);
     
     private:

--- a/RC/RC_Channel.cpp	Thu Oct 18 20:04:16 2012 +0000
+++ b/RC/RC_Channel.cpp	Sat Oct 20 17:28:28 2012 +0000
@@ -6,7 +6,8 @@
     time = -1; // start value to see if there was any value yet
     myinterrupt.rise(this, &RC_Channel::rise);
     myinterrupt.fall(this, &RC_Channel::fall);
-    timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
+    //timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
+    
 }
 
 int RC_Channel::read()
@@ -17,12 +18,16 @@
 void RC_Channel::rise()
 {
     timer.start();
+    LEDs.set(2);
 }
 
 void RC_Channel::fall()
 {
     timer.stop();
-    time = timer.read_us();
+    LEDs.reset(2);
+    int tester = timer.read_us();
+    if(tester >= 1000 && tester <=2000)
+        time = tester;
     timer.reset();
     timer.start();
 }

--- a/RC/RC_Channel.h	Thu Oct 18 20:04:16 2012 +0000
+++ b/RC/RC_Channel.h	Sat Oct 20 17:28:28 2012 +0000
@@ -2,6 +2,7 @@
 #define RC_CHANNEL_H
 
 #include "mbed.h"
+#include "LED.h"
 
 class RC_Channel
 {
@@ -16,6 +17,8 @@
         Timer timer; // timer to measure the up time of the signal and if the signal timed out
         int time; // last measurement data
         
+        LED LEDs;
+        
         Ticker timeoutchecker; // Ticker to see if signal broke down
         void timeoutcheck(); // to check for timeout, checked every second
 };

--- a/Sensors/Comp/HMC5883.cpp	Thu Oct 18 20:04:16 2012 +0000
+++ b/Sensors/Comp/HMC5883.cpp	Sat Oct 20 17:28:28 2012 +0000
@@ -1,14 +1,23 @@
 #include "mbed.h"
 #include "HMC5883.h"
 
-HMC5883::HMC5883(PinName sda, PinName scl) :  i2c(sda, scl)
-{       
+HMC5883::HMC5883(PinName sda, PinName scl) :  local("local"), i2c(sda, scl)
+{
+    // load calibration values
+    FILE *fp = fopen("/local/compass.txt", "r");
+    for(int i = 0; i < 3; i++)
+        fscanf(fp, "%f", &scale[i]);
+    for(int i = 0; i < 3; i++)
+        fscanf(fp, "%f", &offset[i]);
+    fclose(fp);
+    
     // initialize HMC5883 for scaling
     writeReg(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode for scaling!
     writeReg(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
     writeReg(HMC5883_MODE_REG, 0x00); // continuous measurement-mode
     
-    // Scaling
+    /*
+    // Scaling with testmode (not important, just from data sheet)
     for(int j = 0; j < 3; j++) // set all scales to 1 first so the measurement for scaling is not already scaled
         scale[j] = 1;
     
@@ -22,6 +31,7 @@
     scale[0] = (1.16 * 1090)/(data50[0]/50.0); // value that it should be with selftest of 1.1 Gauss * 1090 LSB/Gauss   /   the value it is
     scale[1] = (1.16 * 1090)/(data50[1]/50.0);
     scale[2] = (1.08 * 1090)/(data50[2]/50.0);
+    */
     
     // set normal mode
     writeReg(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode
@@ -29,23 +39,49 @@
 
 void HMC5883::read()
 {
+    readraw();
+    for(int i = 0; i < 3; i++)
+        data[i] = scale[i] * (float)(raw[i]) + offset[i];
+}
+
+void HMC5883::calibrate(int s)
+{
+    Timer calibrate_timer;
+    calibrate_timer.start();
+    
+    while(calibrate_timer.read() < s)
+    {
+        readraw();
+        for(int i = 0; i < 3; i++) {
+            Min[i]= Min[i] < raw[i] ? Min[i] : raw[i];
+            Max[i]= Max[i] > raw[i] ? Max[i] : raw[i];
+    
+            //Scale und Offset aus gesammelten Min Max Werten berechnen
+            //Die neue Untere und obere Grenze bilden -1 und +1
+            scale[i]= 2000 / (float)(Max[i]-Min[i]);
+            offset[i]= 1000 - (float)(Max[i]) * scale[i];
+        }
+    }
+    
+    // save values
+    FILE *fp = fopen("/local/compass.txt", "w");
+    for(int i = 0; i < 3; i++)
+        fprintf(fp, "%f\r\n", scale[i]);
+    for(int i = 0; i < 3; i++)
+        fprintf(fp, "%f\r\n", offset[i]);
+    fclose(fp);
+}
+
+void HMC5883::readraw()
+{
     char buffer[6];
-    int dataint[3];
     
-    readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6);
+    readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read the raw data from I2C
     
     // join MSB and LSB of X, Z and Y (yes, order is so stupid, see datasheet)
-    dataint[0] = (short) (buffer[0] << 8 | buffer[1]);
-    dataint[1] = (short) (buffer[4] << 8 | buffer[5]);
-    dataint[2] = (short) (buffer[2] << 8 | buffer[3]);
-    
-    for(int j = 0; j < 3; j++) {
-        Min[j]= Min[j] < dataint[j] ? Min[j] : dataint[j];
-        Max[j]= Max[j] > dataint[j] ? Max[j] : dataint[j];
-        data[j] = dataint[j]/1.090; //* scale[j];
-    }
-        
-    heading = 57.295779513082320876798154814105*atan2(data[1], data[0]);
+    raw[0] = (short) (buffer[0] << 8 | buffer[1]);
+    raw[1] = (short) (buffer[4] << 8 | buffer[5]);
+    raw[2] = (short) (buffer[2] << 8 | buffer[3]);
 }
 
 void HMC5883::writeReg(char address, char data){ 
@@ -59,46 +95,27 @@
     i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1); //tell it where to read from
     i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size); //tell it where to store the data read
 }
-/*
-void HMC5883::Calibrate(int s)
+
+float HMC5883::get_angle()
 {
-    
-    //Ende der Kalibrierung in ms Millisekunden berechnen
-    int CalibEnd= GlobalTime.read_ms() + s*1000;
-    
-    while(GlobalTime.read_ms() < CalibEnd)
-    {
-        //Update erledigt alles
-        Update();
-    }
-    
-    AutoCalibration= AutoCalibrationBak;
-}*/
-
-// Winkel berechnen
-//---------------------------------------------------------------------------------------------------------------------------------------
-float HMC5883::getAngle(float x, float y)
-    {
     #define Rad2Deg     57.295779513082320876798154814105
-    #define PI          3.1415926535897932384626433832795
 
     float Heading;
-    float DecAngle; 
+    
+    Heading = Rad2Deg * atan2(data[0],data[1]);
     
-    DecAngle = 1.367 / Rad2Deg;         //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
+    Heading += 1.367;                   //bei Ost-Deklination  += DecAngle, bei West-Deklination -= DecAngle
+                                        //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
                                         //Bern ca. 1.367 Grad Ost
                                         //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
-    Heading = atan2((float)y,(float)x);
-    
-    Heading += DecAngle;                //bei Ost-Deklination  += DecAngle, bei West-Deklination -= DecAngle
  
     if(Heading < 0)  
-        Heading += 2*PI;                //korrigieren bei negativem Vorzeichen
+        Heading += 360;                 // minimum 0 degree
         
-    if(Heading > 2*PI)   
-        Heading -= 2*PI;                 //auf 2Pi begrenzen
+    if(Heading > 360)   
+        Heading -= 360;                 // maximum 360 degree
         
-    return  (Heading * 180/PI);         //Radianten in Grad konvertieren
-    }
+    return Heading;
+}
     
 

--- a/Sensors/Comp/HMC5883.h	Thu Oct 18 20:04:16 2012 +0000
+++ b/Sensors/Comp/HMC5883.h	Sat Oct 20 17:28:28 2012 +0000
@@ -20,21 +20,27 @@
         float data[3];
         
         void read();
+        
+        void calibrate(int s);
+        int Min[3];
+        int Max[3];
+        float scale[3];
+        float offset[3];
+        LocalFileSystem local;
+        
+        float get_angle();
+         
+    private:
+        I2C i2c;
+        
+        // raw data and function to measure it
+        int raw[3];
+        void readraw();
+        
+        // I2C functions
         void writeReg(char address, char data);
         void readMultiReg(char address, char* output, int size);
         
-        float scale[3]; //privatisieren
-        
-        float heading;
-        
-        int Min[3];
-        int Max[3];
-        
-        float   getAngle(float x, float y); //von Götti
-        
-    private:
-        I2C i2c;
-        
 };
 
 #endif

--- a/main.cpp	Thu Oct 18 20:04:16 2012 +0000
+++ b/main.cpp	Sat Oct 20 17:28:28 2012 +0000
@@ -13,6 +13,8 @@
 #define Rad2Deg         57.295779513082320876798154814105
 #define RATE            0.02 // speed of Ticker/PID
 
+//#define COMPASSCALIBRATE
+
 Timer GlobalTimer; // global time to calculate processing speed
 Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
 PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
@@ -29,10 +31,11 @@
 Servo       Motor[] = {(p15), (p16), (p17), (p18)};
 
 // variables for loop
-unsigned long   dt = 0;
-unsigned long   time_loop = 0;
+unsigned long   dt_get_data = 0;
+unsigned long   time_get_data = 0;
+unsigned long   dt_read_sensors = 0;
+unsigned long   time_read_sensors = 0;
 float           angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw
-float           Comp_angle = 0;
 float           tempangle = 0;
 int             Motorvalue[3];
 
@@ -40,74 +43,92 @@
 
 void get_Data()
 {
+    time_read_sensors = GlobalTimer.read_us();
+    
     // read data from sensors
     Gyro.read();
     Acc.read();
     Comp.read();
-    Alt.Update();
-
-    //calculate angle for yaw from compass
-    //Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
+    //Alt.Update();
+    
+    dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
     
     // meassure dt
-    dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
-    time_loop = GlobalTimer.read_us();      // set new time for next measurement
+    dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop
+    time_get_data = GlobalTimer.read_us();      // set new time for next measurement
     
     // calculate angles for roll, pitch an yaw
-    angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt/15000000.0;
-    angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt/15000000.0;// TODO Offset accelerometer einstellen
-    tempangle += (Comp_angle - tempangle)/50 - Gyro.data[2] *dt/15000000.0;
-    angle[2] += Gyro.data[2] *dt/15000000.0; // gyro only here
+    angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0;
+    angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen
+    tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
+    angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here
+    
+    // TODO Read RC data
     
-    // Read RC data
-    controller.setProcessValue(RC[3 -1].read());
-    for (int j = 0; j < 4; j++)
-        Motorvalue[j] = controller.compute(); // throttle
-    
-    for (int j = 0; j < 4; j++)
-        Motor[j] = 1000 + 5*abs(angle[1]);//Motorvalue[j]; // set new motorspeeds
-    pid.setProcessValue(angle[0]);
-    pidtester = pid.compute();
+    // calculate new motorspeeds
+    /*
+    Motor[0] = 1000 + (100 + (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
+    Motor[1] = 1000 + (100 + (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
+    Motor[2] = 1000 + (100 - (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
+    Motor[3] = 1000 + (100 - (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
+    */
 }
 
 
 int main() {
+    //NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
+    
+    #ifdef COMPASSCALIBRATE
+        pc.locate(10,5);
+        pc.printf("CALIBRATING");
+        Comp.calibrate(60);
+    #endif
+    
     // init screen
     pc.locate(10,5);
     pc.printf("Flybed v0.2");
     LEDs.roll(2);
     
-    controller.setInputLimits(1000.0, 2000.0);
-    controller.setOutputLimits(1000.0, 2000.0);
-    controller.setMode(AUTO_MODE);
-    
-    pid.setInputLimits(-90.0, 90.0);
-    pid.setOutputLimits(-90.0, 90.0);
-    pid.setMode(AUTO_MODE);
-    pid.setSetPoint(0.0);
-    
     // Start!
     GlobalTimer.start();
-    Datagetter.attach(&get_Data, RATE);     // start to get data all 10ms
-    while(1) {
+    Datagetter.attach(&get_Data, RATE);     // start to get data all RATEms
+    
+    while(1) { 
         pc.locate(10,5); // PC output
-        pc.printf("dt:%dms  %6.1fm   ", dt/1000, Alt.CalcAltitude(Alt.Pressure));
+        pc.printf("dt:%dms   dt_sensors:%dus    Altitude:%6.1fm   ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
         pc.locate(10,8);
         pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", angle[0], angle[1], angle[2]);
         pc.locate(10,9);
         pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f    ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
         pc.locate(10,10);
-        pc.printf("Debug_Yaw:  Comp:%6.1f tempangle:%6.1f  ", Comp_angle, tempangle);
+        pc.printf("Debug_Yaw:  Comp:%6.1f tempangle:%6.1f  ", Comp.get_angle(), tempangle);
         pc.locate(10,12);
-        pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.heading);
+        pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());
         pc.locate(10,13);
         pc.printf("Comp_scale: %6.4f %6.4f %6.4f   ", Comp.scale[0], Comp.scale[1], Comp.scale[2]);
         pc.locate(10,15);
         pc.printf("pidtester: %6.1f   RC: %d %d %d %d     ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
-        pc.locate(10,18);
-        pc.printf("Max: %d %d %d     ", Comp.Max[0], Comp.Max[1], Comp.Max[2]);
+        
         pc.locate(10,19);
-        pc.printf("Min: %d %d %d     ", Comp.Min[0], Comp.Min[1], Comp.Min[2]);
+        pc.printf("RC0: %d :[", RC[0].read());
+        for (int i = 0; i < (RC[0].read() - 1000)/17; i++)
+            pc.printf("=");
+        pc.printf("                                                       ");
+        pc.locate(10,20);
+        pc.printf("RC1: %d :[", RC[1].read());
+        for (int i = 0; i < (RC[1].read() - 1000)/17; i++)
+            pc.printf("=");
+        pc.printf("                                                       ");
+        pc.locate(10,21);
+        pc.printf("RC2: %d :[", RC[2].read());
+        for (int i = 0; i < (RC[2].read() - 1000)/17; i++)
+            pc.printf("=");
+        pc.printf("                                                       ");
+        pc.locate(10,22);
+        pc.printf("RC3: %d :[", RC[3].read());
+        for (int i = 0; i < (RC[3].read() - 1000)/17; i++)
+            pc.printf("=");
+        pc.printf("                                                       ");
         LEDs.rollnext();
     }
 }