Revision 27:9e546fa47c33, committed 2012-11-28
- Comitter:
- maetugr
- Date:
- Wed Nov 28 12:29:02 2012 +0000
- Parent:
- 26:96a072233d7a
- Child:
- 28:ba6ca9f4def4
- Commit message:
- after inserting RC-calibration (not tested yet)
Changed in this revision
--- a/IMU/IMU_Filter.cpp Tue Nov 27 19:49:38 2012 +0000
+++ b/IMU/IMU_Filter.cpp Wed Nov 28 12:29:02 2012 +0000
@@ -8,11 +8,18 @@
void IMU_Filter::compute(unsigned long dt, const float * Gyro_data, const int * Acc_data)
{
- get_Acc_angle(Acc_data);
+ // calculate angles for each sensor
for(int i = 0; i < 3; i++)
d_Gyro_angle[i] = Gyro_data[i] *dt/15000000.0;
+ get_Acc_angle(Acc_data);
- // calculate angles for roll, pitch an yaw
+ // Complementary Filter
+ #if 1 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
+ angle[0] = (0.98*(angle[0] + d_Gyro_angle[0]))+(0.02*(Acc_angle[0]));
+ angle[1] = (0.98*(angle[1] + d_Gyro_angle[1]))+(0.02*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
+ angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
+ #endif
+
#if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
@@ -20,22 +27,15 @@
angle[2] = Gyro_angle[2]; // gyro only here
#endif
- #if 1 // neuer Test 1 (Formel von http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
- angle[0] = (0.98*(angle[0]+(Gyro_data[0] *dt/15000000.0)))+(0.02*(Acc_angle[0]));
- angle[1] = (0.98*(angle[1]+(Gyro_data[1] *dt/15000000.0)))+(0.02*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
- //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
- angle[2] += d_Gyro_angle[2]; // gyro only here
- #endif
-
#if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
angle[2] = Gyro_angle[2]; // gyro only here
#endif
- #if 0 // rein Gyro
+ #if 0 // all gyro only
for(int i = 0; i < 3; i++)
- angle[i] = Gyro_angle[i];
+ angle[i] += d_Gyro_angle[i];
#endif
}
--- a/IMU/IMU_Filter.h Tue Nov 27 19:49:38 2012 +0000
+++ b/IMU/IMU_Filter.h Wed Nov 28 12:29:02 2012 +0000
@@ -15,7 +15,6 @@
float angle[3]; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
private:
float d_Gyro_angle[3];
-
void get_Acc_angle(const int * Acc_data);
float Acc_angle[3];
};
--- a/RC/RC_Channel.cpp Tue Nov 27 19:49:38 2012 +0000
+++ b/RC/RC_Channel.cpp Wed Nov 28 12:29:02 2012 +0000
@@ -1,18 +1,22 @@
#include "RC_Channel.h"
#include "mbed.h"
-RC_Channel::RC_Channel(PinName mypin) : myinterrupt(mypin)
+RC_Channel::RC_Channel(PinName mypin, int index) : myinterrupt(mypin)
{
- time = -100; // start value to see if there was any value yet
+ RC_Channel::index = index;
+ time = -101; // start value to see if there was any value yet
+
+ loadCalibrationValue(&scale, "SCALE");
+ loadCalibrationValue(&offset, "OFFSET");
+
myinterrupt.rise(this, &RC_Channel::rise);
myinterrupt.fall(this, &RC_Channel::fall);
timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
-
}
int RC_Channel::read()
{
- return time;
+ return scale * (float)(time) + offset; // calibration of the readings
}
void RC_Channel::rise()
@@ -25,7 +29,7 @@
timer.stop();
int tester = timer.read_us();
if(tester >= 1000 && tester <=2000)
- time = (tester-70)-1000; // TODO: skalierung mit calibrierung (speichern....)
+ time = tester-1000; // we want only the signal from 1000 on
timer.reset();
timer.start();
}
@@ -34,4 +38,28 @@
{
if (timer.read() > 0.3)
time = -100;
+}
+
+void RC_Channel::saveCalibrationValue(float * value, char * fileextension)
+{
+ char path[40];
+ sprintf(path, "/local/FlyBed/RC_%d_%s", index, fileextension);
+ FILE *fp = fopen(path, "w");
+ if (fp != NULL) {
+ fprintf(fp, "%f", value);
+ fclose(fp);
+ } else
+ value = 0;
+}
+
+void RC_Channel::loadCalibrationValue(float * value, char * fileextension)
+{
+ char path[40];
+ sprintf(path, "/local/FlyBed/RC_%d_%s", index, fileextension);
+ FILE *fp = fopen(path, "r");
+ if (fp != NULL) {
+ fscanf(fp, "%f", value);
+ fclose(fp);
+ } else
+ value = 0;
}
\ No newline at end of file
--- a/RC/RC_Channel.h Tue Nov 27 19:49:38 2012 +0000
+++ b/RC/RC_Channel.h Wed Nov 28 12:29:02 2012 +0000
@@ -6,18 +6,26 @@
class RC_Channel
{
public:
- RC_Channel(PinName mypin); // NO p19/p20!!!!, they don't support InterruptIn
+ RC_Channel(PinName mypin, int index); // NO p19/p20!!!!, they don't support InterruptIn
int read(); // read the last measured data
private:
+ int index; // to know which channel of the RC an instance has
+ int time; // last measurement data
+ float scale; // calibration values
+ float offset;
+
InterruptIn myinterrupt; // interrupt on the pin to react when signal falls or rises
void rise(); // start the time measurement when signal rises
void fall(); // stop the time mesurement and save the value when signal falls
Timer timer; // timer to measure the up time of the signal and if the signal timed out
- int time; // last measurement data
Ticker timeoutchecker; // Ticker to see if signal broke down
void timeoutcheck(); // to check for timeout, checked every second
+
+ // Calibration value saving
+ void saveCalibrationValue(float * value, char * fileextension);
+ void loadCalibrationValue(float * value, char * fileextension);
};
#endif
\ No newline at end of file
--- a/main.cpp Tue Nov 27 19:49:38 2012 +0000
+++ b/main.cpp Wed Nov 28 12:29:02 2012 +0000
@@ -34,7 +34,7 @@
ADXL345 Acc(p28, p27);
HMC5883 Comp(p28, p27);
BMP085_old Alt(p28, p27);
-RC_Channel RC[] = {p11, p12, p13, p14}; // no p19/p20 !
+RC_Channel RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,3), RC_Channel(p14,4)}; // no p19/p20 !
Servo_PWM ESC[] = {p21, p22, p23, p24}; // p21 - p26 only because PWM!
IMU_Filter IMU; // don't write () after constructor for no arguments!
Mixer MIX;
Matthias Grob
