Revision 34:3aa1cbcde59d, committed 2013-06-10
- Comitter:
- maetugr
- Date:
- Mon Jun 10 13:22:46 2013 +0000
- Parent:
- 33:fd98776b6cc7
- Child:
- 35:2a9465fedb99
- Commit message:
- First version with new ESCs (they are working with the KK Board); some strange kicks in the behaviour of balancing, next step a logger
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_Filter/IMU_Filter.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,130 @@
+#include "IMU_Filter.h"
+
+// MARG
+#define PI 3.1415926535897932384626433832795
+#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
+#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
+
+IMU_Filter::IMU_Filter()
+{
+ for(int i=0; i<3; i++)
+ angle[i]=0;
+
+ // MARG
+ q0 = 1; q1 = 0; q2 = 0; q3 = 0;
+ exInt = 0; eyInt = 0; ezInt = 0;
+}
+
+void IMU_Filter::compute(float dt, const float * Gyro_data, const float * Acc_data)
+{
+ // calculate angles for each sensor
+ for(int i = 0; i < 3; i++)
+ d_Gyro_angle[i] = Gyro_data[i] *dt;
+ get_Acc_angle(Acc_data);
+
+ // Complementary Filter
+ #if 0 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
+ angle[0] = (0.999*(angle[0] + d_Gyro_angle[0]))+(0.001*(Acc_angle[0]));
+ angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(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
+ //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
+ angle[2] = 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 // all gyro only
+ for(int i = 0; i < 3; i++)
+ angle[i] += d_Gyro_angle[i];
+ #endif
+
+ // MARG
+ #if 1 // (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
+ float radGyro[3];
+
+ for(int i=0; i<3; i++) // Radians per second
+ radGyro[i] = Gyro_data[i] * PI / 180;
+
+ IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
+
+ float rangle[3]; // calculate angles in radians from quternion output
+ rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2)); // from Wiki
+ rangle[1] = asin(2*q0*q2 - 2*q3*q1);
+ rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3));
+
+ for(int i=0; i<3; i++) // angle in degree
+ angle[i] = rangle[i] * 180 / PI;
+ #endif
+}
+
+void IMU_Filter::get_Acc_angle(const float * Acc_data)
+{
+ // calculate the angles for roll and pitch (0,1)
+ float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
+ float temp[3];
+
+ temp[0] = -(Rad2Deg * acos(Acc_data[1] / R)-90);
+ temp[1] = Rad2Deg * acos(Acc_data[0] / R)-90;
+ temp[2] = Rad2Deg * acos(Acc_data[2] / R);
+
+ for(int i = 0;i < 3; i++)
+ if (temp[i] > -360 && temp[i] < 360)
+ Acc_angle[i] = temp[i];
+}
+
+// MARG
+void IMU_Filter::IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az)
+{
+ float norm;
+ float vx, vy, vz;
+ float ex, ey, ez;
+
+ // normalise the measurements
+ norm = sqrt(ax*ax + ay*ay + az*az);
+ if(norm == 0.0f) return;
+ ax = ax / norm;
+ ay = ay / norm;
+ az = az / norm;
+
+ // estimated direction of gravity
+ vx = 2*(q1*q3 - q0*q2);
+ vy = 2*(q0*q1 + q2*q3);
+ vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+
+ // error is sum of cross product between reference direction of field and direction measured by sensor
+ ex = (ay*vz - az*vy);
+ ey = (az*vx - ax*vz);
+ ez = (ax*vy - ay*vx);
+
+ // integral error scaled integral gain
+ exInt = exInt + ex*Ki;
+ eyInt = eyInt + ey*Ki;
+ ezInt = ezInt + ez*Ki;
+
+ // adjusted gyroscope measurements
+ gx = gx + Kp*ex + exInt;
+ gy = gy + Kp*ey + eyInt;
+ gz = gz + Kp*ez + ezInt;
+
+ // integrate quaternion rate and normalise
+ q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
+ q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
+ q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
+ q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
+
+ // normalise quaternion
+ norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+ q0 = q0 / norm;
+ q1 = q1 / norm;
+ q2 = q2 / norm;
+ q3 = q3 / norm;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_Filter/IMU_Filter.h Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,27 @@
+// by MaEtUgR
+
+#ifndef IMU_FILTER_H
+#define IMU_FILTER_H
+
+#include "mbed.h"
+
+#define Rad2Deg 57.295779513082320876798154814105 // factor between radians and degrees of angle (180/Pi)
+
+class IMU_Filter
+{
+ public:
+ IMU_Filter();
+ void compute(float dt, const float * gyro_data, const float * acc_data);
+ float angle[3]; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
+
+ // MARG
+ float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
+ float exInt , eyInt , ezInt; // scaled integral error
+ void IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az);
+ private:
+ float d_Gyro_angle[3];
+ void get_Acc_angle(const float * Acc_data);
+ float Acc_angle[3];
+};
+
+#endif
\ No newline at end of file
--- a/Mixer/Mixer.cpp Thu Apr 04 14:25:21 2013 +0000
+++ b/Mixer/Mixer.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -19,10 +19,10 @@
{ -1, 0, -1}
},
{
- { RT, RT, 1}, // X configuration
- { -RT, RT, -1},
- { -RT, -RT, 1},
- { RT, -RT, -1}
+ { RT, -RT, 1}, // X configuration
+ { RT, RT, -1},
+ { -RT, RT, 1},
+ { -RT, -RT, -1}
}
};
@@ -34,5 +34,5 @@
}
for(int i = 0; i < 4; i++) // make sure no motor stands still
- Motor_speed[i] = Motor_speed[i] > 50 ? Motor_speed[i] : 50;
+ Motor_speed[i] = Motor_speed[i] > 150 ? Motor_speed[i] : 150;
}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PC/PC.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,38 @@
+#include "PC.h"
+#include "mbed.h"
+
+PC::PC(PinName tx, PinName rx, int baudrate) : Serial(tx, rx)
+{
+ baud(baudrate);
+ cls();
+
+ command[0] = '\0';
+ command_char_count = 0;
+}
+
+
+void PC::cls()
+{
+ printf("\x1B[2J");
+}
+
+
+void PC::locate(int Spalte, int Zeile)
+{
+ printf("\x1B[%d;%dH", Zeile + 1, Spalte + 1);
+}
+
+void PC::readcommand(void (*executer)(char*))
+{
+ char input = getc(); // get the character from serial bus
+ if(input == '\r') { // if return was pressed, the command must be executed
+ command[command_char_count] = '\0';
+ executer(&command[0]);
+
+ command_char_count = 0; // reset command
+ command[command_char_count] = '\0';
+ } else if (command_char_count < COMMAND_MAX_LENGHT) {
+ command[command_char_count] = input;
+ command_char_count++;
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PC/PC.h Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,20 @@
+#include "mbed.h"
+
+#ifndef PC_H
+#define PC_H
+
+#define COMMAND_MAX_LENGHT 300
+
+class PC : public Serial
+{
+ public:
+ PC(PinName tx, PinName rx, int baud);
+ void cls(); // to clear the display
+ void locate(int column, int row); // to relocate the cursor
+ void readcommand(void (*executer)(char*)); // to read a char from the pc to the command string
+
+ char command[COMMAND_MAX_LENGHT];
+ private:
+ int command_char_count;
+};
+#endif
--- a/RC/RC_Channel.cpp Thu Apr 04 14:25:21 2013 +0000
+++ b/RC/RC_Channel.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -3,6 +3,7 @@
RC_Channel::RC_Channel(PinName mypin, int index) : myinterrupt(mypin)
{
+ LocalFileSystem local("local"); // If theres no local yet
RC_Channel::index = index;
time = -100; // start value to see if there was any value yet
--- a/RC/RC_Channel.h Thu Apr 04 14:25:21 2013 +0000
+++ b/RC/RC_Channel.h Mon Jun 10 13:22:46 2013 +0000
@@ -10,7 +10,7 @@
int read(); // read the last measured data
private:
- int index; // to know which channel of the RC an instance has
+ int index; // to know which channel of the RC an instance has (only for calibrations savings)
int time; // last measurement data
float scale; // calibration values
float offset;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Alt/BMP085.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,57 @@
+#include "BMP085.h"
+
+BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS)
+{
+ // initialize BMP085 with settings
+ //writeRegister(0xf4, 0x2e); // TODO: was macht das + register in header!
+
+}
+
+/*void BMP085::read()
+ {
+ long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
+ unsigned long B4, B7;
+
+ // TODO: writeRegister(0xf4, 0x2e); ?!!!
+ twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
+ // Wait at least 4.5ms
+ wait(0.005);
+ UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
+
+ X1 = ((UTemp - AC6) * AC5) >> 15;
+ X2 = (MC << 11) / (X1 + MD);
+ B5 = X1 + X2;
+ Temperature = (float)((B5 + 8) >> 4)/10.0;
+
+ twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
+ // Wait at least 4.5ms
+ wait(0.005);
+ UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
+
+ B6 = B5 - 4000;
+ X1 = (B2 * (B6 * B6) >> 12) >> 11;
+ X2 = (AC2 * B6) >> 11;
+ X3 = X1 + X2;
+ B3 = ((AC1 * 4 + X3) << oss) >> 2;
+
+ X1 = (AC3 * B6) >> 13;
+ X2 = (B1 * (B6 * B6) >> 12) >> 16;
+ X3 = ((X1 + X2) + 2) >> 2;
+ B4 = AC4 * (X3 + 32768) >> 15;
+
+ B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
+
+ if (B7 < 0x80000000)
+ {
+ P = (2 * B7) / B4;
+ }
+ else
+ {
+ P = 2* (B7 / B4);
+ }
+ X1 = (P >> 8) * (P >> 8);
+ X1 = (X1 * 3038) >> 16;
+ X2 = (-7357 * P) >> 16;
+ P = P + ((X1 + X2 + 3791) >> 4);
+ Pressure = (float)P / 100.0;
+ }*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Alt/BMP085.h Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,34 @@
+// based on http://mbed.org/users/okini3939/code/BMP085/
+
+#ifndef BMP085_H
+#define BMP085_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define BMP085_I2C_ADDRESS 0xEE
+
+class BMP085 : public I2C_Sensor
+{
+ public:
+ BMP085(PinName sda, PinName scl);
+
+ //virtual void read();
+
+ void calibrate(int s);
+
+ float get_height();
+
+ private:
+ // raw data and function to measure it
+ int raw[3];
+ //void readraw();
+
+ // calibration parameters and their saving
+ int Min[3];
+ int Max[3];
+ float scale[3];
+ float offset[3];
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Comp/HMC5883.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,81 @@
+#include "HMC5883.h"
+
+HMC5883::HMC5883(PinName sda, PinName scl) : I2C_Sensor(sda, scl, HMC5883_I2C_ADDRESS)
+{
+ // load calibration values
+ //loadCalibrationValues(scale, 3, "COMPASS_SCALE.txt");
+ //loadCalibrationValues(offset, 3, "COMPASS_OFFSET.txt");
+
+ // initialize HMC5883
+ writeRegister(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode
+ //writeRegister(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode! (should get constant values from measurement, see datasheet)
+ writeRegister(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
+ writeRegister(HMC5883_MODE_REG, 0x00); // continuous measurement-mode
+}
+
+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)
+{
+ int Min[3]; // values for achieved maximum and minimum amplitude in calibrating environment
+ int Max[3];
+
+ Timer calibrate_timer; // timer to know when calibration is finished
+ calibrate_timer.start();
+
+ while(calibrate_timer.read() < s) // take measurements for s seconds
+ {
+ readraw();
+ for(int i = 0; i < 3; i++) {
+ Min[i] = Min[i] < raw[i] ? Min[i] : raw[i]; // after each measurement check if there's a new minimum or maximum
+ Max[i] = Max[i] > raw[i] ? Max[i] : raw[i];
+ }
+ }
+
+ for(int i = 0; i < 3; i++) {
+ scale[i]= 2000 / (float)(Max[i]-Min[i]); // calculate scale and offset out of the measured maxima and minima
+ offset[i]= 1000 - (float)(Max[i]) * scale[i]; // the lower bound is -1000, the higher one 1000
+ }
+
+ saveCalibrationValues(scale, 3, "COMPASS_SCALE.txt"); // save new scale and offset values to flash
+ saveCalibrationValues(offset, 3, "COMPASS_OFFSET.txt");
+}
+
+void HMC5883::readraw()
+{
+ char buffer[6]; // 8-Bit pieces of axis data
+
+ readMultiRegister(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read axis registers using I2C
+
+ raw[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
+ raw[1] = (short) (buffer[4] << 8 | buffer[5]); // X, Z and Y (yes, order is stupid like this, see datasheet)
+ raw[2] = (short) (buffer[2] << 8 | buffer[3]);
+}
+
+float HMC5883::get_angle()
+{
+ #define RAD2DEG 57.295779513082320876798154814105
+
+ float Heading;
+
+ Heading = RAD2DEG * atan2(data[0],data[1]);
+ Heading += 1.367; // correction of the angle between geographical and magnetical north direction, called declination
+ // if you need an east-declination += DecAngle, if you need west-declination -= DecAngle
+ // for me in Switzerland, Bern it's ca. 1.367 degree east
+ // see: http://magnetic-declination.com/
+ // for me: http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
+ if(Heading < 0)
+ Heading += 360; // minimum 0 degree
+
+ if(Heading > 360)
+ Heading -= 360; // maximum 360 degree
+
+ return Heading;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Comp/HMC5883.h Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,32 @@
+// based on http://mbed.org/users/BlazeX/code/HMC5883/
+
+#ifndef HMC5883_H
+#define HMC5883_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define HMC5883_I2C_ADDRESS 0x3C
+
+#define HMC5883_CONF_REG_A 0x00
+#define HMC5883_CONF_REG_B 0x01
+#define HMC5883_MODE_REG 0x02
+#define HMC5883_DATA_OUT_X_MSB 0x03
+
+class HMC5883 : public I2C_Sensor
+{
+ public:
+ HMC5883(PinName sda, PinName scl);
+
+ virtual void read(); // read all axis from register to array data
+ void calibrate(int s);
+ float get_angle();
+
+ private:
+ virtual void readraw(); // function to get raw data
+
+ float scale[3]; // calibration parameters
+ float offset[3];
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyro/L3G4200D.h Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,56 @@
+// based on http://mbed.org/users/shimniok/code/L3G4200D/
+
+#ifndef L3G4200D_H
+#define L3G4200D_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define L3G4200D_I2C_ADDRESS 0xD0
+
+// register addresses
+#define L3G4200D_WHO_AM_I 0x0F
+
+#define L3G4200D_CTRL_REG1 0x20
+#define L3G4200D_CTRL_REG2 0x21
+#define L3G4200D_CTRL_REG3 0x22
+#define L3G4200D_CTRL_REG4 0x23
+#define L3G4200D_CTRL_REG5 0x24
+#define L3G4200D_REFERENCE 0x25
+#define L3G4200D_OUT_TEMP 0x26
+#define L3G4200D_STATUS_REG 0x27
+
+#define L3G4200D_OUT_X_L 0x28
+#define L3G4200D_OUT_X_H 0x29
+#define L3G4200D_OUT_Y_L 0x2A
+#define L3G4200D_OUT_Y_H 0x2B
+#define L3G4200D_OUT_Z_L 0x2C
+#define L3G4200D_OUT_Z_H 0x2D
+
+#define L3G4200D_FIFO_CTRL_REG 0x2E
+#define L3G4200D_FIFO_SRC_REG 0x2F
+
+#define L3G4200D_INT1_CFG 0x30
+#define L3G4200D_INT1_SRC 0x31
+#define L3G4200D_INT1_THS_XH 0x32
+#define L3G4200D_INT1_THS_XL 0x33
+#define L3G4200D_INT1_THS_YH 0x34
+#define L3G4200D_INT1_THS_YL 0x35
+#define L3G4200D_INT1_THS_ZH 0x36
+#define L3G4200D_INT1_THS_ZL 0x37
+#define L3G4200D_INT1_DURATION 0x38
+
+class L3G4200D : public I2C_Sensor
+{
+ public:
+ L3G4200D(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
+ virtual void read(); // read all axis from register to array data
+ float offset[3]; // offset that's subtracted from every measurement
+ void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
+ int readTemp(); // read temperature from sensor
+
+ private:
+ virtual void readraw();
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/I2C_Sensor.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,54 @@
+#include "I2C_Sensor.h"
+
+// calculate the 8-Bit write/read I2C-Address from the 7-Bit adress of the device
+#define GET_I2C_WRITE_ADDRESS(ADR) (ADR << 1&0xFE) // ADR & 1111 1110
+#define GET_I2C_READ_ADDRESS(ADR) (ADR << 1|0x01) // ADR | 0000 0001
+
+I2C_Sensor::I2C_Sensor(PinName sda, PinName scl, char i2c_address) : i2c(sda, scl), local("local")
+{
+ I2C_Sensor::i2c_address = i2c_address;
+ i2c.frequency(400000); // standard speed
+ //i2c.frequency(1500000); // ultrafast!
+}
+
+void I2C_Sensor::saveCalibrationValues(float values[], int size, char * filename)
+{
+ FILE *fp = fopen(strcat("/local/", filename), "w");
+ for(int i = 0; i < size; i++)
+ fprintf(fp, "%f\r\n", values[i]);
+ fclose(fp);
+}
+
+void I2C_Sensor::loadCalibrationValues(float values[], int size, char * filename)
+{
+ FILE *fp = fopen(strcat("/local/", filename), "r");
+ for(int i = 0; i < size; i++)
+ fscanf(fp, "%f", &values[i]);
+ fclose(fp);
+}
+
+//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// ATTENTION!!! the I2C option "repeated" = true is important because otherwise interrupts while bus communications cause crashes (see http://www.i2c-bus.org/repeated-start-condition/)
+//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+
+char I2C_Sensor::readRegister(char reg)
+{
+ char value = 0;
+
+ i2c.write(i2c_address, ®, 1, true);
+ i2c.read(i2c_address, &value, 1, true);
+
+ return value;
+}
+
+void I2C_Sensor::writeRegister(char reg, char data)
+{
+ char buffer[2] = {reg, data};
+ i2c.write(i2c_address, buffer, 2, true);
+}
+
+void I2C_Sensor::readMultiRegister(char reg, char* output, int size)
+{
+ i2c.write (i2c_address, ®, 1, true); // tell register address of the MSB get the sensor to do slave-transmit subaddress updating.
+ i2c.read (i2c_address, output, size, true); // tell it where to store the data read
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/I2C_Sensor.h Mon Jun 10 13:22:46 2013 +0000
@@ -0,0 +1,38 @@
+// by MaEtUgR
+
+#ifndef I2C_Sensor_H
+#define I2C_Sensor_H
+
+#include "mbed.h"
+
+class I2C_Sensor
+{
+ public:
+ I2C_Sensor(PinName sda, PinName scl, char address);
+
+ float data[3]; // where the measured data is saved
+ virtual void read() = 0; // read all axis from register to array data
+ //TODO: virtual void calibrate() = 0; // calibrate the sensor and if desired write calibration values to a file
+
+ protected:
+ // Calibration
+ void saveCalibrationValues(float values[], int size, char * filename);
+ void loadCalibrationValues(float values[], int size, char * filename);
+
+ // I2C functions
+ char readRegister(char reg);
+ void writeRegister(char reg, char data);
+ void readMultiRegister(char reg, char* output, int size);
+
+ // raw data and function to measure it
+ int raw[3];
+ virtual void readraw() = 0;
+
+ private:
+ I2C i2c; // I2C-Bus
+ char i2c_address; // address
+
+ LocalFileSystem local; // file access to save calibration values
+};
+
+#endif
--- a/main.cpp Thu Apr 04 14:25:21 2013 +0000
+++ b/main.cpp Mon Jun 10 13:22:46 2013 +0000
@@ -16,16 +16,35 @@
#define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for
#define YAWSPEED 2 // maximal speed of yaw rotation in degree per Rate
-float P = 1.1; // PID values
-float I = 0.3;
-float D = 0.8;
+// RC
+#define AILERON 0
+#define ELEVATOR 1
+#define RUDDER 2
+#define THROTTLE 3
+// Axes
+#define ROLL 0
+#define PITCH 1
+#define YAW 2
#define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC
+// Global variables
+bool armed = false; // this variable is for security (when false no motor rotates any more)
+float dt = 0;
+float time_for_dt = 0;
+float dt_read_sensors = 0;
+float time_read_sensors = 0;
+float controller_value[] = {0,0,0}; // The calculated answer form the Controller
+float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
+
+float P = 1.0; // PID values
+float I = 0;
+float D = 0;
+
Timer GlobalTimer; // global time to calculate processing speed
-Ticker Dutycycler; // timecontrolled interrupt to get data form IMU and RC
+Ticker Dutycycler; // timecontrolled interrupt for exact timed control loop
-// initialisation of hardware (see includes for more info)
+// Initialisation of hardware (see includes for more info)
LED LEDs;
#ifdef PC_CONNECTED
//PC pc(USBTX, USBRX, 115200); // USB
@@ -35,22 +54,11 @@
ADXL345 Acc(p28, p27);
HMC5883 Comp(p28, p27);
BMP085_old Alt(p28, p27);
-RC_Channel RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,4), RC_Channel(p14,3)}; // no p19/p20 !
-Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed!
-IMU_Filter IMU; // don't write () after constructor for no arguments!
-Mixer MIX(1); // 1 for X-Formation
-
-// 0:X:Roll 1:Y:Pitch 2:Z:Yaw
-PID Controller[] = {PID(P, I, D, 1000), PID(P, I, D, 1000), PID(0.5, 0.01, 0, 1000)};
-
-// global variables
-bool armed = false; // this variable is for security (when false no motor rotates any more)
-float dt = 0;
-float time_for_dt = 0;
-float dt_read_sensors = 0;
-float time_read_sensors = 0;
-float controller_value[] = {0,0,0}; // The calculated answer form the Controller
-float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
+RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 !
+Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed!
+IMU_Filter IMU; // (don't write () after constructor for no arguments!)
+Mixer MIX(1); // 0 for +-Formation, 1 for X-Formation
+PID Controller[] = {PID(P, I, D, 1000), PID(P, I, D, 1000), PID(0.5, 0, 0, 1000)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
{
@@ -58,9 +66,9 @@
// read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
Gyro.read();
- Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig
- //Comp.read();
- //Alt.Update(); TODO braucht zu lange zum auslesen!
+ Acc.read();
+ //Comp.read(); // TODO: not every loop every sensor? altitude not that important
+ //Alt.Update(); // TODO needs very long to read because of waits
dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time measure for sensors
@@ -71,13 +79,14 @@
IMU.compute(dt, Gyro.data, Acc.data);
// Arming / disarming
- if(RC[3].read() < 20 && RC[2].read() > 850) {
+ if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
armed = true;
}
- if((RC[3].read() < 30 && RC[2].read() < 30) || RC[2].read() < -10 || RC[3].read() < -10 || RC[1].read() < -10 || RC[0].read() < -10) {
+ if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || RC[RUDDER].read() < -10 || RC[THROTTLE].read() < -10 || RC[ELEVATOR].read() < -10 || RC[AILERON].read() < -10) {
armed = false;
}
+ // RC Angle
for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
if (RC_angle[i] < -RC_SENSITIVITY-2)
@@ -85,6 +94,7 @@
}
//RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500; // for yaw angle is integrated
+ // PID controlling
for(int i=0;i<3;i++) {
Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct
@@ -93,15 +103,7 @@
if (armed) // for SECURITY!
{
- // RC controlling
- /*for(int i=0;i<3;i++)
- AnglePosition[i] -= (RC[i].read()-500)*2/500.0;*/
- /*virt_angle[0] = IMU.angle[0] + (RC[0].read()-500)*MAXPITCH/500.0; // TODO: zuerst RC calibration
- virt_angle[1] = IMU.angle[1] + (RC[1].read()-500)*MAXPITCH/500.0;
- yawposition += (RC[3].read()-500)*YAWSPEED/500;
- virt_angle[2] = IMU.angle[2] + yawposition;*/
-
- MIX.compute(RC[3].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
+ MIX.compute(RC[THROTTLE].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
for(int i=0;i<4;i++) // Set new motorspeeds
ESC[i] = (int)MIX.Motor_speed[i];
@@ -145,6 +147,7 @@
if (pc.readable()) // Get Serial input (polled because interrupts disturb I2C)
pc.readcommand(&commandexecuter);
//pc.printf("%f %f %f %f %f %f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); // For live plot in MATLAB of IMU
+ //pc.printf("%f,%f,%f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
#if 1 //pc.cls();
pc.locate(20,0); // PC output
pc.printf("dt:%3.5fs dt_sensors:%3.5fs Altitude:%6.1fm ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
Matthias Grob
