Revision 33:fd98776b6cc7, committed 2013-04-04
- Comitter:
- maetugr
- Date:
- Thu Apr 04 14:25:21 2013 +0000
- Parent:
- 32:e2e02338805e
- Child:
- 34:3aa1cbcde59d
- Commit message:
- New version developed in eastern holidays, ported Madgwick Filter, added support for chaning PID values while flying over bluetooth, still not flying stable or even controllable
Changed in this revision
--- a/IMU_Filter/IMU_Filter.cpp Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-#include "IMU_Filter.h"
-
-IMU_Filter::IMU_Filter()
-{
- for(int i=0; i<3; i++)
- angle[i]=0;
-}
-
-void IMU_Filter::compute(unsigned long dt, const float * Gyro_data, const int * 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);
-
- // Complementary Filter
- #if 1 // (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
-
-
-}
-
-void IMU_Filter::get_Acc_angle(const int * 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((float)Acc_data[1] / R)-90);
- temp[1] = Rad2Deg * acos((float)Acc_data[0] / R)-90;
- temp[2] = Rad2Deg * acos((float)Acc_data[2] / R);
-
- for(int i = 0;i < 3; i++)
- if (temp[i] > -360 && temp[i] < 360)
- Acc_angle[i] = temp[i];
-}
\ No newline at end of file
--- a/IMU_Filter/IMU_Filter.h Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-// 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(unsigned long dt, const float * gyro_data, const int * acc_data);
- 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];
-};
-
-#endif
\ No newline at end of file
--- a/Mixer/Mixer.cpp Tue Apr 02 16:57:56 2013 +0000
+++ b/Mixer/Mixer.cpp Thu Apr 04 14:25:21 2013 +0000
@@ -8,7 +8,7 @@
Motor_speed[i]=0;
}
-void Mixer::compute(unsigned long dt, int Throttle, const float * controller_value)
+void Mixer::compute(int Throttle, const float * controller_value)
{
// Mixing tables for each configuration
float mix_table[2][4][3] = {
--- a/Mixer/Mixer.h Tue Apr 02 16:57:56 2013 +0000
+++ b/Mixer/Mixer.h Thu Apr 04 14:25:21 2013 +0000
@@ -11,7 +11,7 @@
{
public:
Mixer(int Configuration);
- void compute(unsigned long dt, int Throttle, const float * controller_value);
+ void compute(int Throttle, const float * controller_value);
float Motor_speed[4]; // calculated motor speeds to send to the ESCs
private:
int Configuration; // number of the configuration used (for example +)
--- a/PC/PC.cpp Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-#include "PC.h"
-#include "mbed.h"
-
-PC::PC(PinName tx, PinName rx, int baudrate) : Serial(tx, rx)
-{
- baud(baudrate);
- cls();
-}
-
-
-void PC::cls()
-{
- printf("\x1B[2J");
-}
-
-
-void PC::locate(int Spalte, int Zeile)
-{
- printf("\x1B[%d;%dH", Zeile + 1, Spalte + 1);
-}
--- a/PC/PC.h Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#include "mbed.h"
-
-#ifndef PC_H
-#define PC_H
-
-class PC : public Serial
-{
- public:
- PC(PinName tx, PinName rx, int baud);
- void cls();
- void locate(int column, int row);
-};
-#endif
--- a/PID/PID.cpp Tue Apr 02 16:57:56 2013 +0000
+++ b/PID/PID.cpp Thu Apr 04 14:25:21 2013 +0000
@@ -39,6 +39,13 @@
return Result;
}
+void PID::setPID(float P, float I, float D)
+{
+ PID::P = P;
+ PID::I = I;
+ PID::D = D;
+}
+
void PID::setIntegrate(bool Integrate)
{
PID::Integrate = Integrate;
--- a/PID/PID.h Tue Apr 02 16:57:56 2013 +0000
+++ b/PID/PID.h Thu Apr 04 14:25:21 2013 +0000
@@ -10,6 +10,7 @@
PID(float P, float I, float D, float Integral_Max);
float compute(float SetPoint, float ProcessValue);
void setIntegrate(bool Integrate);
+ void setPID(float P, float I, float D);
private:
float P,I,D; // PID Values
--- a/Sensors/Acc/ADXL345.cpp Tue Apr 02 16:57:56 2013 +0000
+++ b/Sensors/Acc/ADXL345.cpp Thu Apr 04 14:25:21 2013 +0000
@@ -1,82 +1,44 @@
#include "ADXL345.h"
-ADXL345::ADXL345(PinName sda, PinName scl) : i2c(sda, scl) {
- //400kHz, allowing us to use the fastest data rates.
- //there are other chips on board, sorry
- i2c.frequency(400000);
- // initialize the BW data rate
- char tx[2];
- tx[0] = ADXL345_BW_RATE_REG;
- tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
- i2c.write( ADXL345_WRITE , tx, 2);
- //Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
-
- char rx[2];
- rx[0] = ADXL345_DATA_FORMAT_REG;
- rx[1] = 0x0B;
- // full res and +_16g
- i2c.write( ADXL345_WRITE , rx, 2);
-
+ADXL345::ADXL345(PinName sda, PinName scl) : I2C_Sensor(sda, scl, ADXL345_I2C_ADDRESS)
+{
// Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
- char x[2];
- x[0] = ADXL345_OFSX_REG ;
- //x[1] = 0xFD;
- x[1] = 0x00;
- i2c.write( ADXL345_WRITE , x, 2);
- char y[2];
- y[0] = ADXL345_OFSY_REG ;
- //y[1] = 0x03;
- y[1] = 0x00;
- i2c.write( ADXL345_WRITE , y, 2);
- char z[2];
- z[0] = ADXL345_OFSZ_REG ;
- //z[1] = 0xFE;
- z[1] = 0x00;
- i2c.write( ADXL345_WRITE , z, 2);
+ writeRegister(ADXL345_OFSX_REG, 0xFA); // to get these offsets just lie your sensor down on the table always the axis pointing down to earth has 200+ and the others should have more or less 0
+ writeRegister(ADXL345_OFSY_REG, 0xFE);
+ writeRegister(ADXL345_OFSZ_REG, 0x0A);
- // MY INITIALISATION -------------------------------------------------------
-
- writeReg(ADXL345_POWER_CTL_REG, 0x00); // set power control
- writeReg(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format
- setDataRate(ADXL345_3200HZ); // set data rate
- writeReg(ADXL345_POWER_CTL_REG, 0x08); // set mode
+ writeRegister(ADXL345_BW_RATE_REG, 0x0F); // 3200Hz BW-Rate
+ writeRegister(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format to full resolution and +-16g
+ writeRegister(ADXL345_POWER_CTL_REG, 0x08); // set mode
}
void ADXL345::read(){
- char buffer[6];
- readMultiReg(ADXL345_DATAX0_REG, buffer, 6);
-
- data[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
- data[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
- data[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
+ readraw();
+ for (int i = 0; i < 3; i++)
+ data[i] = raw[i] - offset[i]; // TODO: didnt care about units
}
-void ADXL345::writeReg(char address, char data){
- char tx[2];
- tx[0] = address;
- tx[1] = data;
- i2c.write(ADXL345_WRITE, tx, 2);
+void ADXL345::readraw(){
+ char buffer[6];
+ readMultiRegister(ADXL345_DATAX0_REG, buffer, 6);
+
+ raw[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
+ raw[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
+ raw[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
}
-char ADXL345::readReg(char address){
- char tx = address;
- char output;
- i2c.write( ADXL345_WRITE , &tx, 1); //tell it what you want to read
- i2c.read( ADXL345_READ , &output, 1); //tell it where to store the data
- return output;
-}
-
-void ADXL345::readMultiReg(char address, char* output, int size) {
- i2c.write(ADXL345_WRITE, &address, 1, true); //tell it where to read from
- i2c.read(ADXL345_READ , output, size, true); //tell it where to store the data read
-}
-
-void ADXL345::setDataRate(char rate) {
- //Get the current register contents, so we don't clobber the power bit.
- char registerContents = readReg(ADXL345_BW_RATE_REG);
-
- registerContents &= 0x10;
- registerContents |= rate;
-
- writeReg(ADXL345_BW_RATE_REG, registerContents);
+void ADXL345::calibrate(int times, float separation_time)
+{
+ // calibrate sensor with an average of count samples (result of calibration stored in offset[])
+ float calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement
+
+ for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
+ readraw();
+ for (int j = 0; j < 3; j++)
+ calib[j] += raw[j];
+ wait(separation_time);
+ }
+
+ for (int i = 0; i < 2; i++)
+ offset[i] = calib[i]/times; // take the average of the calibration measurements
}
\ No newline at end of file
--- a/Sensors/Acc/ADXL345.h Tue Apr 02 16:57:56 2013 +0000
+++ b/Sensors/Acc/ADXL345.h Thu Apr 04 14:25:21 2013 +0000
@@ -4,6 +4,12 @@
#define ADXL345_H
#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define ADXL345_I2C_ADDRESS 0xA6
+//the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
+//when ALT ADDRESS pin is high:
+//#define ADXL345_I2C_ADDRESS 0x3A
// register addresses
#define ADXL345_DEVID_REG 0x00
@@ -37,48 +43,23 @@
#define ADXL345_FIFO_CTL 0x38
#define ADXL345_FIFO_STATUS 0x39
-// data rate codes
-#define ADXL345_3200HZ 0x0F
-#define ADXL345_1600HZ 0x0E
-#define ADXL345_800HZ 0x0D
-#define ADXL345_400HZ 0x0C
-#define ADXL345_200HZ 0x0B
-#define ADXL345_100HZ 0x0A
-#define ADXL345_50HZ 0x09
-#define ADXL345_25HZ 0x08
-#define ADXL345_12HZ5 0x07
-#define ADXL345_6HZ25 0x06
-
-// read or write bytes
-#define ADXL345_READ 0xA7
-#define ADXL345_WRITE 0xA6
-#define ADXL345_ADDRESS 0x53
-
-//the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
-//when ALT ADDRESS pin is high:
-//#define ADXL345_READ 0x3B
-//#define ADXL345_WRITE 0x3A
-//#define ADXL345_ADDRESS 0x1D
-
#define ADXL345_X 0x00
#define ADXL345_Y 0x01
#define ADXL345_Z 0x02
typedef char byte;
-class ADXL345
+class ADXL345 : public I2C_Sensor
{
public:
- ADXL345(PinName sda, PinName scl); // constructor, uses i2c
- void read(); // read all axis to array
- int data[3]; // where the measured data is saved
+ ADXL345(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
+ virtual void read(); // read all axis to array
+
+ 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)
private:
- I2C i2c; // i2c object to communicate
- void writeReg(byte reg, byte value); // write one single register to sensor
- byte readReg(byte reg); // read one single register from sensor
- void readMultiReg(char startAddress, char* ptr_output, int size); // read multiple regs
- void setDataRate(char rate); // data rate configuration (not only a reg to write)
+ virtual void readraw();
};
#endif
--- a/Sensors/Alt/BMP085.cpp Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-#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
--- a/Sensors/Alt/BMP085.h Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-// 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
--- a/Sensors/Comp/HMC5883.cpp Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-#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;
-}
-
-
--- a/Sensors/Comp/HMC5883.h Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,32 +0,0 @@
-// 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
--- a/Sensors/Gyro/L3G4200D.cpp Tue Apr 02 16:57:56 2013 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp Thu Apr 04 14:25:21 2013 +0000
@@ -27,7 +27,7 @@
writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement
- calibrate();
+ //calibrate(50, 0.01);
}
void L3G4200D::read()
@@ -35,7 +35,7 @@
readraw(); // read raw measurement data
for (int i = 0; i < 3; i++)
- data[i] = raw[i] - offset[i]; // subtract offset from calibration
+ data[i] = (raw[i] - offset[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10)
}
int L3G4200D::readTemp()
@@ -47,26 +47,25 @@
{
char buffer[6]; // 8-Bit pieces of axis data
- readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: wiiiiiiso?! | (1 << 7)
+ readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
raw[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers
raw[1] = (short) (buffer[3] << 8 | buffer[2]);
raw[2] = (short) (buffer[5] << 8 | buffer[4]);
}
-void L3G4200D::calibrate()
+void L3G4200D::calibrate(int times, float separation_time)
{
- // calibrate gyro with an average of count samples (result of calibration stored in offset[])
- float Gyro_calib[3] = {0,0,0}; // temporary var for the sum of calibration measurement
+ // calibrate sensor with an average of count samples (result of calibration stored in offset[])
+ float calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement
- const int count = 50;
- for (int i = 0; i < count; i++) { // read 50 times the data in a very short time
+ for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
readraw();
for (int j = 0; j < 3; j++)
- Gyro_calib[j] += raw[j];
- wait(0.001); // TODO: maybe less or no wait !!
+ calib[j] += raw[j];
+ wait(separation_time);
}
for (int i = 0; i < 3; i++)
- offset[i] = Gyro_calib[i]/count; // take the average of the calibration measurements
+ offset[i] = calib[i]/times; // take the average of the calibration measurements
}
\ No newline at end of file
--- a/Sensors/Gyro/L3G4200D.h Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-// 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
- void calibrate(); // calibrate the gyro (get an offset while not moving)
- int readTemp(); // read temperature from sensor
-
- private:
- float offset[3]; // offset that's subtracted from every measurement
- virtual void readraw();
-};
-
-#endif
\ No newline at end of file
--- a/Sensors/I2C_Sensor.cpp Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,54 +0,0 @@
-#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
--- a/Sensors/I2C_Sensor.h Tue Apr 02 16:57:56 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,38 +0,0 @@
-// 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 value saving
- 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 Tue Apr 02 16:57:56 2013 +0000
+++ b/main.cpp Thu Apr 04 14:25:21 2013 +0000
@@ -13,15 +13,13 @@
#define RATE 0.002 // speed of the interrupt for Sensors and PID
#define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
-#define MAXPITCH 40 // maximal angle from horizontal that the PID is aming for
-#define RC_SENSITIVITY 30
+#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.5; // PID values
-float I = 0;
-float D = 1;
+float P = 1.1; // PID values
+float I = 0.3;
+float D = 0.8;
-//#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
#define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC
Timer GlobalTimer; // global time to calculate processing speed
@@ -31,10 +29,8 @@
LED LEDs;
#ifdef PC_CONNECTED
//PC pc(USBTX, USBRX, 115200); // USB
- PC pc(p9, p10, 115200); // Bluetooth
+ PC pc(p9, p10, 115200); // Bluetooth
#endif
-LocalFileSystem local("local"); // Create the local filesystem under the name "local"
-//FILE *Logger;
L3G4200D Gyro(p28, p27);
ADXL345 Acc(p28, p27);
HMC5883 Comp(p28, p27);
@@ -42,25 +38,23 @@
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);
+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.2, 0, 0.1, 1000)};
+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)
-unsigned long dt = 0;
-unsigned long time_for_dt = 0;
-unsigned long dt_read_sensors = 0;
-unsigned long time_read_sensors = 0;
-float tempangle = 0; // temporärer winkel für yaw mit kompass
-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
-char command[300]; //= {'\0'};
+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
void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
{
- time_read_sensors = GlobalTimer.read_us();
+ time_read_sensors = GlobalTimer.read(); // start time measure for sensors
// read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
Gyro.read();
@@ -68,35 +62,28 @@
//Comp.read();
//Alt.Update(); TODO braucht zu lange zum auslesen!
- dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
+ dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time measure for sensors
- // meassure dt
- dt = GlobalTimer.read_us() - time_for_dt; // time in us since last loop
- time_for_dt = GlobalTimer.read_us(); // set new time for next measurement
+ // meassure dt for the filter
+ dt = GlobalTimer.read() - time_for_dt; // time in us since last loop
+ time_for_dt = GlobalTimer.read(); // set new time for next measurement
IMU.compute(dt, Gyro.data, Acc.data);
// Arming / disarming
if(RC[3].read() < 20 && RC[2].read() > 850) {
armed = true;
- #ifdef LOGGER
- if(Logger == NULL)
- Logger = fopen("/local/log.csv", "a");
- #endif
}
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) {
armed = false;
- #ifdef LOGGER
- if(Logger != NULL) {
- fclose(Logger);
- Logger = NULL;
- }
- #endif
}
- for(int i=0;i<2;i++) // calculate new angle we want the QC to have
+ 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;
- //RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500;
+ if (RC_angle[i] < -RC_SENSITIVITY-2)
+ RC_angle[i] = 0;
+ }
+ //RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500; // for yaw angle is integrated
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
@@ -113,98 +100,54 @@
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;*/
-
- // PID controlling
- /*if (!(RC[0].read() == -100)) { // the RC must be there to controll // alte version mit setpoint, nicht nötig? granzen bei yaw los? :)
- Controller[0].setSetPoint(-((RC[0].read()-500)*MAXPITCH/500.0)); // set angles based on RC input
- Controller[1].setSetPoint(-((RC[1].read()-500)*MAXPITCH/500.0));
- Controller[2].setSetPoint(-((RC[3].read()-500)*180.0/500.0));
- }*/
-
-
- MIX.compute(dt, RC[3].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
+
+ MIX.compute(RC[3].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];
- #ifdef LOGGER
- // Writing Log
- for(int i = 0; i < 3; i++) {
- fprintf(Logger, "%f;", angle[i]);
- fprintf(Logger, "%f;", controller_value[i]);
- }
- fprintf(Logger, "\r\n");
- #endif
} else {
for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
ESC[i] = 0;
}
}
-void execute() {
+void commandexecuter(char* command) { // take new PID values on the fly
if (command[0] == 'p')
- P = atoi(&command[1]);
+ P = atof(&command[1]);
if (command[0] == 'i')
- I = atoi(&command[1]);
+ I = atof(&command[1]);
if (command[0] == 'd')
- D = atoi(&command[1]);
-}
-
-void pc_worker() {
- char input = pc.getc();
-
- if (input == '\r') {
- execute();
- command[0] = '\0';
- } else {
- int i = 0;
- while(command[i] != '\0'){
- i++;
- LEDs.rollnext();
- }
- command[i] = input;
- command[i+1] = '\0';
+ D = atof(&command[1]);
+ for(int i=0;i<2;i++) {
+ Controller[i].setPID(P,I,D); // give the controller the new PID values
}
}
int main() { // main programm for initialisation and debug output
NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished)
- //pc.attach(&pc_worker); // zum Befehle geben
-
- #ifdef LOGGER
- Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
- for(int i = 0; i < 3; i++) {
- fprintf(Logger, "angle[%d];", i);
- fprintf(Logger, "controller_value[%d];", i);
- }
- fprintf(Logger, "\r\n");
- fclose(Logger);
- Logger = NULL;
- #endif
-
#ifdef PC_CONNECTED
- #ifdef COMPASSCALIBRATE
- pc.locate(10,5);
- pc.printf("CALIBRATING");
- Comp.calibrate(60);
- #endif
-
// init screen
pc.locate(10,5);
pc.printf("Flybed v0.2");
#endif
LEDs.roll(2);
+ Gyro.calibrate(50, 0.02);
+ Acc.calibrate(50, 0.02);
+
// Start!
GlobalTimer.start();
Dutycycler.attach(&dutycycle, RATE); // start to process all RATEms
while(1) {
- //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]); // serialplot of IMU
+ 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
#if 1 //pc.cls();
- pc.locate(30,0); // PC output
- pc.printf("dt:%3.3fms dt_sensors:%dus Altitude:%6.1fm ", dt/1000.0, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
+ 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));
pc.locate(5,1);
if(armed)
pc.printf("ARMED!!!!!!!!!!!!!");
@@ -213,11 +156,15 @@
pc.locate(5,3);
pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
pc.locate(5,4);
- pc.printf("P:%6.1f I:%6.1f D:%6.1f ", P, I, D);
+ pc.printf("q0:%6.1f q1:%6.1f q2:%6.1f q3:%6.1f ", IMU.q0, IMU.q1, IMU.q2, IMU.q3);
pc.locate(5,5);
pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
pc.locate(5,6);
- pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]);
+ pc.printf("Acc.data: X:%6.1f Y:%6.1f Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]);
+
+ pc.locate(5,8);
+ pc.printf("P:%6.1f I:%6.1f D:%6.1f ", P, I, D);
+
pc.locate(5,11);
pc.printf("PID Result:");
for(int i=0;i<3;i++)
@@ -229,13 +176,10 @@
// RC
pc.locate(10,19);
- pc.printf("RC0: %4d ", RC[0].read());
- pc.printf("RC1: %4d ", RC[1].read());
- pc.printf("RC2: %4d ", RC[2].read());
- pc.printf("RC3: %4d ", RC[3].read());
+ pc.printf("RC0: %4d RC1: %4d RC2: %4d RC3: %4d ", RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
pc.locate(10,21);
- pc.printf("Commandline: %s ", command);
+ pc.printf("Commandline: %s ", pc.command);
#endif
if(armed){
LEDs.rollnext();
Matthias Grob
