Bo Carøe
Test1 of cmsis and IMU/AHRS (sensor BMA180,HMC5883,ITG3200) IMU/AHRS is not ok
Revision 0:cb04b53e6f9b, committed 2012-06-11
- Comitter:
- caroe
- Date:
- Mon Jun 11 12:02:30 2012 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS/AHRS.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,244 @@
+#include "mbed.h"
+#include "AHRS.h"
+#include "cmsis_os.h"
+
+////////////////////////////////////////////////////////////////
+//Init
+AHRS::AHRS(I2C & I2CBus_, Timer & GlobalTime_) :
+ GlobalTime(GlobalTime_),
+ Acc(I2CBus_, GlobalTime_),
+ Mag(I2CBus_, GlobalTime_),
+ Gyro(I2CBus_, GlobalTime_){}
+
+
+void AHRS::Init()
+{
+ //Vordefinierte Werte geben
+ dT= 0.01f;
+ qWorld= Quaternion(1, 0, 0, 0);
+ qSensor= Quaternion(1, 0, 0, 0);;
+ vEulerAngles= Vector3(0, 0, 0);
+ Roll= Nick= Yaw= 0;
+ q0= 1; q1= q2= q3= 0;
+ Beta= 0.05f;
+
+ //Sensoren initialisieren
+ Acc.Init();
+ Mag.Init();
+ Gyro.Init();
+ osDelay(10);
+}
+
+////////////////////////////////////////////////////////////////
+//Update
+
+void AHRS::Update()
+{
+ //Time Step (=Zeitdifferenz berechnen)
+ static int LastFrame= GlobalTime.read_us();
+ int Now= GlobalTime.read_us();
+ dT= 0.000001 * (float)(Now - LastFrame);
+ LastFrame= Now;
+
+ //Sensoren updaten
+ // Acc.Update();
+ // Mag.Update();
+ // Gyro.Update();
+
+ //Das Madgwick AHRS verwendet ein anderes Koordinatensystem!
+ MadgwickAHRSupdate(-Gyro.Rate[1], Gyro.Rate[0], Gyro.Rate[2],
+ Acc.Acc [0], Acc.Acc [1], Acc.Acc [2],
+ Mag.Mag [0], Mag.Mag [1], Mag.Mag [2]);
+
+ //Quaternionen bilden
+ qWorld.w= q0;
+ qWorld.v.x= -q2;
+ qWorld.v.y= q3;
+ qWorld.v.z= q1;
+ qSensor= qWorld.Conjugate();
+
+ //Eulerwinkel berechnen
+ vEulerAngles= qWorld.CalcEulerAngles();
+ Roll= -vEulerAngles.z;
+ Nick= -vEulerAngles.x;
+ Yaw = vEulerAngles.y;
+}
+
+
+////////////////////////////////////////////////////////////////
+//Madgwicks Code
+
+float invSqrt(float x)
+{
+ float halfx= 0.5f * x;
+ float y= x;
+ long i= *(long*)&y;
+ i= 0x5f3759df - (i>>1);
+ y= *(float*)&i;
+ y= y * (1.5f - (halfx * y * y));
+ return(y);
+}
+
+void AHRS::MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
+ float recipNorm;
+ float s0, s1, s2, s3;
+ float qDot1, qDot2, qDot3, qDot4;
+ float hx, hy;
+ float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+ // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+ if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))
+ {
+ MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+ return;
+ }
+
+ // Rate of change of quaternion from gyroscope
+ qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+ qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+ qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+ qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+ if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
+ {
+
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ _2q0mx = 2.0f * q0 * mx;
+ _2q0my = 2.0f * q0 * my;
+ _2q0mz = 2.0f * q0 * mz;
+ _2q1mx = 2.0f * q1 * mx;
+ _2q0 = 2.0f * q0;
+ _2q1 = 2.0f * q1;
+ _2q2 = 2.0f * q2;
+ _2q3 = 2.0f * q3;
+ _2q0q2 = 2.0f * q0 * q2;
+ _2q2q3 = 2.0f * q2 * q3;
+ q0q0 = q0 * q0;
+ q0q1 = q0 * q1;
+ q0q2 = q0 * q2;
+ q0q3 = q0 * q3;
+ q1q1 = q1 * q1;
+ q1q2 = q1 * q2;
+ q1q3 = q1 * q3;
+ q2q2 = q2 * q2;
+ q2q3 = q2 * q3;
+ q3q3 = q3 * q3;
+
+ // Reference direction of Earth's magnetic field
+ hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+ hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+ _2bx = sqrt(hx * hx + hy * hy);
+ _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+ _4bx = 2.0f * _2bx;
+ _4bz = 2.0f * _2bz;
+
+ // Gradient decent algorithm corrective step
+ s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+ s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+ s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+ s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+ s0 *= recipNorm;
+ s1 *= recipNorm;
+ s2 *= recipNorm;
+ s3 *= recipNorm;
+
+ // Apply feedback step
+ qDot1 -= Beta * s0;
+ qDot2 -= Beta * s1;
+ qDot3 -= Beta * s2;
+ qDot4 -= Beta * s3;
+ }
+
+ // Integrate rate of change of quaternion to yield quaternion
+ q0 += qDot1 * dT;
+ q1 += qDot2 * dT;
+ q2 += qDot3 * dT;
+ q3 += qDot4 * dT;
+
+ // Normalise quaternion
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+}
+
+
+void AHRS::MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
+{
+ float recipNorm;
+ float s0, s1, s2, s3;
+ float qDot1, qDot2, qDot3, qDot4;
+ float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+ // Rate of change of quaternion from gyroscope
+ qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+ qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+ qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+ qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+ if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
+ {
+
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ _2q0 = 2.0f * q0;
+ _2q1 = 2.0f * q1;
+ _2q2 = 2.0f * q2;
+ _2q3 = 2.0f * q3;
+ _4q0 = 4.0f * q0;
+ _4q1 = 4.0f * q1;
+ _4q2 = 4.0f * q2;
+ _8q1 = 8.0f * q1;
+ _8q2 = 8.0f * q2;
+ q0q0 = q0 * q0;
+ q1q1 = q1 * q1;
+ q2q2 = q2 * q2;
+ q3q3 = q3 * q3;
+
+ // Gradient decent algorithm corrective step
+ s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+ s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+ s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+ s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+ s0 *= recipNorm;
+ s1 *= recipNorm;
+ s2 *= recipNorm;
+ s3 *= recipNorm;
+
+ // Apply feedback step
+ qDot1 -= Beta * s0;
+ qDot2 -= Beta * s1;
+ qDot3 -= Beta * s2;
+ qDot4 -= Beta * s3;
+ }
+
+ // Integrate rate of change of quaternion to yield quaternion
+ q0 += qDot1 * dT;
+ q1 += qDot2 * dT;
+ q2 += qDot3 * dT;
+ q3 += qDot4 * dT;
+
+ // Normalise quaternion
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS/AHRS.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,43 @@
+#pragma once
+
+#include "BMA180.h"
+#include "HMC5883.h"
+#include "ITG3200.h"
+#include "GTMath.h"
+
+////////////////////////////////////////////////////////////////
+//Attitude Heading Reference System
+class AHRS
+{
+public:
+ ////////////////////////////////////////////////////////////////
+ //Interfaces: Selbst kalibrieren!
+ Timer & GlobalTime;
+ BMA180 Acc;
+ HMC5883 Mag;
+ ITG3200 Gyro;
+
+ ////////////////////////////////////////////////////////////////
+ //AHRS Outputs
+ float dT; //Time Step
+ Quaternion qWorld; //absolute Orientierung der Sensoren
+ Quaternion qSensor; //relative Orientierung des Raums
+ Vector3 vEulerAngles; //absolute Eulerwinkel
+ float Roll, Nick, Yaw; //Eulerwinkel
+
+ ////////////////////////////////////////////////////////////////
+ //Init
+ AHRS(I2C & I2CBus_, Timer & GlobalTime_);
+ void Init();
+
+ ////////////////////////////////////////////////////////////////
+ //Update
+ void Update();
+
+ //Madgwick AHRS/IMU Code (see http://www.x-io.co.uk/node/8#open_source_imu_and_ahrs_algorithms)
+ float Beta; //Algorithm Gain
+ float q0, q1, q2, q3; //qWorld in Madgwicks Koordinatensystem (wird in Update angepasst)
+
+ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+ void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BMA180/BMA180.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,141 @@
+#include "mbed.h"
+#include "BMA180.h"
+#include "cmsis_os.h"
+#define I2CADR_W(ADR) (ADR<<1&0xFE)
+#define I2CADR_R(ADR) (ADR<<1|0x01)
+
+//Initialisieren
+BMA180::BMA180(I2C & I2CBus_, Timer & GlobalTime_)
+ : I2CBus(I2CBus_),
+ GlobalTime(GlobalTime_)
+{}
+
+void BMA180::Init()
+{
+ //Nullsetzen
+ for(int i= 0; i < 3; i++)
+ {
+ Offset[i]= 0.0;
+ RawAcc[i]= 0;
+ Acc[i]= 0;
+ }
+
+
+ //BMA180 initialisieren
+ char tx[2];
+ char rx[1];
+
+ //Schreiben aktivieren
+ tx[0]= 0x0D;
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(BMA180_ADRESS), rx, 1);
+ tx[1]= rx[0] & 0xEF | (0x01<<4);
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 2);
+
+ //Mode: Low-Noise
+ tx[0]= 0x30;
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(BMA180_ADRESS), rx, 1);
+ tx[1]= rx[0] & 0xFC | (0x00<<0);
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 2);
+
+ //Range: +-3g
+ tx[0]= 0x35;
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(BMA180_ADRESS), rx, 1);
+ tx[1]= rx[0] & 0xF1 | (0x03<<1);
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 2);
+
+ //Bandbreitenfilter: 10Hz
+ tx[0]= 0x20;
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(BMA180_ADRESS), rx, 1);
+ tx[1]= rx[0] & 0x0F | (0x00<<4);
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 2);
+
+ Update(3,10);
+}
+
+
+//Rohdaten lesen
+void BMA180::ReadRawData()
+{
+ //Beschleunigung f�r alle 3 Achsen auslesen
+ char tx[1];
+ char rx[6];
+
+ tx[0]= 0x02;
+ I2CBus.write(I2CADR_W(BMA180_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(BMA180_ADRESS), rx, 6);
+
+ //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen
+ //Die 2 Statusbits muessen "abgeschnitten" werden
+ RawAcc[0]= (rx[1]<<8|rx[0]) & 0xFFFC; // x
+ RawAcc[1]= (rx[3]<<8|rx[2]) & 0xFFFC; // y
+ RawAcc[2]= (rx[5]<<8|rx[4]) & 0xFFFC; // z
+ RawAcc[0]/= 4;
+ RawAcc[1]/= 4;
+ RawAcc[2]/= 4;
+}
+
+//Update-Methode
+void BMA180::Update(int antal_samples,int samples_delay)
+{
+ float AvgSampelAcc[3]= {0.0, 0.0, 0.0};
+ int AvgSampel= 0;
+
+
+ while(AvgSampel <antal_samples) // Antal Samples
+ {
+ //Rohdaten lesen
+ ReadRawData();
+
+ //Durchschnitt bilden
+ AvgSampelAcc[0]+= (float)RawAcc[0];
+ AvgSampelAcc[1]+= (float)RawAcc[1];
+ AvgSampelAcc[2]+= (float)RawAcc[2];
+ AvgSampel+= 1.0;
+
+ osDelay(samples_delay);
+ }
+
+
+ //Beschleunigung f�r alle 3 Achsen auslesen
+
+ //Umrechnungen
+ Acc[0]= fConvMPSS * ((float)(AvgSampelAcc[0] / AvgSampel) + Offset[0]);
+ Acc[1]= fConvMPSS * ((float)(AvgSampelAcc[1] / AvgSampel) + Offset[1]);
+ Acc[2]= fConvMPSS * ((float)(AvgSampelAcc[2] / AvgSampel) + Offset[2]);
+
+
+
+}
+
+//Kalibrieren
+void BMA180::Calibrate(int ms, short * pRaw1g)
+{
+ float AvgCalibAcc[3]= {0.0, 0.0, 0.0};
+ float AvgCalibSampels= 0.0;
+
+ //Ende der Kalibrierung in ms Millisekunden berechnen
+ int CalibEnd= GlobalTime.read_ms() + ms;
+
+ while(GlobalTime.read_ms() < CalibEnd)
+ {
+ //Rohdaten lesen
+ ReadRawData();
+
+ //Durchschnitt bilden
+ AvgCalibAcc[0]+= (float)RawAcc[0];
+ AvgCalibAcc[1]+= (float)RawAcc[1];
+ AvgCalibAcc[2]+= (float)RawAcc[2];
+ AvgCalibSampels+= 1.0;
+
+ osDelay(10);
+ }
+
+ //Genug Daten gesammelt, jetzt den Durchschnitt bilden
+ Offset[0]= -((float)(pRaw1g[0]) + AvgCalibAcc[0] / AvgCalibSampels);
+ Offset[1]= -((float)(pRaw1g[1]) + AvgCalibAcc[1] / AvgCalibSampels);
+ Offset[2]= -((float)(pRaw1g[2]) + AvgCalibAcc[2] / AvgCalibSampels);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BMA180/BMA180.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,42 @@
+#pragma once
+
+//I2C Adresse, entweder 0x40 oder 0x41, abhaengig von Pin VDDIO
+#define BMA180_ADRESS 0x41
+
+//Beschleunigung in Meter pro Quadratsekunde umrechnen
+const float fConvMPSS= 3.4346447e-3;
+
+class BMA180
+{
+private:
+ I2C & I2CBus;
+ Timer & GlobalTime;
+
+ //Offset
+ float Offset[3];
+
+public:
+ //Beschleunigung auf allen drei Achsen
+ short RawAcc[3]; //Rohdaten
+ float Acc[3]; //kalibrierte Rohdaten in m/s^2
+
+
+ //Initialisieren
+ BMA180(I2C & I2CBus_, Timer & GlobalTime_);
+ void Init();
+
+private:
+ //Rohdaten lesen
+ void ReadRawData();
+
+public:
+ //Update-Methode
+ //- Holt aktuelle Daten vom Sensor ab
+ //- Rechnet das Offset hinzu
+ //- Rechnet in andere Einheiten um
+ void Update(int antal_samples,int samples_delay);
+
+ //Kalibrieren aktivieren
+ //- pRaw1g: Array short[3] ideale Rohdaten f�r 1g = ca. {0, 0, -2870}
+ void Calibrate(int ms, short * pRaw1g);
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ConfigFile/ConfigFile.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,415 @@
+/**
+ * Configuration file interface class (Version 0.0.1)
+ *
+ * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems)
+ * http://shinta.main.jp/
+ */
+#include "ConfigFile.h"
+
+#define NEWLINE_UNIX "\n"
+#define NEWLINE_DOS "\r\n"
+#define NEWLINE_MAC "\r"
+
+/**
+ * Create a configuration file class.
+ */
+ConfigFile::ConfigFile() {
+ /*
+ * Allocation for a config_t list.
+ */
+ configlist = (config_t **)malloc(sizeof(config_t *) * MAXCONFIG);
+ for (int i = 0; i < MAXCONFIG; i++) {
+ configlist[i] = NULL;
+ }
+}
+
+/**
+ * Destroy a configuration file class.
+ */
+ConfigFile::~ConfigFile() {
+ /*
+ * Remove all storage and the contents.
+ */
+ for (int i = 0; i < MAXCONFIG; i++) {
+ config_t *cfg = configlist[i];
+ if (cfg != NULL) {
+ free(cfg->key);
+ free(cfg->value);
+ free(cfg);
+ }
+ configlist[i] = NULL;
+ }
+
+ /*
+ * Remove cnofig_t list.
+ */
+ free(configlist);
+ configlist = NULL;
+}
+
+/**
+ * Get a value for a key.
+ *
+ * @param key A target key name.
+ * @param value A pointer to a value storage.
+ * @param siz A size of a value storage.
+ * @return A value or NULL.
+ */
+bool ConfigFile::getValue(char *key, char *value, size_t siz) {
+ /*
+ * Null check.
+ */
+ if (key == NULL) {
+ return false;
+ }
+
+ /*
+ * Search a config_t object from the key.
+ */
+ config_t *p = search(key);
+ if (p == NULL) {
+ return false;
+ }
+
+ /*
+ * Check the storage size.
+ */
+ if (siz <= strlen(p->value)) {
+ return false;
+ }
+
+ /*
+ * Copy the value to the storage.
+ */
+ strcpy(value, p->value);
+ return true;
+}
+
+/**
+ * Set a set of a key and value.
+ *
+ * @param key A key.
+ * @param value A value.
+ *
+ * @return True if it succeed.
+ */
+bool ConfigFile::setValue(char *key, char *value) {
+ /*
+ * Null check.
+ */
+ if ((key == NULL) || (value == NULL)) {
+ return false;
+ }
+
+ /*
+ * Size check.
+ */
+ if ((MAXLEN_KEY < strlen(key)) || (MAXLEN_VALUE < strlen(value))) {
+ return false;
+ }
+
+ /*
+ * Search a config_t object from the key.
+ */
+ config_t *p = search(key);
+ if (p == NULL) {
+ /*
+ * Allocation a memory for a new key.
+ */
+ char *k = (char *)malloc(sizeof(char) * (strlen(key) + 1));
+ if (k == NULL) {
+ return false;
+ }
+ strcpy(k, key);
+
+ /*
+ * Allocation a memory for a new value.
+ */
+ char *v = (char *)malloc(sizeof(char) * (strlen(value) + 1));
+ if (v == NULL) {
+ free(k);
+ return false;
+ }
+ strcpy(v, value);
+
+ /*
+ * Allocation a memory for a new configuration.
+ */
+ config_t *cfg = (config_t *)malloc(sizeof(config_t) * 1);
+ if (cfg == NULL) {
+ free(k);
+ free(v);
+ return false;
+ }
+ cfg->key = k;
+ cfg->value = v;
+
+ /*
+ * Add the new configuration.
+ */
+ if (!add(cfg)) {
+ free(k);
+ free(v);
+ free(cfg);
+ return false;
+ }
+
+ return true;
+ } else {
+ /*
+ * The value is same.
+ */
+ if (strcmp(value, p->value) == 0) {
+ return true;
+ }
+
+ /*
+ * Free a memory for the value.
+ */
+ free(p->value);
+ p->value = NULL;
+
+ /*
+ * Allocation memory for the new value.
+ */
+ char *v = (char *)malloc(sizeof(char) * (strlen(value) + 1));
+ if (v == NULL) {
+ return false;
+ }
+
+ /*
+ * Store it.
+ */
+ strcpy(v, value);
+ p->value = v;
+
+ return true;
+ }
+}
+
+/**
+ * Remove a config.
+ *
+ * @param key A key.
+ *
+ * @return True if it succeed.
+ */
+bool ConfigFile::remove(char *key) {
+ if (key == NULL) {
+ return false;
+ }
+ for (int i = 0; i < MAXCONFIG; i++) {
+ config_t *cfg = configlist[i];
+ if (cfg != NULL) {
+ if (strcmp(cfg->key, key) == 0) {
+ free(cfg->key);
+ free(cfg->value);
+ free(cfg);
+ configlist[i] = NULL;
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+/**
+ * Remove all config.
+ *
+ * @return True if it succeed.
+ */
+bool ConfigFile::removeAll(void) {
+ for (int i = 0; i < MAXCONFIG; i++) {
+ config_t *p = configlist[i];
+ if (p != NULL) {
+ free(p->key);
+ free(p->value);
+ }
+ free(p);
+ configlist[i] = NULL;
+ }
+ return true;
+}
+
+/**
+ * Get a number of configuration sets.
+ *
+ * @return number of configuration sets.
+ */
+int ConfigFile::getCount() {
+ int cnt = 0;
+ for (int i = 0; i < MAXCONFIG; i++) {
+ config_t *p = configlist[i];
+ if (p != NULL) {
+ cnt++;
+ }
+ }
+ return cnt;
+}
+
+/**
+ * Get a key and a value.
+ *
+ * @param index Index number of this list.
+ * @param key A pointer to a buffer for key.
+ * @param keybufsiz A size of the key buffer.
+ * @param value A pointer to a buffer for value.
+ * @param valuebufsiz A size of the value buffer.
+ *
+ * @return true if it succeed.
+ */
+bool ConfigFile::getKeyAndValue(int index, char *key, size_t keybufsiz, char *value, size_t valuebufsiz) {
+ int cnt = 0;
+ for (int i = 0; i < MAXCONFIG; i++) {
+ config_t *p = configlist[i];
+ if (p != NULL) {
+ if (cnt == index) {
+ if ((strlen(p->key) < keybufsiz) && (strlen(p->value) < valuebufsiz)) {
+ strcpy(key, p->key);
+ strcpy(value, p->value);
+ return true;
+ }
+ return false;
+ }
+ cnt++;
+ }
+ }
+ return false;
+}
+
+/**
+ * Read from the target file.
+ *
+ * @param file A target file name.
+ */
+bool ConfigFile::read(char *file) {
+ /*
+ * Open the target file.
+ */
+ FILE *fp = fopen(file, "r");
+ if (fp == NULL) {
+ return false;
+ }
+
+ /*
+ * Remove all configuration.
+ */
+ removeAll();
+
+ /*
+ * Read from a file.
+ */
+ char buf[MAXLEN_KEY + 8 + MAXLEN_VALUE];
+ while (fgets(buf, sizeof(buf), fp) != NULL) {
+ /*
+ * Ignore a comment.
+ */
+ if (buf[0] == '#') {
+ continue;
+ }
+
+ /*
+ * Trim a return.
+ */
+ const size_t len = strlen(buf);
+ for (int i = 0; i < len; i++) {
+ if ((buf[i] == '\r') || (buf[i] == '\n')) {
+ buf[i] = '\0';
+ }
+ }
+
+ /*
+ * Separate key and value.
+ */
+ char k[MAXLEN_KEY];
+ char v[MAXLEN_VALUE];
+ char *sp = strchr(buf, SEPARATOR);
+ if (sp != NULL) {
+ strcpy(v, sp + 1);
+ *sp = '\0';
+ strcpy(k, buf);
+ setValue(k, v);
+ }
+ }
+ fclose(fp);
+ return true;
+}
+
+/**
+ * Write from the target file.
+ *
+ * @param file A pointer to a file name.
+ * @param header A pointer to a header.
+ * @param ff File format.
+ */
+bool ConfigFile::write(char *file, char *header, FileFormat ff) {
+ /*
+ * Open the target file.
+ */
+ FILE *fp = fopen(file, "w");
+ if (fp == NULL) {
+ return false;
+ }
+
+ /*
+ * Set a type of new line.
+ */
+ char *newline = NEWLINE_UNIX;
+ switch (ff) {
+ case UNIX:
+ newline = NEWLINE_UNIX;
+ break;
+ case MAC:
+ newline = NEWLINE_MAC;
+ break;
+ case DOS:
+ newline = NEWLINE_DOS;
+ break;
+ default:
+ newline = NEWLINE_UNIX;
+ break;
+ }
+
+ /*
+ * Write the header.
+ */
+ if (header != NULL) {
+ fprintf(fp, "%s%s", header, newline);
+ }
+
+ /*
+ * Write the data.
+ */
+ for (int i = 0; i < MAXCONFIG; i++) {
+ config_t *cfg = configlist[i];
+ if (cfg != NULL) {
+ fprintf(fp, "%s=%s%s", cfg->key, cfg->value, newline);
+ }
+ }
+ fclose(fp);
+ return true;
+}
+
+ConfigFile::config_t *ConfigFile::search(char *key) {
+ if (key == NULL) {
+ return NULL;
+ }
+ for (int i = 0; i < MAXCONFIG; i++) {
+ if (configlist[i] != NULL) {
+ if (strcmp(configlist[i]->key, key) == 0) {
+ return configlist[i];
+ }
+ }
+ }
+ return NULL;
+}
+
+bool ConfigFile::add(config_t *cfg) {
+ for (int i = 0; i < MAXCONFIG; i++) {
+ if (configlist[i] == NULL) {
+ configlist[i] = cfg;
+ return true;
+ }
+ }
+ return false;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ConfigFile/ConfigFile.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,121 @@
+/**
+ * Configuration file interface class (Version 0.0.1)
+ *
+ * Copyright (C) 2010 Shinichiro Nakamura (CuBeatSystems)
+ * http://shinta.main.jp/
+ */
+#include "mbed.h"
+#include "cmsis_os.h"
+#ifndef _CONFIG_FILE_H_
+#define _CONFIG_FILE_H_
+
+/**
+ * Configuration File class.
+ */
+class ConfigFile {
+public:
+
+ /**
+ * Create a configuration file class.
+ */
+ ConfigFile();
+
+ /**
+ * Destroy a configuration file class.
+ */
+ ~ConfigFile();
+
+ /**
+ * Get a value for a key.
+ *
+ * @param key A target key name.
+ * @param value A pointer to a value storage.
+ * @param siz A size of a value storage.
+ * @return A value or NULL.
+ */
+ bool getValue(char *key, char *value, size_t siz);
+
+ /**
+ * Set a set of a key and value.
+ *
+ * @param key A key.
+ * @param value A value.
+ *
+ * @return True if it succeed.
+ */
+ bool setValue(char *key, char *value);
+
+ /**
+ * Remove a config.
+ *
+ * @param key A key.
+ *
+ * @return True if it succeed.
+ */
+ bool remove(char *key);
+
+ /**
+ * Remove all config.
+ *
+ * @return True if it succeed.
+ */
+ bool removeAll(void);
+
+ /**
+ * Get a number of configuration sets.
+ *
+ * @return number of configuration sets.
+ */
+ int getCount();
+
+ /**
+ * Get a key and a value.
+ *
+ * @param index Index number of this list.
+ * @param key A pointer to a buffer for key.
+ * @param keybufsiz A size of the key buffer.
+ * @param value A pointer to a buffer for value.
+ * @param valuebufsiz A size of the value buffer.
+ *
+ * @return true if it succeed.
+ */
+ bool getKeyAndValue(int index, char *key, size_t keybufsiz, char *value, size_t valuebufsiz);
+
+ /**
+ * Read from the target file.
+ *
+ * @param file A target file name.
+ */
+ bool read(char *file);
+
+ typedef enum {
+ UNIX,
+ MAC,
+ DOS
+ } FileFormat;
+
+ /**
+ * Write from the target file.
+ *
+ * @param file A pointer to a file name.
+ * @param header A pointer to a header.
+ * @param ff File format.
+ */
+ bool write(char *file, char *header = NULL, FileFormat ff = UNIX);
+
+private:
+ typedef struct {
+ char *key;
+ char *value;
+ } config_t;
+ config_t **configlist;
+ static const int MAXCONFIG = 64;
+ static const int MAXLEN_KEY = 64;
+ static const int MAXLEN_VALUE = 128;
+ static const char SEPARATOR = '=';
+
+ config_t *search(char *key);
+ bool add(config_t *cfg);
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GTMath/GTMath.h Mon Jun 11 12:02:30 2012 +0000 @@ -0,0 +1,9 @@ +#pragma once + +#include "math.h" + +#include "Templates.h" +#include "Vector2.h" +#include "Vector3.h" +#include "Quaternion.h" +#include "Matrix3x3.h"
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Matrix3x3.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,337 @@
+#include "GTMath.h"
+
+//Konstruktoren
+Matrix3x3::Matrix3x3()
+{}
+
+Matrix3x3::Matrix3x3(float m11, float m12, float m13,
+ float m21, float m22, float m23,
+ float m31, float m32, float m33) :
+ m11(m11), m12(m12), m13(m13),
+ m21(m21), m22(m22), m23(m23),
+ m31(m31), m32(m32), m33(m33)
+{}
+
+Matrix3x3::Matrix3x3(const Matrix3x3 &m) :
+ m11(m.m11), m12(m.m12), m13(m.m13),
+ m21(m.m21), m22(m.m22), m23(m.m23),
+ m31(m.m31), m32(m.m32), m33(m.m33)
+{}
+
+
+//Casting
+//Direkter Zugriff auf die Daten
+Matrix3x3::operator float* ()
+{
+ return(af);
+}
+
+//operator() f� besseren Zugriff
+float Matrix3x3::operator()(int iRow, int iColumn) const { return(aaf[iRow-1][iColumn-1]); }
+float & Matrix3x3::operator()(int iRow, int iColumn) { return(aaf[iRow-1][iColumn-1]); }
+
+//Zuweisungen
+Matrix3x3 & Matrix3x3::operator = (const Matrix3x3 &m)
+{
+ m11= m.m11; m12= m.m12; m13= m.m13;
+ m21= m.m21; m22= m.m22; m23= m.m23;
+ m31= m.m31; m32= m.m32; m33= m.m33;
+ return(*this);
+}
+Matrix3x3 & Matrix3x3::operator += (const Matrix3x3 &m)
+{
+ m11+= m.m11; m12+= m.m12; m13+= m.m13;
+ m21+= m.m21; m22+= m.m22; m23+= m.m23;
+ m31+= m.m31; m32+= m.m32; m33+= m.m33;
+ return(*this);
+}
+Matrix3x3 & Matrix3x3::operator-=(const Matrix3x3 &m)
+{
+ m11-= m.m11; m12-= m.m12; m13-= m.m13;
+ m21-= m.m21; m22-= m.m22; m23-= m.m23;
+ m31-= m.m31; m32-= m.m32; m33-= m.m33;
+ return(*this);
+}
+Matrix3x3 & Matrix3x3::operator*=(const Matrix3x3 &m)
+{
+ return(*this= Matrix3x3(m.m11 * m11 + m.m21 * m12 + m.m31 * m13,
+ m.m12 * m11 + m.m22 * m12 + m.m32 * m13,
+ m.m13 * m11 + m.m23 * m12 + m.m33 * m13,
+ m.m11 * m21 + m.m21 * m22 + m.m31 * m23,
+ m.m12 * m21 + m.m22 * m22 + m.m32 * m23,
+ m.m13 * m21 + m.m23 * m22 + m.m33 * m23,
+ m.m11 * m31 + m.m21 * m32 + m.m31 * m33,
+ m.m12 * m31 + m.m22 * m32 + m.m32 * m33,
+ m.m13 * m31 + m.m23 * m32 + m.m33 * m33));
+}
+Matrix3x3 & Matrix3x3::operator/=(const Matrix3x3 &m)
+{
+ return(*this *= m.Invert());
+}
+
+Matrix3x3 & Matrix3x3::operator*=(float f)
+{
+ m11*=f; m12*=f; m13*=f;
+ m21*=f; m22*=f; m23*=f;
+ m31*=f; m32*=f; m33*=f;
+ return *this;
+}
+Matrix3x3 & Matrix3x3::operator/=(float f)
+{
+ const float fInv= 1.0 / f;
+ m11*=fInv; m12*=fInv; m13*=fInv;
+ m21*=fInv; m22*=fInv; m23*=fInv;
+ m31*=fInv; m32*=fInv; m33*=fInv;
+ return *this;
+}
+
+////////////////////////////////////////////////////////////
+//Arithmetik
+const Matrix3x3 Matrix3x3::operator+(const Matrix3x3 &m) const
+{
+ return(Matrix3x3(m11+m.m11, m12+m.m12, m13+m.m13,
+ m21+m.m21, m22+m.m22, m23+m.m23,
+ m31+m.m31, m32+m.m32, m33+m.m33));
+}
+const Matrix3x3 Matrix3x3::operator-(const Matrix3x3 &m) const
+{
+ return(Matrix3x3(m11-m.m11, m12-m.m12, m13-m.m13,
+ m21-m.m21, m22-m.m22, m23-m.m23,
+ m31-m.m31, m32-m.m32, m33-m.m33));
+}
+const Matrix3x3 Matrix3x3::operator*(const Matrix3x3 &m) const
+{
+ return(Matrix3x3(m.m11 * m11 + m.m21 * m12 + m.m31 * m13,
+ m.m12 * m11 + m.m22 * m12 + m.m32 * m13,
+ m.m13 * m11 + m.m23 * m12 + m.m33 * m13,
+ m.m11 * m21 + m.m21 * m22 + m.m31 * m23,
+ m.m12 * m21 + m.m22 * m22 + m.m32 * m23,
+ m.m13 * m21 + m.m23 * m22 + m.m33 * m23,
+ m.m11 * m31 + m.m21 * m32 + m.m31 * m33,
+ m.m12 * m31 + m.m22 * m32 + m.m32 * m33,
+ m.m13 * m31 + m.m23 * m32 + m.m33 * m33));
+}
+const Matrix3x3 Matrix3x3::operator/(const Matrix3x3 &m) const
+{
+ return(*this * m.Invert());
+}
+const Matrix3x3 Matrix3x3::operator*(float f) const
+{
+ return(Matrix3x3(m11*f, m12*f, m13*f,
+ m21*f, m22*f, m23*f,
+ m31*f, m32*f, m33*f));
+}
+
+const Matrix3x3 Matrix3x3::operator/(float f) const
+{
+ const float fInv= 1.0 / f;
+ return(Matrix3x3(m11*fInv, m12*fInv, m13*fInv,
+ m21*fInv, m22*fInv, m23*fInv,
+ m31*fInv, m32*fInv, m33*fInv));
+}
+
+////////////////////////////////////////////////////////////
+//Identit�Matrix3x3, Transponieren
+const Matrix3x3 & Matrix3x3::Identity()
+{
+ static const Matrix3x3 mIdentity(
+ 1.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, 1.0);
+ return(mIdentity);
+}
+const Matrix3x3 Matrix3x3::Transpose() const
+{
+ return(Matrix3x3(m11, m21, m31,
+ m12, m22, m32,
+ m13, m23, m33));
+}
+
+////////////////////////////////////////////////////////////
+//Determinante, Invertieren
+//Determinante
+float Matrix3x3::Determinate() const
+{
+ return(m11 * (m22*m33 - m23*m32) -
+ m12 * (m21*m33 - m23*m31) +
+ m13 * (m21*m32 - m22*m31));
+}
+//Invertieren
+const Matrix3x3 Matrix3x3::Invert() const
+{
+ float fInvDet= Determinate();
+ if(fInvDet == 0.0)
+ return(Identity());
+ fInvDet= 1.0 / fInvDet;
+
+ Matrix3x3 mResult;
+ mResult.m11 = fInvDet * (m22*m33 - m23*m32);
+ mResult.m12 = -fInvDet * (m12*m33 - m13*m32);
+ mResult.m13 = fInvDet * (m12*m23 - m13*m22);
+ mResult.m21 = -fInvDet * (m21*m33 - m23*m31);
+ mResult.m22 = fInvDet * (m11*m33 - m13*m31);
+ mResult.m23 = -fInvDet * (m11*m23 - m13*m21);
+ mResult.m31 = fInvDet * (m21*m32 - m22*m31);
+ mResult.m32 = -fInvDet * (m11*m32 - m12*m31);
+ mResult.m33 = fInvDet * (m11*m22 - m12*m21);
+
+ return(mResult);
+}
+
+////////////////////////////////////////////////////////////
+//Skalierung, Achsen-Matrix
+//Skalierung
+const Matrix3x3 Matrix3x3::Scale(const Vector3 &v)
+{
+ return(Matrix3x3(v.x, 0.0, 0.0,
+ 0.0, v.y, 0.0,
+ 0.0, 0.0, v.z));
+}
+//AchsenMatrix3x3
+const Matrix3x3 Matrix3x3::Axes(const Vector3 &vX, const Vector3 &vY, const Vector3 &vZ)
+{
+ return(Matrix3x3(vX.x, vX.y, vX.z,
+ vY.x, vY.y, vY.z,
+ vZ.x, vZ.y, vZ.z));
+}
+////////////////////////////////////////////////////////////
+//Rotationsmatrizen
+//Rotation um X-Achse
+const Matrix3x3 Matrix3x3::RotateX(float fAngle)
+{
+ Matrix3x3 mResult;
+
+ mResult.m11 = 1.0; mResult.m12 = 0.0; mResult.m13 = 0.0;
+ mResult.m21 = 0.0;
+ mResult.m31 = 0.0;
+
+ mResult.m22 = mResult.m33 = cos(fAngle);
+ mResult.m23 = sin(fAngle);
+ mResult.m32 = -mResult.m23;
+
+ return(mResult);
+}
+//Rotation um Y-Achse
+const Matrix3x3 Matrix3x3::RotateY(float fAngle)
+{
+ Matrix3x3 mResult;
+
+ mResult.m12 = 0.0F;
+ mResult.m21 = 0.0F; mResult.m22 = 1.0F; mResult.m23 = 0.0F;
+ mResult.m32 = 0.0F;
+
+ mResult.m11 = mResult.m33 = cos(fAngle);
+ mResult.m31 = sin(fAngle);
+ mResult.m13 = -mResult.m31;
+
+ return(mResult);
+}
+//Rotation um Z-Achse
+const Matrix3x3 Matrix3x3::RotateZ(float fAngle)
+{
+ Matrix3x3 mResult;
+
+ mResult.m13 = 0.0F;
+ mResult.m23 = 0.0F;
+ mResult.m31 = 0.0F; mResult.m32 = 0.0F; mResult.m33 = 1.0F;
+
+ mResult.m11 = mResult.m22 = cos(fAngle);
+ mResult.m12 = sin(fAngle);
+ mResult.m21 = -mResult.m12;
+
+ return(mResult);
+}
+//Rotation um alle Achsen zugleich
+const Matrix3x3 Matrix3x3::RotateXYZ(const Vector3 &v)
+{
+ return(RotateX(v.x) * RotateY(v.y) * RotateZ(v.z));
+}
+const Matrix3x3 Matrix3x3::RotateXZY(const Vector3 &v)
+{
+ return(RotateX(v.x) * RotateZ(v.z) * RotateY(v.y));
+}
+const Matrix3x3 Matrix3x3::RotateYXZ(const Vector3 &v)
+{
+ return(RotateY(v.y) * RotateX(v.x) * RotateZ(v.z));
+}
+const Matrix3x3 Matrix3x3::RotateYZX(const Vector3 &v)
+{
+ return(RotateY(v.y) * RotateZ(v.z) * RotateX(v.x));
+}
+const Matrix3x3 Matrix3x3::RotateZXY(const Vector3 &v)
+{
+ return(RotateZ(v.z) * RotateX(v.x) * RotateY(v.y));
+}
+const Matrix3x3 Matrix3x3::RotateZYX(const Vector3 &v)
+{
+ return(RotateZ(v.z) * RotateY(v.y) * RotateX(v.x));
+}
+
+//Rotation um beliebige Achse
+const Matrix3x3 Matrix3x3::RotateAxis(const Vector3 &v, float fAngle)
+{
+ const float fSin = sin(-fAngle);
+ const float fCos = cos(-fAngle);
+ const float fOneMinusCos= 1.0-fCos;
+
+ const Vector3 vAxis(v.Normalize());
+
+ return(Matrix3x3((vAxis.x*vAxis.x) * fOneMinusCos + fCos,
+ (vAxis.x*vAxis.y) * fOneMinusCos - (vAxis.z*fSin),
+ (vAxis.x*vAxis.z) * fOneMinusCos + (vAxis.y*fSin),
+ (vAxis.y*vAxis.x) * fOneMinusCos + (vAxis.z*fSin),
+ (vAxis.y*vAxis.y) * fOneMinusCos + fCos,
+ (vAxis.y*vAxis.z) * fOneMinusCos - (vAxis.x*fSin),
+ (vAxis.z*vAxis.x) * fOneMinusCos - (vAxis.y*fSin),
+ (vAxis.z*vAxis.y) * fOneMinusCos + (vAxis.x*fSin),
+ (vAxis.z*vAxis.z) * fOneMinusCos + fCos));
+}
+//Rotation aus Quaternion
+const Matrix3x3 Matrix3x3::RotateQuaternion(const Quaternion &q)
+{
+ return(Matrix3x3(1.0F - 2.0F*q.v.y*q.v.y - 2.0F*q.v.z*q.v.z, 2.0F*q.v.x*q.v.y + 2.0F*q. w*q.v.z, 2.0F*q.v.x*q.v.z - 2.0F*q. w*q.v.y,
+ 2.0F*q.v.x*q.v.y - 2.0F*q. w*q.v.z, 1.0F - 2.0F*q.v.x*q.v.x - 2.0F*q.v.z*q.v.z, 2.0F*q.v.y*q.v.z + 2.0F*q. w*q.v.x,
+ 2.0F*q.v.x*q.v.z + 2.0F*q. w*q.v.y, 2.0F*q.v.y*q.v.z - 2.0F*q. w*q.v.x, 1.0F - 2.0F*q.v.x*q.v.x - 2.0F*q.v.y*q.v.y ));
+}
+
+
+//Rotationsmatix in Euler-Winkel umwandeln
+const Vector3 Matrix3x3::CalcEulerAngles() const
+{
+ return(Vector3(-atan2(m32, m22),
+ atan2(m31, m33),
+ atan2(m12, m22)));
+}
+
+//Vector3 transformieren
+const Vector3 Matrix3x3::Transform(const Vector3 &v) const
+{
+ Vector3 vResult(v.x*m11 + v.y*m21 + v.z*m31,
+ v.x*m12 + v.y*m22 + v.z*m32,
+ v.x*m13 + v.y*m23 + v.z*m33);
+
+ return(vResult);
+}
+
+////////////////////////////////////////////////////////////
+//Arithmetik, die nicht definiert werden kann
+const Matrix3x3 operator - (const Matrix3x3 &m)
+{
+ return( Matrix3x3(-m.m11, -m.m12, -m.m13,
+ -m.m21, -m.m22, -m.m23,
+ -m.m31, -m.m32, -m.m33) );
+}
+const Matrix3x3 operator * (float f, const Matrix3x3 &m)
+{
+ return( Matrix3x3(f*m.m11, f*m.m12, f*m.m13,
+ f*m.m21, f*m.m22, f*m.m23,
+ f*m.m31, f*m.m32, f*m.m33) );
+}
+const Matrix3x3 operator / (float f, const Matrix3x3 &m)
+{
+ const float fInv= 1.0 / f;
+ return( Matrix3x3(fInv*m.m11, fInv*m.m12, fInv*m.m13,
+ fInv*m.m21, fInv*m.m22, fInv*m.m23,
+ fInv*m.m31, fInv*m.m32, fInv*m.m33) );
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Matrix3x3.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,95 @@
+#pragma once
+
+class Matrix3x3
+{
+public:
+ //Daten
+ union
+ {
+ struct
+ {
+ float m11, m12, m13,
+ m21, m22, m23,
+ m31, m32, m33;
+ };
+ float aaf[3][3];
+ float af[9];
+ };
+
+ //Konstruktoren
+ Matrix3x3();
+ Matrix3x3(float m11, float m12, float m13,
+ float m21, float m22, float m23,
+ float m31, float m32, float m33);
+ Matrix3x3(const Matrix3x3 & m);
+
+
+ //Casting
+ operator float* ();
+
+ //operator() f� komfortableren Zugriff
+ float operator()(int iRow, int iColumn) const;
+ float & operator()(int iRow, int iColumn);
+
+
+ //Zuweisungen
+ Matrix3x3 & operator = (const Matrix3x3 &m);
+ Matrix3x3 & operator += (const Matrix3x3 &m);
+ Matrix3x3 & operator -= (const Matrix3x3 &m);
+ Matrix3x3 & operator *= (const Matrix3x3 &m);
+ Matrix3x3 & operator /= (const Matrix3x3 &m);
+ Matrix3x3 & operator *= (float f);
+ Matrix3x3 & operator /= (float f);
+
+
+ //Arithmetik
+ const Matrix3x3 operator +(const Matrix3x3 &m) const;
+ const Matrix3x3 operator - (const Matrix3x3 &m) const;
+ const Matrix3x3 operator * (const Matrix3x3 &m) const;
+ const Matrix3x3 operator / (const Matrix3x3 &m) const;
+ const Matrix3x3 operator * (float f) const;
+ const Matrix3x3 operator / (float f) const;
+
+
+ //Identitaets-Matrix, Transponieren
+ static const Matrix3x3 & Identity();
+ const Matrix3x3 Transpose() const;
+
+ //Determinante, Invertieren
+ float Determinate() const;
+ const Matrix3x3 Invert() const;
+
+ //Skalierung, Achsen-Matrix
+ static const Matrix3x3 Scale(const Vector3 &v);
+ static const Matrix3x3 Axes(const Vector3 &vX, const Vector3 &vY, const Vector3 &vZ);
+
+ //Rotation
+ //Eulerwinkel
+ static const Matrix3x3 RotateX(float fAngle);
+ static const Matrix3x3 RotateY(float fAngle);
+ static const Matrix3x3 RotateZ(float fAngle);
+
+ static const Matrix3x3 RotateXYZ(const Vector3 &v);
+ static const Matrix3x3 RotateXZY(const Vector3 &v);
+ static const Matrix3x3 RotateYXZ(const Vector3 &v);
+ static const Matrix3x3 RotateYZX(const Vector3 &v);
+ static const Matrix3x3 RotateZXY(const Vector3 &v);
+ static const Matrix3x3 RotateZYX(const Vector3 &v);
+
+ //Um Achse
+ static const Matrix3x3 RotateAxis(const Vector3 &v, float fAngle);
+ //Rotation aus Quaternion
+ const Matrix3x3 RotateQuaternion(const Quaternion &q);
+
+ //Rotationsmatix in Euler-Winkel umwandeln
+ const Vector3 CalcEulerAngles() const;
+
+ //Transformation von Vector3: Da 3x3 nur Rotation/Skalierung moeglich
+ const Vector3 Transform(const Vector3 &v) const;
+};
+
+////////////////////////////////////////////////////////////
+//Arithmetik, die nicht inline definiert werden kann
+const Matrix3x3 operator - (const Matrix3x3 &m);
+const Matrix3x3 operator * (float f, const Matrix3x3 &m);
+const Matrix3x3 operator / (float f, const Matrix3x3 &m);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Quaternion.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,275 @@
+#include "GTMath.h"
+
+///////////////////////////////////////////////////////////////////
+//Konstruktoren
+Quaternion::Quaternion()
+{}
+Quaternion::Quaternion(const Quaternion& q) :
+ w(q.w),
+ v(q.v.x, q.v.y, q.v.z)
+{}
+Quaternion::Quaternion(float _w, const Vector3& _v) :
+ w(_w),
+ v(_v)
+{}
+Quaternion::Quaternion(float _w, float _x= 0.0F, float _y= 0.0F, float _z= 0.0F) :
+ w(_w),
+ v(_x, _y, _z)
+{}
+
+
+//////////////////////////////////////////////////////////////////
+//Casting
+//Direkter Zugriff auf die Daten
+Quaternion::operator float* ()
+{
+ return(&w);
+}
+
+
+///////////////////////////////////////////////////////////////////
+//Zuweisungen
+Quaternion& Quaternion::operator =(const Quaternion& q)
+{
+ w= q.w;
+ v= q.v;
+ return(*this);
+}
+Quaternion& Quaternion::operator+=(const Quaternion& q)
+{
+ w+= q.w;
+ v+= q.v;
+ return(*this);
+}
+Quaternion& Quaternion::operator-=(const Quaternion& q)
+{
+ w-= q.w;
+ v-= q.v;
+ return(*this);
+}
+Quaternion& Quaternion::operator*=(const Quaternion& q)
+{
+ const float a= (v.z - v.y) * (q.v.y - q.v.z),
+ b= ( w + v.x) * (q. w + q.v.x),
+ c= ( w - v.x) * (q.v.y + q.v.z),
+ d= (v.y + v.z) * (q. w - q.v.x),
+ e= (v.z - v.x) * (q.v.x - q.v.y),
+ f= (v.z + v.x) * (q.v.x + q.v.y),
+ g= ( w + v.y) * (q. w - q.v.z),
+ h= ( w - v.y) * (q. w + q.v.z),
+ i= f + g + h,
+ j= 0.5F * (e + i);
+
+ return(*this= Quaternion(a + j - f,
+ b + j - i,
+ c + j - h,
+ d + j - g));
+}
+Quaternion& Quaternion::operator/=(const Quaternion& q)
+{
+ return(*this*= q.Invert());
+}
+Quaternion& Quaternion::operator*=(float f)
+{
+ w*= f;
+ v*= f;
+ return(*this);
+}
+Quaternion& Quaternion::operator/=(float f)
+{
+ f= 1.0F/f;
+ w*= f;
+ v*= f;
+ return(*this);
+}
+
+////////////////////////////////////////////////////////////
+//Arithmetik
+const Quaternion Quaternion::operator+(const Quaternion& q) const
+{
+ return(Quaternion(w+q.w, v.x+q.v.x, v.y+q.v.y, v.z+q.v.z));
+}
+const Quaternion Quaternion::operator-(const Quaternion& q) const
+{
+ return(Quaternion(w-q.w, v.x-q.v.x, v.y-q.v.y, v.z-q.v.z));
+}
+const Quaternion Quaternion::operator*(const Quaternion& q) const
+{
+ const float a= (v.z - v.y) * (q.v.y - q.v.z),
+ b= ( w + v.x) * (q. w + q.v.x),
+ c= ( w - v.x) * (q.v.y + q.v.z),
+ d= (v.y + v.z) * (q. w - q.v.x),
+ e= (v.z - v.x) * (q.v.x - q.v.y),
+ f= (v.z + v.x) * (q.v.x + q.v.y),
+ g= ( w + v.y) * (q. w - q.v.z),
+ h= ( w - v.y) * (q. w + q.v.z),
+ i= f + g + h,
+ j= 0.5F * (e + i);
+
+ return(Quaternion(a + j - f,
+ b + j - i,
+ c + j - h,
+ d + j - g));
+}
+const Quaternion Quaternion::operator/(const Quaternion& q) const
+{
+ return(*this * q.Invert());
+}
+const Quaternion Quaternion::operator*(float f) const
+{
+ return(Quaternion(w*f, v*f));
+}
+const Quaternion Quaternion::operator/(float f) const
+{
+ f= 1.0F/f; return(Quaternion(w*f, v*f));
+}
+const Quaternion Quaternion::operator-() const
+{
+ return(Quaternion(-w, -v.x, -v.y, -v.z));
+}
+
+////////////////////////////////////////////////////////////
+//Identit�quaternion
+const Quaternion Quaternion::MulIdentity()
+{
+ return(Quaternion(1.0F, 0.0F, 0.0F, 0.0F));
+}
+const Quaternion Quaternion::AddIdentity()
+{
+ return(Quaternion(0.0F, 0.0F, 0.0F, 0.0F));
+}
+
+////////////////////////////////////////////////////////////
+//Betrag (=L�e), Normalisieren
+float Quaternion::Length() const
+{
+ return(sqrtf(w*w + v.x*v.x + v.y*v.y + v.z*v.z));
+}
+float Quaternion::LengthSq() const
+{
+ return(w*w + v.x*v.x + v.y*v.y + v.z*v.z);
+}
+const Quaternion Quaternion::Normalize() const
+{
+ return(*this / Length());
+}
+
+
+////////////////////////////////////////////////////////////
+//Konjugieren , Invertieren
+const Quaternion Quaternion::Conjugate() const
+{
+ return(Quaternion(w, -v.x, -v.y, -v.z));
+}
+const Quaternion Quaternion::Invert() const
+{
+ const float fLengthSqInv= 1.0F / (w*w + v.x*v.x + v.y*v.y + v.z*v.z);
+ return(Quaternion( w * fLengthSqInv,
+ -v.x * fLengthSqInv,
+ -v.y * fLengthSqInv,
+ -v.z * fLengthSqInv));
+}
+
+////////////////////////////////////////////////////////////
+//Punktprodukt berechnen
+float Quaternion::DotP(const Quaternion& q) const
+{
+ return(w*q.w + v.x*q.v.x + v.y*q.v.y + v.z*q.v.z);
+}
+
+////////////////////////////////////////////////////////////
+//Rotation von Vektoren
+const Vector3 Quaternion::RotateVector(const Vector3 &v) const
+{
+ //nach der Formel: x'= q * x * qk
+ return((*this * Quaternion(0, v) * Conjugate()).v);
+}
+
+
+////////////////////////////////////////////////////////////
+//Konstruieren
+//aus: Normale und Winkel(Radiant!)
+const Quaternion Quaternion::CreateFromAxisAndAngle(const Vector3& vAxis, float fAngle)
+{
+ fAngle*= 0.5F; //Halbieren, da immer nur die H�te des Winkels ben�t wird
+ return(Quaternion(cosf(fAngle),
+ vAxis * sinf(fAngle)));
+}
+//aus: Euler-Rotation (=3 Winkel als Vektor3)
+const Quaternion Quaternion::CreateFromEuler(Vector3 vEulerAngles)
+{
+ vEulerAngles*= 0.5F; //Halbieren, da immer nur die H�te der Winkel ben�t wird
+
+ return( Quaternion(cosf(vEulerAngles.x), sinf(vEulerAngles.x), 0.0F, 0.0F) *
+ Quaternion(cosf(vEulerAngles.y), 0.0F, sinf(vEulerAngles.y), 0.0F) *
+ Quaternion(cosf(vEulerAngles.z), 0.0F, 0.0F, sinf(vEulerAngles.z)) );
+}
+//aus: Matrix3x3
+const Quaternion Quaternion::CreateFromMatrix(const Matrix3x3 & m)
+{
+ float qw, qx, qy, qz;
+ float tr= m.m11 + m.m22 + m.m33;
+
+ if(tr > 0)
+ {
+ float S = sqrt(tr+1.0) * 2; // S=4*qw
+ qw = 0.25 * S;
+ qx = (m.m32 - m.m23) / S;
+ qy = (m.m13 - m.m31) / S;
+ qz = (m.m21 - m.m12) / S;
+ }
+ else if((m.m11 > m.m22)&(m.m11 > m.m33))
+ {
+ float S = sqrt(1.0 + m.m11 - m.m22 - m.m33) * 2; // S=4*qx
+ qw = (m.m32 - m.m23) / S;
+ qx = 0.25 * S;
+ qy = (m.m12 + m.m21) / S;
+ qz = (m.m13 + m.m31) / S;
+ }
+ else if(m.m22 > m.m33)
+ {
+ float S = sqrt(1.0 + m.m22 - m.m11 - m.m33) * 2; // S=4*qy
+ qw = (m.m13 - m.m31) / S;
+ qx = (m.m12 + m.m21) / S;
+ qy = 0.25 * S;
+ qz = (m.m23 + m.m32) / S;
+ }
+ else
+ {
+ float S = sqrt(1.0 + m.m33 - m.m11 - m.m22) * 2; // S=4*qz
+ qw = (m.m21 - m.m12) / S;
+ qx = (m.m13 + m.m31) / S;
+ qy = (m.m23 + m.m32) / S;
+ qz = 0.25 * S;
+ }
+ return(Quaternion(qw, qx, qy, qz));
+}
+
+//Quaternion in Euler-Winkel umwandeln
+const Vector3 Quaternion::CalcEulerAngles() const
+{
+ return(Vector3( asin(-2.0 * (w*v.x + v.y*v.z)),
+ atan2(-2.0 * (w*v.y - v.x*v.z), 1.0 - 2.0 * (v.y*v.y + v.x*v.x)),
+ atan2(-2.0 * (w*v.z - v.x*v.y), 1.0 - 2.0 * (v.x*v.x + v.z*v.z)) ));
+}
+
+////////////////////////////////////////////////////////////
+//Lineare Interpolation
+const Quaternion Quaternion::Lerp(Quaternion q, float f) const
+{
+ return((*this*f + q*(1.0F-f)).Normalize());
+}
+//Sphaerische lineare Interpolation
+const Quaternion Quaternion::Slerp(Quaternion q, float f) const
+{
+ //den kuerzeren weg gehen
+ if(DotP(q) < 0)
+ q= -q;
+ const float fAngle= v.Angle(q.v);
+ if(fAngle < 0.1F)
+ return((*this*f + q*(1.0F-f)).Normalize());
+
+ const float fInvSinAngle= 1.0F / sinf(fAngle);
+ return(*this * sinf( f * fAngle) * fInvSinAngle
+ + q * sinf((1.0F-f) * fAngle) * fInvSinAngle);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Quaternion.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,88 @@
+#pragma once
+
+//Forward Declaration
+class Matrix3x3;
+
+class Quaternion
+{
+public:
+ ///////////////////////////////////////////////////////////////////
+ //Variablen
+ float w;
+ Vector3 v;
+
+
+ ///////////////////////////////////////////////////////////////////
+ //Konstruktoren
+ Quaternion();
+ Quaternion(const Quaternion & q);
+ Quaternion(float _w, const Vector3 & _v);
+ Quaternion(float _w, float _x, float _y, float _z);
+
+ //////////////////////////////////////////////////////////////////
+ //Casting
+ operator float* ();
+
+ ///////////////////////////////////////////////////////////////////
+ //Zuweisungen
+ Quaternion& operator =(const Quaternion& q);
+ Quaternion& operator+=(const Quaternion& q);
+ Quaternion& operator-=(const Quaternion& q);
+ Quaternion& operator*=(const Quaternion& q);
+ Quaternion& operator/=(const Quaternion& q);
+ Quaternion& operator*=(float f);
+ Quaternion& operator/=(float f);
+
+ ////////////////////////////////////////////////////////////
+ //Arithmetik
+ const Quaternion operator+(const Quaternion& q) const;
+ const Quaternion operator-(const Quaternion& q) const;
+ const Quaternion operator*(const Quaternion& q) const;
+ const Quaternion operator/(const Quaternion& q) const;
+ const Quaternion operator*(float f) const;
+ const Quaternion operator/(float f) const;
+ const Quaternion operator-() const;
+
+ ////////////////////////////////////////////////////////////
+ //Identit�quaternion
+ static const Quaternion MulIdentity();
+ static const Quaternion AddIdentity();
+
+ ////////////////////////////////////////////////////////////
+ //Betrag (=L�e), Normalisieren
+ float Length() const;
+ float LengthSq() const;
+ const Quaternion Normalize() const;
+
+ ////////////////////////////////////////////////////////////
+ //Konjugieren, Invertieren
+ const Quaternion Conjugate() const;
+ const Quaternion Invert() const;
+
+ ////////////////////////////////////////////////////////////
+ //Punktprodukt berechnen
+ float DotP(const Quaternion& q) const;
+
+ ////////////////////////////////////////////////////////////
+ //Rotation von Vektoren
+ const Vector3 RotateVector(const Vector3 &v) const;
+
+ ////////////////////////////////////////////////////////////
+ //Konstruieren
+ //aus: Richtungsvektor und Winkel (Radiant!)
+ static const Quaternion CreateFromAxisAndAngle(const Vector3& vAxis, float fAngle);
+ //aus: Euler-Rotation (=3 Winkel als Vektor3)
+ static const Quaternion CreateFromEuler(Vector3 vEulerAngles);
+ //aus: Matrix3x3
+ static const Quaternion CreateFromMatrix(const Matrix3x3 & m);
+
+
+ //Quaternion in Euler-Winkel umwandeln
+ const Vector3 CalcEulerAngles() const;
+
+ ////////////////////////////////////////////////////////////
+ //Lineare Interpolation
+ const Quaternion Lerp(Quaternion q, float f) const;
+ //Sphaerische lineare Interpolation
+ const Quaternion Slerp(Quaternion q, float f) const;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Templates.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,22 @@
+#pragma once
+
+//Minimum
+template<typename T>
+const T & TMin(const T & A, const T & B)
+{
+ return(A < B ? A : B);
+}
+
+//Maximum
+template<typename T>
+const T & TMax(const T & A, const T & B)
+{
+ return(A > B ? A : B);
+}
+
+//Lineare Interpolation
+template<typename T, typename S>
+const T TLerp(const T & A, const T & B, const S f)
+{
+ return(A*f + B*(1.0-f));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Vector2.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,158 @@
+#include "mbed.h"
+#include "GTMath.h"
+
+//Konstruktor
+Vector2::Vector2()
+{}
+Vector2::Vector2(float all) :
+ x(all),
+ y(all)
+{}
+Vector2::Vector2(float _x, float _y) :
+ x(_x),
+ y(_y)
+{}
+Vector2::Vector2(const Vector2 & v) :
+ x(v.x),
+ y(v.y)
+{}
+
+//Casting
+Vector2::operator float* ()
+{
+ return(af);
+}
+
+//Arithmetik komponentenweise
+const Vector2 Vector2::operator +(const Vector2 & v) const
+{
+ return(Vector2(x+v.x, y+v.y));
+}
+const Vector2 Vector2::operator -(const Vector2 & v) const
+{
+ return(Vector2(x-v.x, y-v.y));
+}
+const Vector2 Vector2::operator *(const Vector2 & v) const
+{
+ return(Vector2(x*v.x, y*v.y));
+}
+const Vector2 Vector2::operator /(const Vector2 & v) const
+{
+ return(Vector2(x/v.x, y/v.y));
+}
+const Vector2 Vector2::operator *(float f) const
+{
+ return(Vector2(x*f, y*f));
+}
+const Vector2 Vector2::operator /(float f) const
+{
+ const float fi= 1.0 / f;
+ return(Vector2(x*fi, y*fi));
+}
+
+//Zuweisungsoperatoren komponentenweise
+Vector2 & Vector2::operator =(const Vector2 & v)
+{
+ x= v.x;
+ y= v.y;
+ return(*this);
+}
+Vector2 & Vector2::operator +=(const Vector2 & v)
+{
+ x+= v.x;
+ y+= v.y;
+ return(*this);
+}
+Vector2 & Vector2::operator -=(const Vector2 & v)
+{
+ x-= v.x;
+ y-= v.y;
+ return(*this);
+}
+Vector2 & Vector2::operator *=(const Vector2 & v)
+{
+ x*= v.x;
+ y*= v.y;
+ return(*this);
+}
+Vector2 & Vector2::operator /=(const Vector2 & v)
+{
+ x/= v.x;
+ y/= v.y;
+ return(*this);
+}
+Vector2 & Vector2::operator *=(float f)
+{
+ x*= f;
+ y*= f;
+ return(*this);
+}
+Vector2 & Vector2::operator /=(float f)
+{
+ const float fi= 1.0 / f;
+ x*= fi;
+ y*= fi;
+ return(*this);
+}
+
+//Vergleich
+bool Vector2::operator == (const Vector2 & v) const
+{
+ return(x==v.x && y==v.y);
+}
+bool Vector2::operator != (const Vector2 & v) const
+{
+ return(x!=v.x || y!=v.y);
+}
+
+//Min, Max
+const Vector2 Vector2::Min(const Vector2 & v) const
+{
+ return(Vector2(TMin(x, v.x), TMin(y, v.y)));
+}
+const Vector2 Vector2::Max(const Vector2 & v) const
+{
+ return(Vector2(TMax(x, v.x), TMax(y, v.y)));
+}
+
+//Skalarprodukt, Winkel
+float Vector2::DotP(const Vector2 & v) const
+{
+ return(x*v.x + y*v.y);
+}
+float Vector2::Angle(const Vector2 &v) const
+{
+ return(acosf( (x*v.x + y*v.y) /
+ sqrtf((x*x + y*y) *
+ (v.x * v.x + v.y * v.y)) ));
+}
+
+//Laenge, Quadratlaenge, Nomralisieren
+float Vector2::Length() const
+{
+ return(sqrtf(x*x + y*y));
+}
+float Vector2::LengthSq() const
+{
+ return(x*x + y*y);
+}
+const Vector2 Vector2::Normalize() const
+{
+ return(*this / sqrtf(x*x + y*y));
+}
+
+
+//Operatoren, die nicht innerhalt der Klasse definiert werden koennen
+const Vector2 operator - (const Vector2 & v)
+{
+ return(Vector2(-v.x, -v.y));
+}
+const Vector2 operator * (float f, const Vector2 & v)
+{
+ return(Vector2(f*v.x, f*v.y));
+}
+const Vector2 operator / (float f, const Vector2 & v)
+{
+ const float fi= 1.0 / f;
+ return(Vector2(fi*v.x, fi*v.y));
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Vector2.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,64 @@
+#pragma once
+
+class Vector2
+{
+public:
+ //Daten
+ union
+ {
+ struct
+ {
+ float x;
+ float y;
+ };
+ float af[2];
+ };
+
+ //Konstruktor
+ Vector2();
+ Vector2(float all);
+ Vector2(float _x, float _y);
+ Vector2(const Vector2 & v);
+
+ //Casting
+ operator float* ();
+
+ //Arithmetik komponentenweise
+ const Vector2 operator +(const Vector2 & v) const;
+ const Vector2 operator -(const Vector2 & v) const;
+ const Vector2 operator *(const Vector2 & v) const;
+ const Vector2 operator /(const Vector2 & v) const;
+ const Vector2 operator *(float f) const;
+ const Vector2 operator /(float f) const;
+
+ //Zuweisungsoperatoren komponentenweise
+ Vector2 & operator =(const Vector2 & v);
+ Vector2 & operator +=(const Vector2 & v);
+ Vector2 & operator -=(const Vector2 & v);
+ Vector2 & operator *=(const Vector2 & v);
+ Vector2 & operator /=(const Vector2 & v);
+ Vector2 & operator *=(float f);
+ Vector2 & operator /=(float f);
+
+ //Vergleich
+ bool operator == (const Vector2 & v) const;
+ bool operator != (const Vector2 & v) const;
+
+ //Min, Max
+ const Vector2 Min(const Vector2 & v) const;
+ const Vector2 Max(const Vector2 & v) const;
+
+ //Skalarprodukt, Winkel
+ float DotP(const Vector2 & v) const;
+ float Angle(const Vector2 &v) const;
+
+ //Laenge, Quadratlaenge, Nomralisieren
+ float Length() const;
+ float LengthSq() const;
+ const Vector2 Normalize() const;
+};
+
+//Operatoren, die nicht innerhalt der Klasse definiert werden koennen
+const Vector2 operator - (const Vector2 & v);
+const Vector2 operator * (float f, const Vector2 & v);
+const Vector2 operator / (float f, const Vector2 & v);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Vector3.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,172 @@
+#include "mbed.h"
+#include "GTMath.h"
+
+//Konstruktor
+Vector3::Vector3()
+{}
+Vector3::Vector3(float all) :
+ x(all),
+ y(all),
+ z(all)
+{}
+Vector3::Vector3(float _x, float _y, float _z) :
+ x(_x),
+ y(_y),
+ z(_z)
+{}
+Vector3::Vector3(const Vector3 & v) :
+ x(v.x),
+ y(v.y),
+ z(v.z)
+{}
+
+//Casting
+Vector3::operator float* ()
+{
+ return(af);
+}
+
+//Arithmetik komponentenweise
+const Vector3 Vector3::operator +(const Vector3 & v) const
+{
+ return(Vector3(x+v.x, y+v.y, z+v.z));
+}
+const Vector3 Vector3::operator -(const Vector3 & v) const
+{
+ return(Vector3(x-v.x, y-v.y, z-v.z));
+}
+const Vector3 Vector3::operator *(const Vector3 & v) const
+{
+ return(Vector3(x*v.x, y*v.y, z*v.z));
+}
+const Vector3 Vector3::operator /(const Vector3 & v) const
+{
+ return(Vector3(x/v.x, y/v.y, z/v.z));
+}
+const Vector3 Vector3::operator *(float f) const
+{
+ return(Vector3(x*f, y*f, z*f));
+}
+const Vector3 Vector3::operator /(float f) const
+{
+ const float fi= 1.0 / f;
+ return(Vector3(x*fi, y*fi, z*fi));
+}
+
+//Zuweisungsoperatoren komponentenweise
+Vector3 & Vector3::operator =(const Vector3 & v)
+{
+ x= v.x;
+ y= v.y;
+ z= v.z;
+ return(*this);
+}
+Vector3 & Vector3::operator +=(const Vector3 & v)
+{
+ x+= v.x;
+ y+= v.y;
+ z+= v.z;
+ return(*this);
+}
+Vector3 & Vector3::operator -=(const Vector3 & v)
+{
+ x-= v.x;
+ y-= v.y;
+ z-= v.z;
+ return(*this);
+}
+Vector3 & Vector3::operator *=(const Vector3 & v)
+{
+ x*= v.x;
+ y*= v.y;
+ z*= v.z;
+ return(*this);
+}
+Vector3 & Vector3::operator /=(const Vector3 & v)
+{
+ x/= v.x;
+ y/= v.y;
+ z/= v.z;
+ return(*this);
+}
+Vector3 & Vector3::operator *=(float f)
+{
+ x*= f;
+ y*= f;
+ z*= f;
+ return(*this);
+}
+Vector3 & Vector3::operator /=(float f)
+{
+ const float fi= 1.0 / f;
+ x*= fi;
+ y*= fi;
+ z*= fi;
+ return(*this);
+}
+
+//Vergleich
+bool Vector3::operator == (const Vector3 & v) const
+{
+ return(x==v.x && y==v.y && z==v.z);
+}
+bool Vector3::operator != (const Vector3 & v) const
+{
+ return(x!=v.x || y!=v.y || z!=v.z);
+}
+
+//Min, Max
+const Vector3 Vector3::Min(const Vector3 & v) const
+{
+ return(Vector3(TMin(x, v.x), TMin(y, v.y), TMin(z, v.z)));
+}
+const Vector3 Vector3::Max(const Vector3 & v) const
+{
+ return(Vector3(TMax(x, v.x), TMax(y, v.y), TMax(z, v.z)));
+}
+
+//Skalar- und Kreuzprodukt, Winkel
+float Vector3::DotP(const Vector3 & v) const
+{
+ return(x*v.x + y*v.y + z*v.z);
+}
+const Vector3 Vector3::CrossP(const Vector3 & v) const
+{
+ return(Vector3(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x));
+}
+float Vector3::Angle(const Vector3 &v) const
+{
+ return(acosf( (x*v.x + y*v.y + z*v.z) /
+ sqrtf((x*x + y*y + z*z) *
+ (v.x * v.x + v.y * v.y + v.z * v.z)) ));
+}
+
+//Laenge, Quadratlaenge, Nomralisieren
+float Vector3::Length() const
+{
+ return(sqrtf(x*x + y*y + z*z));
+}
+float Vector3::LengthSq() const
+{
+ return(x*x + y*y + z*z);
+}
+const Vector3 Vector3::Normalize() const
+{
+ return(*this / sqrtf(x*x + y*y + z*z));
+}
+
+
+//Operatoren, die nicht innerhalt der Klasse definiert werden koennen
+const Vector3 operator - (const Vector3 & v)
+{
+ return(Vector3(-v.x, -v.y, -v.z));
+}
+const Vector3 operator * (float f, const Vector3 & v)
+{
+ return(Vector3(f*v.x, f*v.y, f*v.z));
+}
+const Vector3 operator / (float f, const Vector3 & v)
+{
+ const float fi= 1.0 / f;
+ return(Vector3(fi*v.x, fi*v.y, fi*v.z));
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GTMath/Vector3.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,66 @@
+#pragma once
+
+class Vector3
+{
+public:
+ //Daten
+ union
+ {
+ struct
+ {
+ float x;
+ float y;
+ float z;
+ };
+ float af[3];
+ };
+
+ //Konstruktor
+ Vector3();
+ Vector3(float all);
+ Vector3(float _x, float _y, float _z);
+ Vector3(const Vector3 & v);
+
+ //Casting
+ operator float* ();
+
+ //Arithmetik komponentenweise
+ const Vector3 operator +(const Vector3 & v) const;
+ const Vector3 operator -(const Vector3 & v) const;
+ const Vector3 operator *(const Vector3 & v) const;
+ const Vector3 operator /(const Vector3 & v) const;
+ const Vector3 operator *(float f) const;
+ const Vector3 operator /(float f) const;
+
+ //Zuweisungsoperatoren komponentenweise
+ Vector3 & operator =(const Vector3 & v);
+ Vector3 & operator +=(const Vector3 & v);
+ Vector3 & operator -=(const Vector3 & v);
+ Vector3 & operator *=(const Vector3 & v);
+ Vector3 & operator /=(const Vector3 & v);
+ Vector3 & operator *=(float f);
+ Vector3 & operator /=(float f);
+
+ //Vergleich
+ bool operator == (const Vector3 & v) const;
+ bool operator != (const Vector3 & v) const;
+
+ //Min, Max
+ const Vector3 Min(const Vector3 & v) const;
+ const Vector3 Max(const Vector3 & v) const;
+
+ //Skalar- und Kreuzprodukt, Winkel
+ float DotP(const Vector3 & v) const;
+ const Vector3 CrossP(const Vector3 & v) const;
+ float Angle(const Vector3 &v) const;
+
+ //Laenge, Quadratlaenge, Nomralisieren
+ float Length() const;
+ float LengthSq() const;
+ const Vector3 Normalize() const;
+};
+
+//Operatoren, die nicht innerhalt der Klasse definiert werden koennen
+const Vector3 operator - (const Vector3 & v);
+const Vector3 operator * (float f, const Vector3 & v);
+const Vector3 operator / (float f, const Vector3 & v);
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883/HMC5883.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,154 @@
+#include "mbed.h"
+#include "HMC5883.h"
+#include "cmsis_os.h"
+
+#define I2CADR_W(ADR) (ADR<<1&0xFE)
+#define I2CADR_R(ADR) (ADR<<1|0x01)
+
+//Initialisieren
+HMC5883::HMC5883(I2C & I2CBus_, Timer & GlobalTime_) :
+ I2CBus(I2CBus_),
+ GlobalTime(GlobalTime_)
+
+{}
+
+void HMC5883::Init()
+{
+ //Nullsetzen
+ MeasurementError= 0;
+ AutoCalibration= 0;
+ #pragma unroll
+ for(int i= 0; i < 3; i++)
+ {
+ RawMin[i]= -400;
+ RawMax[i]= 400;
+ Offset[i]= 0;
+ Scale[i]= 1.0;
+ RawMag[i]= 0;
+ Mag[i]= 0;
+ }
+
+
+ //HMC5883 initialisieren
+ char tx[4];
+
+ //1 Sample pro Messung
+ //75Hz Output Rate
+ //Range: +- 1.3 Gauss
+ //Continuous-Measurement Mode
+ tx[0]= 0x00;
+ tx[1]= 0x78; //Configuration Register A
+ tx[2]= 0x20; //Configuration Register B
+ tx[3]= 0x00; //Mode Register
+ I2CBus.write(I2CADR_W(HMC5883_ADRESS), tx, 4);
+// osDelay(100);
+
+ // Update(5,10);
+}
+
+//Rohdaten lesen
+void HMC5883::ReadRawData()
+{
+ //Drehrate aller Axen abholen
+ char tx[1];
+ char rx[6];
+
+ tx[0]= 0x03;
+ I2CBus.write(I2CADR_W(HMC5883_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(HMC5883_ADRESS), rx, 6);
+
+ //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen
+ short r[3];
+ r[0]= rx[0]<<8|rx[1]; // X
+ r[1]= rx[4]<<8|rx[5]; // Z
+ r[2]= rx[2]<<8|rx[3]; // Y
+
+ //Grober Messfehler?
+ if(r[0] == -4096
+ || r[1] == -4096
+ || r[2] == -4096)
+ MeasurementError= 1;
+ else
+ {
+ MeasurementError= 0;
+ RawMag[0]= r[0];
+ RawMag[1]= r[1];
+ RawMag[2]= r[2];
+ }
+}
+
+
+//Update-Methode
+void HMC5883::Update(int antal_samples,int samples_delay)
+{
+ float AvgSampelMag[3]= {0.0, 0.0, 0.0};
+ int AvgSampel= 0;
+
+ while(AvgSampel <antal_samples) // Antal Samples
+ {
+ //Rohdaten lesen
+ ReadRawData();
+
+ //Durchschnitt bilden
+ AvgSampelMag[0]+= (float)RawMag[0];
+ AvgSampelMag[1]+= (float)RawMag[1];
+ AvgSampelMag[2]+= (float)RawMag[2];
+ AvgSampel+= 1.0;
+
+ osDelay(samples_delay);
+ }
+
+ if(AutoCalibration)
+ {
+ #pragma unroll
+ for(int i= 0; i < 3; i++)
+ {
+ //Neuer Min Max Wert?
+ RawMin[i]= RawMin[i] < (AvgSampelMag[i] / AvgSampel) ? RawMin[i] : (AvgSampelMag[i] / AvgSampel);
+ RawMax[i]= RawMax[i] > (AvgSampelMag[i] / AvgSampel) ? RawMax[i] : (AvgSampelMag[i] / AvgSampel);
+
+ //Scale und Offset aus gesammelten Min Max Werten berechnen
+ //Die neue Untere und obere Grenze bilden -1 und +1
+ Scale[i]= 2.0 / (float)(RawMax[i]-RawMin[i]);
+ Offset[i]= 1.0 - (float)(RawMax[i]) * Scale[i];
+ }
+ }
+
+ //Feldstaerke berechnen
+ Mag[0]= Scale[0] * (float)(AvgSampelMag[0] / AvgSampel) + Offset[0];
+ Mag[1]= Scale[1] * (float)(AvgSampelMag[1] / AvgSampel) + Offset[1];
+ Mag[2]= Scale[2] * (float)(AvgSampelMag[2] / AvgSampel) + Offset[2];
+}
+
+
+//Kalibrieren
+void HMC5883::Calibrate(const short * pRawMin, const short * pRawMax)
+{
+ #pragma unroll
+ for(int i= 0; i < 3; i++)
+ {
+ RawMin[i]= pRawMin[i];
+ RawMax[i]= pRawMax[i];
+ }
+ char AutoCalibrationBak= AutoCalibration;
+ AutoCalibration= 1;
+ Update(3,10);
+ AutoCalibration= AutoCalibrationBak;
+}
+
+void HMC5883::Calibrate(int s)
+{
+ char AutoCalibrationBak= AutoCalibration;
+ AutoCalibration= 1;
+
+ //Ende der Kalibrierung in ms Millisekunden berechnen
+ int CalibEnd= GlobalTime.read_ms() + s*1000;
+
+ while(GlobalTime.read_ms() < CalibEnd)
+ {
+ //Update erledigt alles
+ Update(3,10);
+ }
+
+ AutoCalibration= AutoCalibrationBak;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883/HMC5883.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,48 @@
+#pragma once
+
+//I2C Adresse
+#define HMC5883_ADRESS 0x1E
+
+
+class HMC5883
+{
+private:
+ I2C & I2CBus;
+ Timer & GlobalTime;
+
+public:
+ //Calibration
+ char AutoCalibration; //automatische Kalibrierung der Sensordaten
+ short RawMin[3], RawMax[3]; //gespeicherte Daten fuer die Auto-Kalibrierung
+ float Scale[3]; //jede Achse einzeln skalieren
+ float Offset[3]; //zum Schluss noch das Offset dazu
+
+ //Feldstaerke auf allen drei Achsen
+ short RawMag[3]; //Rohdaten
+ float Mag[3]; //kalibrierte Rohdaten (nicht normalisiert)
+
+ //Bei zu hoher Feldstaerke wird -4096 gelesen
+ //Wenn dies eine Achse betreffen sollte, ist diese Variable 1
+ short MeasurementError;
+ float AvgSampelMag[3];
+
+
+ //Initialisieren
+ HMC5883(I2C & I2CBus_, Timer & GlobalTime_);
+ void Init();
+
+private:
+ //Rohdaten lesen
+ void ReadRawData();
+ int AvgSampel;
+public:
+ //Update-Methode
+ void Update(int antal_samples,int samples_delay);
+
+ //Kalibrieren
+ //Fertige Daten benutzen. Scale[0] muss 1.0 sein!
+ void Calibrate(const short * pRawMin, const short * pRawMax);
+
+ //Selbst auswerten, dauert s Sekunden lang
+ void Calibrate(int s);
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter/IMUfilter.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,248 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+/**
+ * Includes
+ */
+#include "IMUfilter.h"
+
+IMUfilter::IMUfilter(Timer & GlobalTime): GlobalTime(GlobalTime){
+
+ firstUpdate = 0;
+
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ AEq_1 = 1;
+ AEq_2 = 0;
+ AEq_3 = 0;
+ AEq_4 = 0;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ SEq_1 = 1;
+ SEq_2 = 0;
+ SEq_3 = 0;
+ SEq_4 = 0;
+
+ //Sampling period (typical value is ~0.1s).
+ deltat =0.1;
+
+ //Gyroscope measurement error (in degrees per second).
+ gyroMeasError = 0;
+
+ //Compute beta.
+ beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
+
+}
+
+void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
+
+ //Local system variables.
+
+ //Vector norm.
+ double norm;
+ //Quaternion rate from gyroscope elements.
+ double SEqDot_omega_1;
+ double SEqDot_omega_2;
+ double SEqDot_omega_3;
+ double SEqDot_omega_4;
+ //Objective function elements.
+ double f_1;
+ double f_2;
+ double f_3;
+ //Objective function Jacobian elements.
+ double J_11or24;
+ double J_12or23;
+ double J_13or22;
+ double J_14or21;
+ double J_32;
+ double J_33;
+ //Objective function gradient elements.
+ double nablaf_1;
+ double nablaf_2;
+ double nablaf_3;
+ double nablaf_4;
+
+ //Auxiliary variables to avoid reapeated calculations.
+ double halfSEq_1 = 0.5 * SEq_1;
+ double halfSEq_2 = 0.5 * SEq_2;
+ double halfSEq_3 = 0.5 * SEq_3;
+ double halfSEq_4 = 0.5 * SEq_4;
+ double twoSEq_1 = 2.0 * SEq_1;
+ double twoSEq_2 = 2.0 * SEq_2;
+ double twoSEq_3 = 2.0 * SEq_3;
+
+//Time Step (=Zeitdifferenz berechnen)
+ static int LastFrame= GlobalTime.read_us();
+ int Now= GlobalTime.read_us();
+ deltat= 0.000001 * (float)(Now - LastFrame);
+ LastFrame= Now;
+
+ //Compute the quaternion rate measured by gyroscopes.
+ SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
+ SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
+ SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
+ SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
+
+ //Normalise the accelerometer measurement.
+ norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
+ a_x /= norm;
+ a_y /= norm;
+ a_z /= norm;
+
+ //Compute the objective function and Jacobian.
+ f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
+ f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
+ f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
+ //J_11 negated in matrix multiplication.
+ J_11or24 = twoSEq_3;
+ J_12or23 = 2 * SEq_4;
+ //J_12 negated in matrix multiplication
+ J_13or22 = twoSEq_1;
+ J_14or21 = twoSEq_2;
+ //Negated in matrix multiplication.
+ J_32 = 2 * J_14or21;
+ //Negated in matrix multiplication.
+ J_33 = 2 * J_11or24;
+
+ //Compute the gradient (matrix multiplication).
+ nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
+ nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
+ nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
+ nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
+
+ //Normalise the gradient.
+ norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
+ nablaf_1 /= norm;
+ nablaf_2 /= norm;
+ nablaf_3 /= norm;
+ nablaf_4 /= norm;
+
+ //Compute then integrate the estimated quaternion rate.
+ SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
+ SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
+ SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
+ SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
+
+ //Normalise quaternion
+ norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
+ SEq_1 /= norm;
+ SEq_2 /= norm;
+ SEq_3 /= norm;
+ SEq_4 /= norm;
+
+ if (firstUpdate == 0) {
+ //Store orientation of auxiliary frame.
+ AEq_1 = SEq_1;
+ AEq_2 = SEq_2;
+ AEq_3 = SEq_3;
+ AEq_4 = SEq_4;
+ firstUpdate = 1;
+ }
+
+}
+
+void IMUfilter::computeEuler(void){
+
+ //Quaternion describing orientation of sensor relative to earth.
+ double ESq_1, ESq_2, ESq_3, ESq_4;
+ //Quaternion describing orientation of sensor relative to auxiliary frame.
+ double ASq_1, ASq_2, ASq_3, ASq_4;
+
+ //Compute the quaternion conjugate.
+ ESq_1 = SEq_1;
+ ESq_2 = -SEq_2;
+ ESq_3 = -SEq_3;
+ ESq_4 = -SEq_4;
+
+ //Compute the quaternion product.
+ ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
+ ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
+ ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
+ ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
+
+ //Compute the Euler angles from the quaternion.
+ phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
+ theta = asin (2 * ASq_2 * ASq_4 - 2 * ASq_1 * ASq_3);
+ psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
+ //asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3); With theta = asin(2 * ASq_2 * ASq_4 - 2 * ASq_1 * ASq_3);
+}
+
+double IMUfilter::getRoll(void){
+
+ return phi;
+
+}
+
+double IMUfilter::getPitch(void){
+
+ return theta;
+
+}
+
+double IMUfilter::getYaw(void){
+
+ return psi;
+
+}
+
+void IMUfilter::setGyroError(double gyroscopeMeasurementError)
+ {
+ gyroMeasError = gyroscopeMeasurementError;
+ beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
+ // reset();
+ }
+
+double IMUfilter::getGyroError(void)
+ {
+ return gyroMeasError;
+ }
+
+void IMUfilter::reset(void) {
+
+ firstUpdate = 0;
+
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ AEq_1 = 1;
+ AEq_2 = 0;
+ AEq_3 = 0;
+ AEq_4 = 0;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ SEq_1 = 1;
+ SEq_2 = 0;
+ SEq_3 = 0;
+ SEq_4 = 0;
+
+ //Compute beta.
+ beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter/IMUfilter.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,156 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+#ifndef IMU_FILTER_H
+#define IMU_FILTER_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PI 3.1415926536
+
+/**
+ * IMU orientation filter.
+ */
+class IMUfilter {
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * Initializes filter variables.
+ *
+ * @param rate The rate at which the filter should be updated.
+ * @param gyroscopeMeasurementError The error of the gyroscope in degrees
+ * per second. This used to calculate a tuning constant for the filter.
+ * Try changing this value if there are jittery readings, or they change
+ * too much or too fast when rotating the IMU.
+ */
+ IMUfilter(Timer & GlobalTime_);
+
+ /**
+ * Update the filter variables.
+ *
+ * @param w_x X-axis gyroscope reading in rad/s.
+ * @param w_y Y-axis gyroscope reading in rad/s.
+ * @param w_z Z-axis gyroscope reading in rad/s.
+ * @param a_x X-axis accelerometer reading in m/s/s.
+ * @param a_y Y-axis accelerometer reading in m/s/s.
+ * @param a_z Z-axis accelerometer reading in m/s/s.
+ */
+ void updateFilter(double w_x, double w_y, double w_z,
+ double a_x, double a_y, double a_z);
+
+ /**
+ * Compute the Euler angles based on the current filter data.
+ */
+ void computeEuler(void);
+
+ /**
+ * Get the current roll.
+ *
+ * @return The current roll angle in radians.
+ */
+ double getRoll(void);
+
+ /**
+ * Get the current pitch.
+ *
+ * @return The current pitch angle in radians.
+ */
+ double getPitch(void);
+
+ /**
+ * Get the current yaw.
+ *
+ * @return The current yaw angle in radians.
+ */
+ double getYaw(void);
+
+ /**
+ * Get the current Gyro error rate.
+ *
+ * @return The Gyro error rate setting.
+ */
+ double getGyroError(void);
+
+ /**
+ * Set the Gyro error rate.
+ * The filter resets during calling this routine.
+ */
+ void setGyroError(double gyroscopeMeasurementError);
+
+ /**
+ * Reset the filter.
+ */
+ void reset(void);
+
+private:
+
+ int firstUpdate;
+ Timer & GlobalTime;
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ double AEq_1;
+ double AEq_2;
+ double AEq_3;
+ double AEq_4;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ double SEq_1;
+ double SEq_2;
+ double SEq_3;
+ double SEq_4;
+
+ //Sampling period
+ double deltat;
+
+ //Gyroscope measurement error (in degrees per second).
+ double gyroMeasError;
+
+ //Compute beta (filter tuning constant..
+ double beta;
+
+ double phi;
+ double theta;
+ double psi;
+
+};
+
+#endif /* IMU_FILTER_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200/ITG3200.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,130 @@
+#include "mbed.h"
+#include "ITG3200.h"
+#include "cmsis_os.h"
+#define I2CADR_W(ADR) (ADR<<1&0xFE)
+#define I2CADR_R(ADR) (ADR<<1|0x01)
+
+//Initialisieren
+ITG3200::ITG3200(I2C & I2CBus_, Timer & GlobalTime_)
+ : I2CBus(I2CBus_),
+ GlobalTime(GlobalTime_)
+{}
+
+void ITG3200::Init()
+{
+ //Nullsetzen
+ for(int i= 0; i < 3; i++)
+ {
+ RawRate[i]= 0;
+ Offset[i]= 0.0;
+ Rate[i]= 0.0;
+ }
+
+
+ //ITG3200 initialisieren
+ char tx[2];
+
+ //Full-Scale (2000deg/sec) nutzen
+ //Digitalen Low Pass Filter auf 10 Hz setzen
+ //Intern mit 1 kHz sampeln
+ tx[0]= 0x16;
+ tx[1]= 0x1D;
+ I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
+
+ //Einen internen Taktgeber verwenden (X-Gyro)
+ tx[0]= 0x3E;
+ tx[1]= 0x01;
+ I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 2);
+
+ //kurz warten
+ osDelay(100);
+
+ //erste Werte abholen
+ Update(3,2);
+}
+
+
+//Rohdaten lesen
+void ITG3200::ReadRawData()
+{
+ //Drehrate aller Axen abholen
+ char tx[1];
+ char rx[6];
+
+ tx[0]= 0x1D;
+ I2CBus.write(I2CADR_W(ITG3200_ADRESS), tx, 1);
+ I2CBus.read (I2CADR_R(ITG3200_ADRESS), rx, 6);
+
+ //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen
+ RawRate[0]= rx[0]<<8|rx[1]; // x
+ RawRate[1]= rx[2]<<8|rx[3]; // y
+ RawRate[2]= rx[4]<<8|rx[5]; // z
+}
+
+
+//Update-Methode
+void ITG3200::Update(int antal_samples,int samples_delay)
+{
+ float AvgSampelAcc[3]= {0.0, 0.0, 0.0};
+ int AvgSampel= 0;
+
+ while(AvgSampel <antal_samples) // Antal Samples
+ {
+ //Rohdaten lesen
+ ReadRawData();
+
+ //Durchschnitt bilden
+ AvgSampelAcc[0]+= (float)RawRate[0];
+ AvgSampelAcc[1]+= (float)RawRate[1];
+ AvgSampelAcc[2]+= (float)RawRate[2];
+ AvgSampel+= 1.0;
+
+ osDelay(samples_delay);
+ }
+
+
+ //Beschleunigung für alle 3 Achsen auslesen
+
+
+ //Drehrate berechnen
+ Rate[0]= fConvRPS * ((float)(AvgSampelAcc[0] / AvgSampel) + Offset[0]);
+ Rate[1]= fConvRPS * ((float)(AvgSampelAcc[1] / AvgSampel) + Offset[1]);
+ Rate[2]= fConvRPS * ((float)(AvgSampelAcc[2] / AvgSampel) + Offset[2]);
+}
+
+
+
+
+
+
+
+
+
+//Kalibrieren
+void ITG3200::Calibrate(int ms)
+{
+ float AvgCalibRate[3]= {0.0, 0.0, 0.0};
+ float AvgCalibSampels= 0.0;
+
+ //Ende der Kalibrierung in ms Millisekunden berechnen
+ int CalibEnd= GlobalTime.read_ms() + ms;
+
+ while(GlobalTime.read_ms() < CalibEnd)
+ {
+ //Rohdaten lesen
+ ReadRawData();
+
+ //Durchschnitt bilden
+ AvgCalibRate[0]+= (float)RawRate[0];
+ AvgCalibRate[1]+= (float)RawRate[1];
+ AvgCalibRate[2]+= (float)RawRate[2];
+ AvgCalibSampels+= 1.0;
+
+ osDelay(2);
+ }
+
+ //Genug Daten gesammelt, jetzt den Durchschnitt bilden
+ Offset[0]= -(AvgCalibRate[0] / AvgCalibSampels);
+ Offset[1]= -(AvgCalibRate[1] / AvgCalibSampels);
+ Offset[2]= -(AvgCalibRate[2] / AvgCalibSampels);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200/ITG3200.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,39 @@
+#pragma once
+
+//I2C Adresse, entweder 0x68 oder 0x69, abhaengig von Pin AD0
+#define ITG3200_ADRESS 0x68
+
+//Gyro-Rohdaten in Radiant pro Sekunde umrechnen
+const float fConvRPS= 1.2141420883e-3;
+
+class ITG3200
+{
+protected:
+ I2C & I2CBus;
+ Timer & GlobalTime;
+
+public:
+ //Offset
+ float Offset[3];
+
+ //Drehgeschwindigkeit um alle drei Achsen
+ short RawRate[3]; //Rohdaten
+ float Rate[3]; //kalibrierte Drehraten in Radiant pro Sekunde
+
+
+ //Initialisieren
+ ITG3200(I2C & I2CBus_, Timer & GlobalTime_);
+ void Init();
+
+
+private:
+ //Rohdaten lesen
+ void ReadRawData();
+
+public:
+ //Update-Methode
+ void Update(int antal_samples,int samples_delay);
+
+ //Kalibrieren
+ void Calibrate(int ms);
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MARGfilter/MARGfilter.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,304 @@
+/**
+ * @author David X. Elrom
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Adapted from IMU filter library
+ * IMU orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+/**
+ * Includes
+ */
+#include "MARGfilter.h"
+
+MARGfilter::MARGfilter(Timer & GlobalTime): GlobalTime(GlobalTime){
+/*
+ firstUpdate = 0;
+ printf("\nmagfilter run \n");
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ AEq_1 = 1;
+ AEq_2 = 0;
+ AEq_3 = 0;
+ AEq_4 = 0;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ SEq_1 = 1;
+ SEq_2 = 0;
+ SEq_3 = 0;
+ SEq_4 = 0;
+
+ //Sampling period (typical value is ~0.1s).
+ deltat = rate;
+
+ //Gyroscope measurement error (in degrees per second).
+ gyroMeasError = gyroscopeMeasurementError;
+ gyroMeasDrift = gyroscopeMeasurementDrift;
+
+ b_x = 1;
+ b_z = 0;
+
+ w_bx = 0;
+ w_by = 0;
+ w_bz = 0;
+
+ //Compute beta.
+ beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
+ zeta = sqrt(3.0 / 4.0) * (PI * (gyroMeasDrift / 180.0));
+*/
+}
+
+void MARGfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z) {
+ //m_x=0.0;m_y=0.0;m_z=0.0;
+
+ // local system variables
+ double norm; // vector norm
+ double SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements
+ double f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements
+ double J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements
+ J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
+ double SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
+ double w_err_x, w_err_y, w_err_z; // estimated direction of the gyroscope error (angular)
+ double h_x, h_y, h_z; // computed flux in the earth frame
+ // axulirary variables to avoid reapeated calcualtions
+ double halfSEq_1 = 0.5 * SEq_1;
+ double halfSEq_2 = 0.5 * SEq_2;
+ double halfSEq_3 = 0.5 * SEq_3;
+ double halfSEq_4 = 0.5 * SEq_4;
+ double twoSEq_1 = 2.0 * SEq_1;
+ double twoSEq_2 = 2.0 * SEq_2;
+ double twoSEq_3 = 2.0 * SEq_3;
+ double twoSEq_4 = 2.0 * SEq_4;
+ double twob_x = 2.0 * b_x;
+ double twob_z = 2.0 * b_z;
+ double twob_xSEq_1 = 2.0 * b_x * SEq_1;
+ double twob_xSEq_2 = 2.0 * b_x * SEq_2;
+ double twob_xSEq_3 = 2.0 * b_x * SEq_3;
+ double twob_xSEq_4 = 2.0 * b_x * SEq_4;
+ double twob_zSEq_1 = 2.0 * b_z * SEq_1;
+ double twob_zSEq_2 = 2.0* b_z * SEq_2;
+ double twob_zSEq_3 = 2.0 * b_z * SEq_3;
+ double twob_zSEq_4 = 2.0 * b_z * SEq_4;
+ double SEq_1SEq_2;
+ double SEq_1SEq_3 = SEq_1 * SEq_3;
+ double SEq_1SEq_4;
+ double SEq_2SEq_3;
+ double SEq_2SEq_4 = SEq_2 * SEq_4;
+ double SEq_3SEq_4;
+ double twom_x = 2.0 * m_x;
+ double twom_y = 2.0 * m_y;
+ double twom_z = 2.0 * m_z;
+
+ //Time Step (=Zeitdifferenz berechnen)
+ static int LastFrame= GlobalTime.read_us();
+ int Now= GlobalTime.read_us();
+ deltat= 0.000001 * (float)(Now - LastFrame);
+ LastFrame= Now;
+
+ // normalise the accelerometer measurement
+ norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
+ a_x /= norm;
+ a_y /= norm;
+ a_z /= norm;
+ // normalise the magnetometer measurement
+ norm = sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
+ m_x /= norm;
+ m_y /= norm;
+ m_z /= norm;
+ // compute the objective function and Jacobian
+ f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
+ f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
+ f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
+ f_4 = twob_x * (0.5 - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - m_x;
+ f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - m_y;
+ f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5 - SEq_2 * SEq_2 - SEq_3 * SEq_3) - m_z;
+ J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
+ J_12or23 = 2.0 * SEq_4;
+ J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
+ J_14or21 = twoSEq_2;
+ J_32 = 2.0 * J_14or21; // negated in matrix multiplication
+ J_33 = 2.0 * J_11or24; // negated in matrix multiplication
+ J_41 = twob_zSEq_3; // negated in matrix multiplication
+ J_42 = twob_zSEq_4;
+ J_43 = 2.0 * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication
+ J_44 = 2.0 * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
+ J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
+ J_52 = twob_xSEq_3 + twob_zSEq_1;
+ J_53 = twob_xSEq_2 + twob_zSEq_4;
+ J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication
+ J_61 = twob_xSEq_3;
+ J_62 = twob_xSEq_4 - 2.0 * twob_zSEq_2;
+ J_63 = twob_xSEq_1 - 2.0 * twob_zSEq_3;
+ J_64 = twob_xSEq_2;
+ // compute the gradient (matrix multiplication)
+ SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
+ SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3 + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
+ SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
+ SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
+ // normalise the gradient to estimate direction of the gyroscope error
+ norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
+ SEqHatDot_1 = SEqHatDot_1 / norm;
+ SEqHatDot_2 = SEqHatDot_2 / norm;
+ SEqHatDot_3 = SEqHatDot_3 / norm;
+ SEqHatDot_4 = SEqHatDot_4 / norm;
+ // compute angular estimated direction of the gyroscope error
+ w_err_x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
+ w_err_y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
+ w_err_z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
+ // compute and remove the gyroscope baises
+ w_bx += w_err_x * deltat * zeta;
+ w_by += w_err_y * deltat * zeta;
+ w_bz += w_err_z * deltat * zeta;
+ w_x -= w_bx;
+ w_y -= w_by;
+ w_z -= w_bz;
+ // compute the quaternion rate measured by gyroscopes
+ SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
+ SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
+ SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
+ SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
+ // compute then integrate the estimated quaternion rate
+ SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
+ SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
+ SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
+ SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
+ // normalise quaternion
+ norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
+ SEq_1 /= norm;
+ SEq_2 /= norm;
+ SEq_3 /= norm;
+ SEq_4 /= norm;
+ // compute flux in the earth frame
+ SEq_1SEq_2 = SEq_1 * SEq_2; // recompute axulirary variables
+ SEq_1SEq_3 = SEq_1 * SEq_3;
+ SEq_1SEq_4 = SEq_1 * SEq_4;
+ SEq_3SEq_4 = SEq_3 * SEq_4;
+ SEq_2SEq_3 = SEq_2 * SEq_3;
+ SEq_2SEq_4 = SEq_2 * SEq_4;
+ h_x = twom_x * (0.5 - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom_y * (SEq_2SEq_3 - SEq_1SEq_4) + twom_z * (SEq_2SEq_4 + SEq_1SEq_3);
+ h_y = twom_x * (SEq_2SEq_3 + SEq_1SEq_4) + twom_y * (0.5 - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom_z * (SEq_3SEq_4 - SEq_1SEq_2);
+ h_z = twom_x * (SEq_2SEq_4 - SEq_1SEq_3) + twom_y * (SEq_3SEq_4 + SEq_1SEq_2) + twom_z * (0.5 - SEq_2 * SEq_2 - SEq_3 * SEq_3);
+ // normalise the flux vector to have only components in the x and z
+ b_x = sqrt((h_x * h_x) + (h_y * h_y));
+ b_z = h_z;
+
+ if (firstUpdate == 0) {
+ //Store orientation of auxiliary frame.
+ AEq_1 = SEq_1;
+ AEq_2 = SEq_2;
+ AEq_3 = SEq_3;
+ AEq_4 = SEq_4;
+ firstUpdate = 1;
+ }
+
+}
+
+void MARGfilter::computeEuler(void){
+
+ //Quaternion describing orientation of sensor relative to earth.
+ double ESq_1, ESq_2, ESq_3, ESq_4;
+ //Quaternion describing orientation of sensor relative to auxiliary frame.
+ double ASq_1, ASq_2, ASq_3, ASq_4;
+
+ //Compute the quaternion conjugate.
+ ESq_1 = SEq_1;
+ ESq_2 = -SEq_2;
+ ESq_3 = -SEq_3;
+ ESq_4 = -SEq_4;
+
+ //Compute the quaternion product.
+ ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
+ ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
+ ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
+ ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
+
+ //Compute the Euler angles from the quaternion.
+ phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
+ theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
+ psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
+
+}
+
+double MARGfilter::getRoll(void){
+
+ return phi;
+
+}
+
+double MARGfilter::getPitch(void){
+
+ return theta;
+
+}
+
+double MARGfilter::getYaw(void){
+
+ return psi;
+
+}
+
+void MARGfilter::reset(double gyroscopeMeasurementError, double gyroscopeMeasurementDrift) {
+
+ firstUpdate = 0;
+
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ AEq_1 = 1;
+ AEq_2 = 0;
+ AEq_3 = 0;
+ AEq_4 = 0;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ SEq_1 = 1;
+ SEq_2 = 0;
+ SEq_3 = 0;
+ SEq_4 = 0;
+
+ //Sampling period (typical value is ~0.1s).
+ // deltat = rate;
+
+ //Gyroscope measurement error (in degrees per second).
+ gyroMeasError = gyroscopeMeasurementError;
+ gyroMeasDrift = gyroscopeMeasurementDrift;
+
+ b_x = 1;
+ b_z = 0;
+
+ w_bx = 0;
+ w_by = 0;
+ w_bz = 0;
+
+ //Compute beta.
+ beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
+ zeta = sqrt(3.0 / 4.0) * (PI * (gyroMeasDrift / 180.0));
+
+
+
+
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MARGfilter/MARGfilter.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,163 @@
+/**
+ * @author David X. Elrom
+ *
+ *
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Adapted from mbed IMU filter library
+ * IMU orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+#ifndef MARG_FILTER_H
+#define MARG_FILTER_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "Parameter.h"
+/**
+ * Defines
+ */
+#define PI 3.1415926536
+
+/**
+ * MARG orientation filter.
+ */
+class MARGfilter {
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * Initializes filter variables.
+ *
+ * @param rate The rate at which the filter should be updated.
+ * @param gyroscopeMeasurementError The error of the gyroscope in degrees
+ * per second. This used to calculate a tuning constant for the filter.
+ * Try changing this value if there are jittery readings, or they change
+ * too much or too fast when rotating the IMU.
+ */
+ MARGfilter(Timer & GlobalTime_);
+
+ /**
+ * Update the filter variables.
+ *
+ * @param w_x X-axis gyroscope reading in rad/s.
+ * @param w_y Y-axis gyroscope reading in rad/s.
+ * @param w_z Z-axis gyroscope reading in rad/s.
+ * @param a_x X-axis accelerometer reading in m/s/s.
+ * @param a_y Y-axis accelerometer reading in m/s/s.
+ * @param a_z Z-axis accelerometer reading in m/s/s.
+ * @param m_x X-axis magnetometer reading in not too sure just yet.
+ * @param m_y Y-axis magnetometer reading in not too sure just yet.
+ * @param m_z Z-axis magnetometer reading in not too sure just yet.
+ */
+ void updateFilter(double w_x, double w_y, double w_z,
+ double a_x, double a_y, double a_z,
+ double m_x, double m_y, double m_z);
+
+ /**
+ * Compute the Euler angles based on the current filter data.
+ */
+ void computeEuler(void);
+
+ /**
+ * Get the current roll.
+ *
+ * @return The current roll angle in radians.
+ */
+ double getRoll(void);
+
+ /**
+ * Get the current pitch.
+ *
+ * @return The current pitch angle in radians.
+ */
+ double getPitch(void);
+
+ /**
+ * Get the current yaw.
+ *
+ * @return The current yaw angle in radians.
+ */
+ double getYaw(void);
+
+ /**
+ * Reset the filter.
+ */
+ void reset(double gyroscopeMeasurementError, double gyroscopeMeasurementDrift);
+ //Compute beta (filter tuning constant..
+ double beta;
+
+ //Compute zeta (filter tuning constant..
+ double zeta;
+private:
+
+ int firstUpdate;
+ Timer & GlobalTime;
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ double AEq_1;
+ double AEq_2;
+ double AEq_3;
+ double AEq_4;
+
+ //Estimated orientation quaternion elements with initial conditions.
+ double SEq_1;
+ double SEq_2;
+ double SEq_3;
+ double SEq_4;
+
+ // reference direction of flux in earth frame
+ double b_z;
+ double b_x;
+
+ //Sampling period
+ double deltat;
+
+ //gyroscope biasses
+ double w_bx;
+ double w_by;
+ double w_bz;
+
+ //Gyroscope measurement error (in degrees per second).
+ double gyroMeasError;
+
+
+ double gyroMeasDrift;
+
+ double phi;
+ double theta;
+ double psi;
+
+};
+
+#endif /* MARG_FILTER_H */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Parameter/Parameter.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,182 @@
+#include "mbed.h"
+#include "cmsis_os.h"
+#include "ConfigFile.h"
+#include "Parameter.h"
+
+//Filesystem.
+ LocalFileSystem local("local");
+
+
+ ConfigFile cfg; // configfile lib
+// Loader alle paramert from local/input.cfg
+
+ Parameter::Parameter(Timer & GlobalTime_) : GlobalTime(GlobalTime_) {
+
+ }
+
+
+ float RATE; //PID loop timing
+
+//PID tuning constants.
+ float L_KC;//Forward left motor Kc
+ float L_TI; //Forward left motor Ti
+ float L_TD; //Forward left motor Td
+ float R_KC; //Forward right motor Kc
+ float R_TI; //Forward right motor Ti
+ float R_TD; //Forward right motor Td
+
+//PID input/output limits.
+ int L_INPUT_MIN; //Forward left motor minimum input
+ int L_INPUT_MAX; //Forward left motor maximum input
+ float L_OUTPUT_MIN; //Forward left motor minimum output
+ float L_OUTPUT_MAX; //Forward left motor maximum output
+
+ int R_INPUT_MIN; //Forward right motor input minimum
+ int R_INPUT_MAX; //Forward right motor input maximum
+ float R_OUTPUT_MIN; //Forward right motor output minimum
+ float R_OUTPUT_MAX; //Forward right motor output maximum
+
+//Physical dimensions.
+ float PULSES_PER_REV; //(59 pulses pr motor omgang gear 50:1)
+ float WHEEL_DIAMETER; //mm
+ float ROTATION_DISTANCE; //mm
+ float REVS_PER_ROTATION; // (ROTATION_DISTANCE / WHEEL_DIAMETER);
+ float PULSES_PER_ROTATION;// (REVS_PER_ROTATION * PULSES_PER_REV);
+ float PULSES_PER_MM;// (PULSES_PER_REV / WHEEL_DIAMETER);
+ float DISTANCE_PER_PULSE;// (WHEEL_DIAMETER / PULSES_PER_REV);
+ float _WHEEL_DISTANCE; // (ROTATION_DISTANCE / DISTANCE_PER_PULSE);
+ float GYRO_MEAS_ERROR;
+ float GYRO_MEAS_DRIFT;
+ int CALIBRATION_SAMPLES;
+ int SAMPLES;
+ float IMU_RATE; // 0.025
+ float COMMAND_RATE; //0.005
+ float PID_RATE;// 0.005
+ float SENSOR_RATE;// 0.005
+ float GYROSCOPE_GAIN; // (1 / 14.375)
+ float ACCELEROMETER_GAIN;// 3.4346447e-3; //(0.0034346447 * g0) //0.0034346447; //Beschleunigung in Meter pro Quadratsekunde umrechnen
+ float COMPASS_GAIN;
+
+void Parameter::Loadparm(void) {
+
+ char *key1 ="RATE"; //ATE 0.01 //PID loop timing
+ char *key2 ="L_KC"; // 0.4312 //Forward left motor Kc
+ char *key3 ="L_TI"; // 0.1 //Forward left motor Ti
+ char *key4 ="L_TD"; // 0.0 //Forward left motor Td
+ char *key5 ="R_KC"; // 0.4620 //Forward right motor Kc
+ char *key6 ="R_TI"; // 0.1 //Forward right motor Ti
+ char *key7 ="R_TD"; // 0.0 //Forward right motor Td
+
+ char *key8 ="L_INPUT_MIN"; // 0 //Forward left motor minimum input
+ char *key9 ="L_INPUT_MAX"; // 3000 //Forward left motor maximum input
+ char *key10="L_OUTPUT_MIN";// 0.0 //Forward left motor minimum output
+ char *key11="L_OUTPUT_MAX"; // 0.9 //Forward left motor maximum output
+
+ char *key12="R_INPUT_MIN"; // 0 //Forward right motor input minimum
+ char *key13="R_INPUT_MAX"; // 3200 //Forward right motor input maximum
+ char *key14="R_OUTPUT_MIN"; // 0.0 //Forward right motor output minimum
+ char *key15="R_OUTPUT_MAX"; // 0.9 //Forward right motor output maximum
+ char *key16="PULSES_PER_REV"; // 120*50 //(59 pulses pr motor omgang gear 50:1)
+ char *key17="WHEEL_DIAMETER"; // 127.32 //mm
+ char *key18="ROTATION_DISTANCE"; // 400.0 //mm
+ char *key19="_WHEEL_DISTANCE"; // (ROTATION_DISTANCE / DISTANCE_PER_PULSE);
+ char *key20="GYRO_MEAS_ERROR";
+ char *key21="GYRO_MEAS_DRIFT";
+ char *key22="CALIBRATION_SAMPLES";
+ char *key23="SAMPLES";
+ char *key24="IMU_RATE"; // 0.025
+ char *key25="COMMAND_RATE"; //0.005
+ char *key26="PID_RATE"; // 0.005
+ char *key27="SENSOR_RATE";// 0.005
+ char *key28="GYROSCOPE_GAIN";// (1 / 14.375)
+ char *key29="ACCELEROMETER_GAIN";// 3.4346447e-3; //(0.0034346447 * g0) //0.0034346447; //Beschleunigung in Meter pro Quadratsekunde umrechnen
+ char *key30="COMPASS_GAIN";
+ char value[BUFSIZ];
+ /*
+ * Read a configuration file from a mbed.
+ */
+ // printf("indlaeser parameter liste\n");
+
+ if (!cfg.read("/local/input.cfg")) {
+ printf("Failure to read /local/input.cfg configuration file.system stoppet!!! \n");
+ exit(EXIT_FAILURE);
+ }
+
+
+ // Get a configuration value.
+
+ if (cfg.getValue(key1 , &value[0], sizeof(value))) RATE=atof(value);
+ if (cfg.getValue(key2 , &value[0], sizeof(value))) L_KC=atof(value);
+ if (cfg.getValue(key3 , &value[0], sizeof(value))) L_TI=atof(value);
+ if (cfg.getValue(key4 , &value[0], sizeof(value))) L_TD=atof(value);
+ if (cfg.getValue(key5 , &value[0], sizeof(value))) R_KC=atof(value);
+ if (cfg.getValue(key6 , &value[0], sizeof(value))) R_TI=atof(value);
+ if (cfg.getValue(key7 , &value[0], sizeof(value))) R_TD=atof(value);
+ if (cfg.getValue(key8 , &value[0], sizeof(value))) L_INPUT_MIN=atoi(value);
+ if (cfg.getValue(key9 , &value[0], sizeof(value))) L_INPUT_MIN=atoi(value);
+ if (cfg.getValue(key10, &value[0], sizeof(value))) L_OUTPUT_MIN=atof(value);
+ if (cfg.getValue(key11, &value[0], sizeof(value))) L_OUTPUT_MAX=atof(value);
+ if (cfg.getValue(key12, &value[0], sizeof(value))) R_INPUT_MIN=atoi(value);
+ if (cfg.getValue(key13, &value[0], sizeof(value))) R_INPUT_MAX=atoi(value);
+ if (cfg.getValue(key14, &value[0], sizeof(value))) R_OUTPUT_MIN=atof(value);
+ if (cfg.getValue(key15, &value[0], sizeof(value))) R_OUTPUT_MAX=atof(value);
+ if (cfg.getValue(key16, &value[0], sizeof(value))) PULSES_PER_REV=atof(value);
+ if (cfg.getValue(key17, &value[0], sizeof(value))) WHEEL_DIAMETER=atof(value);
+ if (cfg.getValue(key18, &value[0], sizeof(value))) ROTATION_DISTANCE=atof(value);
+ if (cfg.getValue(key19, &value[0], sizeof(value))) _WHEEL_DISTANCE=atof(value); // (ROTATION_DISTANCE / DISTANCE_PER_PULSE);
+ if (cfg.getValue(key20, &value[0], sizeof(value))) GYRO_MEAS_ERROR=atof(value);
+ if (cfg.getValue(key21, &value[0], sizeof(value))) GYRO_MEAS_DRIFT=atof(value);
+ if (cfg.getValue(key22, &value[0], sizeof(value))) CALIBRATION_SAMPLES=atof(value);
+ if (cfg.getValue(key23, &value[0], sizeof(value))) SAMPLES=atof(value);
+ if (cfg.getValue(key24, &value[0], sizeof(value))) IMU_RATE=atof(value); // 0.025
+ if (cfg.getValue(key25, &value[0], sizeof(value))) COMMAND_RATE=atof(value); //0.005
+ if (cfg.getValue(key26, &value[0], sizeof(value))) PID_RATE=atof(value); // 0.005
+ if (cfg.getValue(key27, &value[0], sizeof(value))) SENSOR_RATE=atof(value); // 0.005
+ if (cfg.getValue(key28, &value[0], sizeof(value))) GYROSCOPE_GAIN=atof(value); // (1 / 14.375)
+ if (cfg.getValue(key29, &value[0], sizeof(value))) ACCELEROMETER_GAIN=atof(value);// 3.4346447e-3; //(0.0034346447 * g0) //0.0034346447; //Beschleunigung in Meter pro Quadratsekunde umrechnen
+ if (cfg.getValue(key30, &value[0], sizeof(value))) COMPASS_GAIN=atof(value);
+
+
+ // printf("Parameter indlaesning udfort. \n");
+
+ }
+///////////////////////////////////////////////////
+/* input.cfg
+RATE=5
+L_KC=0.4312
+L_TI=0.1
+L_TD=0.0
+R_KC=0.4620
+R_TI=0.1
+R_TD=0.0
+L_INPUT_MIN=0
+L_INPUT_MAX=3000
+L_OUTPUT_MIN=0.0
+L_OUTPUT_MAX=0.9
+R_INPUT_MIN=0
+R_INPUT_MAX=3200
+R_OUTPUT_MIN=0.0
+R_OUTPUT_MAX=0.9
+PULSES_PER_REV=6000
+WHEEL_DIAMETER=127.32
+ROTATION_DISTANCE=400.0
+_WHEEL_DISTANCE=0
+GYRO_MEAS_ERROR=-0.0002
+CALIBRATION_SAMPLES=250
+SAMPLES=4
+IMU_RATE=10
+ACCELEROMETER_RATE=5
+ACCELEROMETER_GAIN=3.4346447e-3
+COMPASS_GAIN=1
+
+COMMAND_RATE=1000
+SENSOR_RATE=10
+PID_RATE=500
+*/
+
+
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Parameter/Parameter.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,64 @@
+#ifndef MBED_PARAMETER_H
+#define MBED_PARAMETER_H
+#include "mbed.h"
+
+
+class Parameter {
+public:
+Parameter( Timer & GlobalTime_);
+
+ void flash(int n);
+
+ float RATE; //PID loop timing
+
+//PID tuning constants.
+ float L_KC;//Forward left motor Kc
+ float L_TI; //Forward left motor Ti
+ float L_TD; //Forward left motor Td
+ float R_KC; //Forward right motor Kc
+ float R_TI; //Forward right motor Ti
+ float R_TD; //Forward right motor Td
+
+//PID input/output limits.
+ int L_INPUT_MIN; //Forward left motor minimum input
+ int L_INPUT_MAX; //Forward left motor maximum input
+ float L_OUTPUT_MIN; //Forward left motor minimum output
+ float L_OUTPUT_MAX; //Forward left motor maximum output
+
+ int R_INPUT_MIN; //Forward right motor input minimum
+ int R_INPUT_MAX; //Forward right motor input maximum
+ float R_OUTPUT_MIN; //Forward right motor output minimum
+ float R_OUTPUT_MAX; //Forward right motor output maximum
+
+//Physical dimensions.
+ float PULSES_PER_REV; //(59 pulses pr motor omgang gear 50:1)
+ float WHEEL_DIAMETER; //mm
+ float ROTATION_DISTANCE; //mm
+ float REVS_PER_ROTATION; // (ROTATION_DISTANCE / WHEEL_DIAMETER);
+ float PULSES_PER_ROTATION;// (REVS_PER_ROTATION * PULSES_PER_REV);
+ float PULSES_PER_MM;// (PULSES_PER_REV / WHEEL_DIAMETER);
+ float DISTANCE_PER_PULSE;// (WHEEL_DIAMETER / PULSES_PER_REV);
+ float _WHEEL_DISTANCE; // (ROTATION_DISTANCE / DISTANCE_PER_PULSE);
+ float GYRO_MEAS_ERROR;
+ float GYRO_MEAS_DRIFT;
+ int CALIBRATION_SAMPLES;
+ int SAMPLES;
+ float IMU_RATE; // 0.025
+ float COMMAND_RATE; //0.005
+ float PID_RATE;// 0.005
+ float SENSOR_RATE;// 0.005
+ float GYROSCOPE_GAIN; // (1 / 14.375)
+ float ACCELEROMETER_GAIN;// 3.4346447e-3; //(0.0034346447 * g0) //0.0034346447; //Beschleunigung in Meter pro Quadratsekunde umrechnen
+ float COMPASS_GAIN;
+
+ void Loadparm();
+
+
+private:
+ // DigitalOut _pin;
+ Timer & GlobalTime;
+
+
+
+};
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "cmsis_os.h"
+#include "AHRS.h"
+#include "Parameter.h"
+//#include "MARGfilter.h"
+//#include "IMUfilter.h"
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+Serial pc(USBTX, USBRX);
+
+I2C I2CBus(p28, p27);
+Timer GlobalTime;
+
+AHRS Ahrs(I2CBus,GlobalTime);
+Parameter Parm(GlobalTime);
+//MARGfilter MargFilter (GlobalTime);
+//IMUfilter IMUfilter(GlobalTime);
+#define PI 3.1415926536
+#define toDegrees(x) (x * 57.2957795)//Multiply radians to get degrees.
+#define toRadians(x) (x * 0.01745329252)//Multiply degrees to get radians.
+//#define toAcceleration(x) (x * (3 * g0 * 0.001)) //Full scale resolution on the ADXL345 is 3mg/LSB.
+
+
+// Sensor task..og sensor opdatering **********************
+void Sensor_thread(void const *argument)
+{
+ // Ahrs.Mag.AutoCalibration= 1; // In Echtzeit kalibrieren
+ Ahrs.Init();
+ Ahrs.Gyro.Calibrate(500); // 0.5 sec Calibrer gyro
+ short Raw1g[3]= {0, 0, -2870}; // -2870 //1 g sind ca -2870 auf der 3. Achse
+ Ahrs.Acc.Calibrate(500, Raw1g); // 0.5 Sekunden kalibrieren
+ Ahrs.Mag.AutoCalibration= 1; // In Echtzeit kalibrieren
+ // Ahrs.Mag.Update(10,10);
+
+ while(1) //Sensoren updaten loop
+ {
+ Ahrs.Acc.Update(8,2);
+ led1 = !led1;
+ Ahrs.Mag.Update(4,2);
+ led1 = !led1;
+ Ahrs.Gyro.Update(4,2);
+ led1 = !led1;
+ }
+}
+
+
+// IMU task *****************************
+void IMU_thread(void const *argument) {
+osDelay(5000);
+//MargFilter.reset(Parm.GYRO_MEAS_ERROR,Parm.GYRO_MEAS_DRIFT);
+//IMUfilter.setGyroError(Parm.GYRO_MEAS_ERROR);
+ while(1) // MargFilter loop
+ {
+ // MargFilter.updateFilter (-Ahrs.Gyro.Rate[0],Ahrs.Gyro.Rate[1],Ahrs.Gyro.Rate[2],
+ // Ahrs.Acc.Acc[0] ,Ahrs.Acc.Acc[1] ,Ahrs.Acc.Acc[2],
+ // Ahrs.Mag.Mag[0] ,Ahrs.Mag.Mag[1] ,Ahrs.Mag.Mag[2]);
+
+ // IMUfilter.updateFilter (Ahrs.Gyro.Rate[1],Ahrs.Gyro.Rate[0],Ahrs.Gyro.Rate[2],
+ // Ahrs.Acc.Acc[0] ,Ahrs.Acc.Acc[1] ,Ahrs.Acc.Acc[2]);
+
+ // MargFilter.computeEuler ();
+ // IMUfilter.computeEuler();
+ Ahrs.Update();
+ led2 = !led2;
+ osDelay(Parm.IMU_RATE);
+ }
+}
+
+
+// Parameter task *****************************
+void Parmeter_thread(void const *argument) {
+
+ while(1)
+ {
+ Ahrs.Beta = sqrt(3.0 / 4.0) * (PI * (Parm.GYRO_MEAS_ERROR / 180.0));// gyroscope measurement error in rad/s (shown as 5 deg/s)
+ // Ahrs.zeta = sqrt(3.0 / 4.0) * (PI * (Parm.GYRO_MEAS_DRIFT / 180.0)); // gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s)
+// IMUfilter.setGyroError(Parm.GYRO_MEAS_ERROR);
+ led3 = !led3;
+ osDelay(10000); // load parm pr 10 sec
+ Parm.Loadparm();
+ }
+
+}
+
+
+
+
+osThreadDef(Sensor_thread, osPriorityHigh, 1, DEFAULT_STACK_SIZE);
+osThreadDef(IMU_thread, osPriorityNormal, 1, DEFAULT_STACK_SIZE);
+osThreadDef(Parmeter_thread, osPriorityLow, 1, DEFAULT_STACK_SIZE* 2.25);
+
+// MAIN ********************************
+
+int main() {
+ I2CBus.frequency(400000);
+ GlobalTime.start();
+ Parm.Loadparm();
+ pc.baud(9600);
+ osThreadCreate(osThread(Sensor_thread), NULL);
+ osThreadCreate(osThread(IMU_thread), NULL);
+ osThreadCreate(osThread(Parmeter_thread), NULL);
+ osDelay(2000);
+
+ while(1)
+ {
+
+
+ // pc.printf("%f,%f,%f\r\n",toDegrees(Ahrs.Roll),toDegrees(Ahrs.Nick),toDegrees(Ahrs.Yaw));
+ // pc.printf("%f,%f,%f\n",toDegrees(IMUfilter.getRoll()),toDegrees(IMUfilter.getPitch()),toDegrees(IMUfilter.getYaw()));
+ // pc.printf("%f,%f,%f\n",Ahrs.Mag.Mag[0],Ahrs.Mag.Mag[1],Ahrs.Mag.Mag[2]);
+ // pc.printf("%f,%f,%f\n",Ahrs.Acc.Acc[0],Ahrs.Acc.Acc[1],Ahrs.Acc.Acc[2]);
+ // pc.printf("%f,%f,%f\n",Ahrs.Gyro.Rate[0],Ahrs.Gyro.Rate[1],Ahrs.Gyro.Rate[2]);
+
+ pc.printf("%f,%f,%f\n",(toDegrees(Ahrs.Roll)),toDegrees((Ahrs.Nick)),toDegrees((Ahrs.Yaw)));
+
+ osDelay(100);
+ }
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jun 11 12:02:30 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479
Binary file rtos/LPC11U24/uARM/rtos.ar has changed
Binary file rtos/LPC11U24/uARM/rtx.ar has changed
Binary file rtos/LPC1768/ARM/rtos.ar has changed
Binary file rtos/LPC1768/ARM/rtx.ar has changed
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/Mail.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,89 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef MAIL_H
+#define MAIL_H
+
+#include <stdint.h>
+#include <string.h>
+
+#include "cmsis_os.h"
+
+namespace rtos {
+
+/*! The Mail class allow to control, send, receive, or wait for mail.
+ A mail is a memory block that is send to a thread or interrupt service routine.
+ \tparam T data type of a single message element.
+ \tparam queue_sz maximum number of messages in queue.
+*/
+template<typename T, uint32_t queue_sz>
+class Mail {
+public:
+ /*! Create and Initialise Mail queue. */
+ Mail() {
+ #ifdef CMSIS_OS_RTX
+ memset(_mail_q, 0, sizeof(_mail_q));
+ _mail_p[0] = _mail_q;
+
+ memset(_mail_m, 0, sizeof(_mail_m));
+ _mail_p[1] = _mail_m;
+
+ _mail_def.pool = _mail_p;
+ _mail_def.queue_sz = queue_sz;
+ _mail_def.item_sz = sizeof(T);
+ #endif
+ _mail_id = osMailCreate(&_mail_def, NULL);
+ }
+
+ /*! Allocate a memory block of type T
+ \param millisec timeout value or 0 in case of no time-out. (default: 0).
+ \return pointer to memory block that can be filled with mail or NULL in case error.
+ */
+ T* alloc(uint32_t millisec=0) {
+ return (T*)osMailAlloc(_mail_id, millisec);
+ }
+
+ /*! Allocate a memory block of type T and set memory block to zero.
+ \param millisec timeout value or 0 in case of no time-out. (default: 0).
+ \return pointer to memory block that can be filled with mail or NULL in case error.
+ */
+ T* calloc(uint32_t millisec=0) {
+ return (T*)osMailCAlloc(_mail_id, millisec);
+ }
+
+ /*! Put a mail in the queue.
+ \param mptr memory block previously allocated with Mail::alloc or Mail::calloc.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus put(T *mptr) {
+ return osMailPut(_mail_id, (void*)mptr);
+ }
+
+ /*! Get a mail from a queue.
+ \param millisec timeout value or 0 in case of no time-out. (default: osWaitForever).
+ \return event that contains mail information or error code.
+ */
+ osEvent get(uint32_t millisec=osWaitForever) {
+ return osMailGet(_mail_id, millisec);
+ }
+
+ /*! Free a memory block from a mail.
+ \param mptr pointer to the memory block that was obtained with Mail::get.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus free(T *mptr) {
+ return osMailFree(_mail_id, (void*)mptr);
+ }
+
+private:
+ osMailQId _mail_id;
+ osMailQDef_t _mail_def;
+#ifdef CMSIS_OS_RTX
+ uint32_t _mail_q[4+(queue_sz)];
+ uint32_t _mail_m[3+((sizeof(T)+3)/4)*(queue_sz)];
+ void *_mail_p[2];
+#endif
+};
+
+}
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/MemoryPool.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,62 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef MEMORYPOOL_H
+#define MEMORYPOOL_H
+
+#include <stdint.h>
+#include <string.h>
+
+#include "cmsis_os.h"
+
+namespace rtos {
+
+/*! Define and manage fixed-size memory pools of objects of a given type.
+ \tparam T data type of a single object (element).
+ \tparam queue_sz maximum number of objects (elements) in the memory pool.
+*/
+template<typename T, uint32_t pool_sz>
+class MemoryPool {
+public:
+ /*! Create and Initialize a memory pool. */
+ MemoryPool() {
+ #ifdef CMSIS_OS_RTX
+ memset(_pool_m, 0, sizeof(_pool_m));
+ _pool_def.pool = _pool_m;
+
+ _pool_def.pool_sz = pool_sz;
+ _pool_def.item_sz = sizeof(T);
+ #endif
+ _pool_id = osPoolCreate(&_pool_def);
+ }
+
+ /*! Allocate a memory block of type T from a memory pool.
+ \return address of the allocated memory block or NULL in case of no memory available.
+ */
+ T* alloc(void) {
+ return (T*)osPoolAlloc(_pool_id);
+ }
+
+ /*! Allocate a memory block of type T from a memory pool and set memory block to zero.
+ \return address of the allocated memory block or NULL in case of no memory available.
+ */
+ T* calloc(void) {
+ return (T*)osPoolCAlloc(_pool_id);
+ }
+
+ /*! Return an allocated memory block back to a specific memory pool.
+ \param address of the allocated memory block that is returned to the memory pool.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus free(T *block) {
+ return osPoolFree(_pool_id, (void*)block);
+ }
+
+private:
+ osPoolId _pool_id;
+ osPoolDef_t _pool_def;
+#ifdef CMSIS_OS_RTX
+ uint32_t _pool_m[3+((sizeof(T)+3)/4)*(pool_sz)];
+#endif
+};
+
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/Mutex.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,43 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef MUTEX_H
+#define MUTEX_H
+
+#include <stdint.h>
+#include "cmsis_os.h"
+
+namespace rtos {
+
+/*! The Mutex class is used to synchronise the execution of threads.
+ This is for example used to protect access to a shared resource.
+*/
+class Mutex {
+public:
+ /*! Create and Initialize a Mutex object */
+ Mutex();
+
+ /*! Wait until a Mutex becomes available.
+ \param millisec timeout value or 0 in case of no time-out. (default: osWaitForever)
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus lock(uint32_t millisec=osWaitForever);
+
+ /*! Try to lock the mutex, and return immediately
+ \return true if the mutex was acquired, false otherwise.
+ */
+ bool trylock();
+
+ /*! Unlock the mutex that has previously been locked by the same thread
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus unlock();
+
+private:
+ osMutexId _osMutexId;
+ osMutexDef_t _osMutexDef;
+#ifdef CMSIS_OS_RTX
+ int32_t _mutex_data[3];
+#endif
+};
+
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/Queue.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,61 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef QUEUE_H
+#define QUEUE_H
+
+#include <stdint.h>
+#include <string.h>
+
+#include "cmsis_os.h"
+#include "error.h"
+
+namespace rtos {
+
+/*! The Queue class allow to control, send, receive, or wait for messages.
+ A message can be a integer or pointer value to a certain type T that is send
+ to a thread or interrupt service routine.
+ \tparam T data type of a single message element.
+ \tparam queue_sz maximum number of messages in queue.
+*/
+template<typename T, uint32_t queue_sz>
+class Queue {
+public:
+ /*! Create and initialise a message Queue. */
+ Queue() {
+ #ifdef CMSIS_OS_RTX
+ memset(_queue_q, 0, sizeof(_queue_q));
+ _queue_def.pool = _queue_q;
+ _queue_def.queue_sz = queue_sz;
+ #endif
+ _queue_id = osMessageCreate(&_queue_def, NULL);
+ if (_queue_id == NULL) {
+ error("Error initialising the queue object\n");
+ }
+ }
+
+ /*! Put a message in a Queue.
+ \param data message pointer.
+ \param millisec timeout value or 0 in case of no time-out. (default: 0)
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus put(T* data, uint32_t millisec=0) {
+ return osMessagePut(_queue_id, (uint32_t)data, millisec);
+ }
+
+ /*! Get a message or Wait for a message from a Queue.
+ \param millisec timeout value or 0 in case of no time-out. (default: osWaitForever).
+ \return event information that includes the message and the status code.
+ */
+ osEvent get(uint32_t millisec=osWaitForever) {
+ return osMessageGet(_queue_id, millisec);
+ }
+
+private:
+ osMessageQId _queue_id;
+ osMessageQDef_t _queue_def;
+#ifdef CMSIS_OS_RTX
+ uint32_t _queue_q[4+(queue_sz)];
+#endif
+};
+
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/RTX_CM_lib.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,391 @@
+/*----------------------------------------------------------------------------
+ * RL-ARM - RTX
+ *----------------------------------------------------------------------------
+ * Name: RTX_CM_LIB.H
+ * Purpose: RTX Kernel System Configuration
+ * Rev.: V4.21
+ *----------------------------------------------------------------------------
+ *
+ * Copyright (c) 1999-2009 KEIL, 2009-2012 ARM Germany GmbH
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * - Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * - Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *---------------------------------------------------------------------------*/
+
+#if defined (__CC_ARM)
+#pragma O3
+#define __USED __attribute__((used))
+#elif defined (__GNUC__)
+#pragma GCC optimize ("O3")
+#define __USED __attribute__((used))
+#elif defined (__ICCARM__)
+#define __USED __root
+#endif
+
+
+/*----------------------------------------------------------------------------
+ * Definitions
+ *---------------------------------------------------------------------------*/
+
+#define _declare_box(pool,size,cnt) uint32_t pool[(((size)+3)/4)*(cnt) + 3]
+#define _declare_box8(pool,size,cnt) uint64_t pool[(((size)+7)/8)*(cnt) + 2]
+
+#define OS_TCB_SIZE 48
+#define OS_TMR_SIZE 8
+
+#if defined (__CC_ARM) && !defined (__MICROLIB)
+
+typedef void *OS_ID;
+typedef uint32_t OS_TID;
+typedef uint32_t OS_MUT[3];
+typedef uint32_t OS_RESULT;
+
+#define runtask_id() rt_tsk_self()
+#define mutex_init(m) rt_mut_init(m)
+#define mutex_wait(m) os_mut_wait(m,0xFFFF)
+#define mutex_rel(m) os_mut_release(m)
+
+extern OS_TID rt_tsk_self (void);
+extern void rt_mut_init (OS_ID mutex);
+extern OS_RESULT rt_mut_release (OS_ID mutex);
+extern OS_RESULT rt_mut_wait (OS_ID mutex, uint16_t timeout);
+
+#define os_mut_wait(mutex,timeout) _os_mut_wait((uint32_t)rt_mut_wait,mutex,timeout)
+#define os_mut_release(mutex) _os_mut_release((uint32_t)rt_mut_release,mutex)
+
+OS_RESULT _os_mut_release (uint32_t p, OS_ID mutex) __svc_indirect(0);
+OS_RESULT _os_mut_wait (uint32_t p, OS_ID mutex, uint16_t timeout) __svc_indirect(0);
+
+#endif
+
+
+/*----------------------------------------------------------------------------
+ * Global Variables
+ *---------------------------------------------------------------------------*/
+
+#if (OS_TIMERS != 0)
+#define OS_TASK_CNT (OS_TASKCNT + 1)
+#define OS_PRIV_CNT (OS_PRIVCNT + 2)
+#define OS_STACK_SZ (4*(OS_PRIVSTKSIZE+OS_MAINSTKSIZE+OS_TIMERSTKSZ))
+#else
+#define OS_TASK_CNT OS_TASKCNT
+#define OS_PRIV_CNT (OS_PRIVCNT + 1)
+#define OS_STACK_SZ (4*(OS_PRIVSTKSIZE+OS_MAINSTKSIZE))
+#endif
+
+extern uint16_t const os_maxtaskrun = OS_TASK_CNT;
+extern uint32_t const os_stackinfo = (OS_STKCHECK<<24)| (OS_PRIV_CNT<<16) | (OS_STKSIZE*4);
+extern uint32_t const os_rrobin = (OS_ROBIN << 16) | OS_ROBINTOUT;
+extern uint32_t const os_trv = OS_TRV;
+extern uint8_t const os_flags = OS_RUNPRIV;
+
+/* Export following defines to uVision debugger. */
+extern __USED uint32_t const os_clockrate = OS_TICK;
+extern __USED uint32_t const os_timernum = 0;
+
+/* Memory pool for TCB allocation */
+_declare_box (mp_tcb, OS_TCB_SIZE, OS_TASK_CNT);
+extern uint16_t const mp_tcb_size = sizeof(mp_tcb);
+
+/* Memory pool for System stack allocation (+os_idle_demon). */
+_declare_box8 (mp_stk, OS_STKSIZE*4, OS_TASK_CNT-OS_PRIV_CNT+1);
+extern uint32_t const mp_stk_size = sizeof(mp_stk);
+
+/* Memory pool for user specified stack allocation (+main, +timer) */
+uint64_t os_stack_mem[2+OS_PRIV_CNT+(OS_STACK_SZ/8)];
+extern uint32_t const os_stack_sz = sizeof(os_stack_mem);
+
+#ifndef OS_FIFOSZ
+ #define OS_FIFOSZ 16
+#endif
+
+/* Fifo Queue buffer for ISR requests.*/
+uint32_t os_fifo[OS_FIFOSZ*2+1];
+extern uint8_t const os_fifo_size = OS_FIFOSZ;
+
+/* An array of Active task pointers. */
+void *os_active_TCB[OS_TASK_CNT];
+
+/* User Timers Resources */
+#if (OS_TIMERS != 0)
+extern void osTimerThread (void const *argument);
+osThreadDef(osTimerThread, (osPriority)(OS_TIMERPRIO-3), 1, 4*OS_TIMERSTKSZ);
+osThreadId osThreadId_osTimerThread;
+osMessageQDef(osTimerMessageQ, OS_TIMERCBQS, void *);
+osMessageQId osMessageQId_osTimerMessageQ;
+#else
+osThreadDef_t os_thread_def_osTimerThread = { NULL };
+osThreadId osThreadId_osTimerThread;
+osMessageQDef(osTimerMessageQ, 0, void *);
+osMessageQId osMessageQId_osTimerMessageQ;
+#endif
+
+/* Legacy RTX User Timers not used */
+uint32_t os_tmr = 0;
+extern uint32_t const *m_tmr = NULL;
+extern uint16_t const mp_tmr_size = 0;
+
+#if defined (__CC_ARM) && !defined (__MICROLIB)
+ /* A memory space for arm standard library. */
+ static uint32_t std_libspace[OS_TASK_CNT][96/4];
+ static OS_MUT std_libmutex[OS_MUTEXCNT];
+ static uint32_t nr_mutex;
+ extern void *__libspace_start;
+#endif
+
+
+/*----------------------------------------------------------------------------
+ * RTX Optimizations (empty functions)
+ *---------------------------------------------------------------------------*/
+
+#if OS_ROBIN == 0
+ void rt_init_robin (void) {;}
+ void rt_chk_robin (void) {;}
+#endif
+
+#if OS_STKCHECK == 0
+ void rt_stk_check (void) {;}
+#endif
+
+
+/*----------------------------------------------------------------------------
+ * Standard Library multithreading interface
+ *---------------------------------------------------------------------------*/
+
+#if defined (__CC_ARM) && !defined (__MICROLIB)
+
+/*--------------------------- __user_perthread_libspace ---------------------*/
+
+void *__user_perthread_libspace (void) {
+ /* Provide a separate libspace for each task. */
+ uint32_t idx;
+
+ idx = runtask_id ();
+ if (idx == 0) {
+ /* RTX not running yet. */
+ return (&__libspace_start);
+ }
+ return ((void *)&std_libspace[idx-1]);
+}
+
+/*--------------------------- _mutex_initialize -----------------------------*/
+
+int _mutex_initialize (OS_ID *mutex) {
+ /* Allocate and initialize a system mutex. */
+
+ if (nr_mutex >= OS_MUTEXCNT) {
+ /* If you are here, you need to increase the number OS_MUTEXCNT. */
+ for (;;);
+ }
+ *mutex = &std_libmutex[nr_mutex++];
+ mutex_init (*mutex);
+ return (1);
+}
+
+
+/*--------------------------- _mutex_acquire --------------------------------*/
+
+__attribute__((used)) void _mutex_acquire (OS_ID *mutex) {
+ /* Acquire a system mutex, lock stdlib resources. */
+ if (runtask_id ()) {
+ /* RTX running, acquire a mutex. */
+ mutex_wait (*mutex);
+ }
+}
+
+
+/*--------------------------- _mutex_release --------------------------------*/
+
+__attribute__((used)) void _mutex_release (OS_ID *mutex) {
+ /* Release a system mutex, unlock stdlib resources. */
+ if (runtask_id ()) {
+ /* RTX runnning, release a mutex. */
+ mutex_rel (*mutex);
+ }
+}
+
+#endif
+
+
+/*----------------------------------------------------------------------------
+ * RTX Startup
+ *---------------------------------------------------------------------------*/
+
+/* Main Thread definition */
+extern int main (void);
+osThreadDef_t os_thread_def_main = {(os_pthread)main, osPriorityNormal, 1, 4*OS_MAINSTKSIZE };
+
+
+#if defined (__CC_ARM)
+
+typedef void PROC();
+
+extern const size_t SHT$$INIT_ARRAY$$Base[];
+extern const size_t SHT$$INIT_ARRAY$$Limit[];
+
+void cpp_init(void) {
+ const size_t *base = SHT$$INIT_ARRAY$$Base;
+ const size_t *lim = SHT$$INIT_ARRAY$$Limit;
+
+ for (; base != lim; ++base) {
+ PROC *proc = (PROC*)((const char*)base + *base);
+ (*proc)();
+ }
+}
+
+// void $Super$$__cpp_initialize__aeabi_(void);
+void $Sub$$__cpp_initialize__aeabi_(void) {}
+
+void $Super$$main(void);
+void $Sub$$main(void) {
+ // $Super$$__cpp_initialize__aeabi_();
+ cpp_init();
+
+ $Super$$main();
+}
+
+#ifdef __MICROLIB
+void _main_init (void) __attribute__((section(".ARM.Collect$$$$000000FF")));
+void _main_init (void) {
+ osKernelStart(&os_thread_def_main, NULL);
+ for (;;);
+}
+#else
+__asm void __rt_entry (void) {
+
+ IMPORT __user_setup_stackheap
+ IMPORT __rt_lib_init
+ IMPORT os_thread_def_main
+ IMPORT osKernelStart
+ IMPORT exit
+
+ BL __user_setup_stackheap
+ MOV R1,R2
+ BL __rt_lib_init
+ LDR R0,=os_thread_def_main
+ MOVS R1,#0
+ BL osKernelStart
+ BL exit
+
+ ALIGN
+}
+#endif
+
+#elif defined (__GNUC__)
+
+#ifdef __CS3__
+
+/* CS3 start_c routine.
+ *
+ * Copyright (c) 2006, 2007 CodeSourcery Inc
+ *
+ * The authors hereby grant permission to use, copy, modify, distribute,
+ * and license this software and its documentation for any purpose, provided
+ * that existing copyright notices are retained in all copies and that this
+ * notice is included verbatim in any distributions. No written agreement,
+ * license, or royalty fee is required for any of the authorized uses.
+ * Modifications to this software may be copyrighted by their authors
+ * and need not follow the licensing terms described here, provided that
+ * the new terms are clearly indicated on the first page of each file where
+ * they apply.
+ */
+
+#include "cs3.h"
+
+extern void __libc_init_array (void);
+
+__attribute ((noreturn)) void __cs3_start_c (void){
+ unsigned regions = __cs3_region_num;
+ const struct __cs3_region *rptr = __cs3_regions;
+
+ /* Initialize memory */
+ for (regions = __cs3_region_num, rptr = __cs3_regions; regions--; rptr++) {
+ long long *src = (long long *)rptr->init;
+ long long *dst = (long long *)rptr->data;
+ unsigned limit = rptr->init_size;
+ unsigned count;
+
+ if (src != dst)
+ for (count = 0; count != limit; count += sizeof (long long))
+ *dst++ = *src++;
+ else
+ dst = (long long *)((char *)dst + limit);
+ limit = rptr->zero_size;
+ for (count = 0; count != limit; count += sizeof (long long))
+ *dst++ = 0;
+ }
+
+ /* Run initializers. */
+ __libc_init_array ();
+
+ osKernelStart(&os_thread_def_main, NULL);
+ for (;;);
+}
+
+#else
+
+__attribute__((naked)) void software_init_hook (void) {
+ __asm (
+ ".syntax unified\n"
+ ".thumb\n"
+ "movs r0,#0\n"
+ "movs r1,#0\n"
+ "mov r4,r0\n"
+ "mov r5,r1\n"
+ "ldr r0,= __libc_fini_array\n"
+ "bl atexit\n"
+ "bl __libc_init_array\n"
+ "mov r0,r4\n"
+ "mov r1,r5\n"
+ "ldr r0,=os_thread_def_main\n"
+ "movs r1,#0\n"
+ "bl osKernelStart\n"
+ "bl exit\n"
+ );
+}
+
+#endif
+
+#elif defined (__ICCARM__)
+
+extern int __low_level_init(void);
+extern void __iar_data_init3(void);
+extern void exit(int arg);
+
+__noreturn __stackless void __cmain(void) {
+ int a;
+
+ if (__low_level_init() != 0) {
+ __iar_data_init3();
+ }
+ a = osKernelStart(&os_thread_def_main, NULL);
+ exit(a);
+}
+
+#endif
+
+
+/*----------------------------------------------------------------------------
+ * end of file
+ *---------------------------------------------------------------------------*/
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/RTX_Conf_CM.c Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,266 @@
+/*----------------------------------------------------------------------------
+ * RL-ARM - RTX
+ *----------------------------------------------------------------------------
+ * Name: RTX_Conf_CM.C
+ * Purpose: Configuration of CMSIS RTX Kernel for Cortex-M
+ * Rev.: V4.20
+ *----------------------------------------------------------------------------
+ *
+ * Copyright (c) 1999-2009 KEIL, 2009-2012 ARM Germany GmbH
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * - Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * - Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *---------------------------------------------------------------------------*/
+
+#include "cmsis_os.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*----------------------------------------------------------------------------
+ * RTX User configuration part BEGIN
+ *---------------------------------------------------------------------------*/
+
+//-------- <<< Use Configuration Wizard in Context Menu >>> -----------------
+//
+// <h>Thread Configuration
+// =======================
+// <o>Number of concurrent running threads <0-250>
+// <i> Defines max. number of threads that will run at the same time.
+// counting "main", but not counting "osTimerThread"
+// <i> Default: 6
+#ifndef OS_TASKCNT
+# if defined(TARGET_LPC1768) || defined(TARGET_LPC2368)
+# define OS_TASKCNT 7
+# elif defined(TARGET_LPC11U24)
+# define OS_TASKCNT 3
+# endif
+#endif
+
+// <o>Number of threads with user-provided stack size <0-250>
+// The stack of "main" and "osTimerThread" are calculated separately
+// <i> Defines the number of threads with user-provided stack size.
+// <i> Default: 0
+#ifndef OS_PRIVCNT
+ #define OS_PRIVCNT (OS_TASKCNT - 1)
+#endif
+
+// <o>Default Thread stack size [bytes] <64-4096:8><#/4>
+// <i> Defines default stack size for threads.
+// <i> Default: 200
+#ifndef OS_STKSIZE
+ #define OS_STKSIZE WORDS_STACK_SIZE
+#endif
+
+// <o>Main Thread stack size [bytes] <64-4096:8><#/4>
+// <i> Defines stack size for main thread.
+// <i> Default: 200
+#ifndef OS_MAINSTKSIZE
+//Donatien: default allocated stack size is a bit too low
+ #define OS_MAINSTKSIZE 512
+#endif
+
+// <o>Total stack size [bytes] for threads with user-provided stack size <0-4096:8><#/4>
+// <i> Defines the combined stack size for threads with user-provided stack size.
+// <i> Default: 0
+#ifndef OS_PRIVSTKSIZE
+//Donatien: default allocated stack size is a bit too low
+# if defined(TARGET_LPC1768) || defined(TARGET_LPC2368)
+# define OS_PRIVSTKSIZE 3000
+# elif defined(TARGET_LPC11U24)
+# define OS_PRIVSTKSIZE 512
+# endif
+#endif
+
+// <q>Check for stack overflow
+// ===========================
+// <i> Includes the stack checking code for stack overflow.
+// <i> Note that additional code reduces the Kernel performance.
+#ifndef OS_STKCHECK
+ #define OS_STKCHECK 1
+#endif
+
+// <q>Run in privileged mode
+// =========================
+// <i> Runs all Threads in privileged mode.
+// <i> Default: Unprivileged
+#ifndef OS_RUNPRIV
+ #define OS_RUNPRIV 1
+#endif
+
+// </h>
+// <h>SysTick Timer Configuration
+// ==============================
+//
+// <o>Timer clock value [Hz] <1-1000000000>
+// <i> Defines the timer clock value.
+// <i> Default: 6000000 (6MHz)
+#ifndef OS_CLOCK
+# if defined(TARGET_LPC1768) || defined(TARGET_LPC2368)
+# define OS_CLOCK 96000000
+# elif defined(TARGET_LPC11U24)
+# define OS_CLOCK 48000000
+# endif
+#endif
+
+// <o>Timer tick value [us] <1-1000000>
+// <i> Defines the timer tick value.
+// <i> Default: 1000 (1ms)
+#ifndef OS_TICK
+ #define OS_TICK 1000
+#endif
+
+// </h>
+
+// <h>System Configuration
+// =======================
+//
+// <e>Round-Robin Thread switching
+// ===============================
+//
+// <i> Enables Round-Robin Thread switching.
+#ifndef OS_ROBIN
+ #define OS_ROBIN 1
+#endif
+
+// <o>Round-Robin Timeout [ticks] <1-1000>
+// <i> Defines how long a thread will execute before a thread switch.
+// <i> Default: 5
+#ifndef OS_ROBINTOUT
+ #define OS_ROBINTOUT 5
+#endif
+
+// </e>
+
+// <e>User Timers
+// ==============
+// <i> Enables user Timers
+#ifndef OS_TIMERS
+ #define OS_TIMERS 1
+#endif
+
+// <o>Timer Thread Priority
+// <1=> Low
+// <2=> Below Normal <3=> Normal
+// <4=> Above Normal
+// <5=> High
+// <6=> Realtime
+// <i> Defines priority for Timer Thread
+// <i> Default: High
+#ifndef OS_TIMERPRIO
+ #define OS_TIMERPRIO 5
+#endif
+
+// <o>Timer Thread stack size [bytes] <64-4096:8><#/4>
+// <i> Defines stack size for Timer thread.
+// <i> Default: 200
+#ifndef OS_TIMERSTKSZ
+ #define OS_TIMERSTKSZ WORDS_STACK_SIZE
+#endif
+
+// <o>Timer Callback Queue size <1-32>
+// <i> Defines number of concurrent callbacks that will be queued.
+// <i> Default: 4
+#ifndef OS_TIMERCBQSZ
+ #define OS_TIMERCBQS 4
+#endif
+
+// </e>
+
+// <o>ISR FIFO Queue size<4=> 4 entries <8=> 8 entries
+// <12=> 12 entries <16=> 16 entries
+// <24=> 24 entries <32=> 32 entries
+// <48=> 48 entries <64=> 64 entries
+// <96=> 96 entries
+// <i> ISR functions store requests to this buffer,
+// <i> when they are called from the iterrupt handler.
+// <i> Default: 16 entries
+#ifndef OS_FIFOSZ
+ #define OS_FIFOSZ 16
+#endif
+
+// </h>
+
+//------------- <<< end of configuration section >>> -----------------------
+
+// Standard library system mutexes
+// ===============================
+// Define max. number system mutexes that are used to protect
+// the arm standard runtime library. For microlib they are not used.
+#ifndef OS_MUTEXCNT
+ #define OS_MUTEXCNT 8
+#endif
+
+/*----------------------------------------------------------------------------
+ * RTX User configuration part END
+ *---------------------------------------------------------------------------*/
+#define OS_TRV ((uint32_t)(((double)OS_CLOCK*(double)OS_TICK)/1E6)-1)
+
+
+/*----------------------------------------------------------------------------
+ * OS Idle daemon
+ *---------------------------------------------------------------------------*/
+void os_idle_demon (void) {
+ /* The idle demon is a system thread, running when no other thread is */
+ /* ready to run. */
+
+ /* Sleep: ideally, we should put the chip to sleep.
+ Unfortunately, this usually requires disconnecting the interface chip (debugger).
+ This can be done, but it would break the local file system.
+ */
+ for (;;) {
+ // sleep();
+ }
+}
+
+/*----------------------------------------------------------------------------
+ * RTX Errors
+ *---------------------------------------------------------------------------*/
+extern void mbed_die(void);
+
+void os_error (uint32_t err_code) {
+ /* This function is called when a runtime error is detected. Parameter */
+ /* 'err_code' holds the runtime error code (defined in RTX_Config.h). */
+ mbed_die();
+}
+
+void sysThreadError(osStatus status) {
+ if (status != osOK) {
+ mbed_die();
+ }
+}
+
+/*----------------------------------------------------------------------------
+ * RTX Configuration Functions
+ *---------------------------------------------------------------------------*/
+
+#include "RTX_CM_lib.h"
+
+#ifdef __cplusplus
+}
+#endif
+
+/*----------------------------------------------------------------------------
+ * end of file
+ *---------------------------------------------------------------------------*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/RtosTimer.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,49 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef TIMER_H
+#define TIMER_H
+
+#include <stdint.h>
+#include "cmsis_os.h"
+
+namespace rtos {
+
+/*! The RtosTimer class allow creating and and controlling of timer functions in the system.
+ A timer function is called when a time period expires whereby both on-shot and
+ periodic timers are possible. A timer can be started, restarted, or stopped.
+
+ Timers are handled in the thread osTimerThread.
+ Callback functions run under control of this thread and may use CMSIS-RTOS API calls.
+*/
+class RtosTimer {
+public:
+ /*! Create and Start timer.
+ \param task name of the timer call back function.
+ \param type osTimerOnce for one-shot or osTimerPeriodic for periodic behaviour. (default: osTimerPeriodic)
+ \param argument argument to the timer call back function. (default: NULL)
+ */
+ RtosTimer(void (*task)(void const *argument),
+ os_timer_type type=osTimerPeriodic,
+ void *argument=NULL);
+
+ /*! Stop the timer.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus stop(void);
+
+ /*! start a timer.
+ \param millisec time delay value of the timer.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus start(uint32_t millisec);
+
+private:
+ osTimerId _timer_id;
+ osTimerDef_t _timer;
+#ifdef CMSIS_OS_RTX
+ uint32_t _timer_data[5];
+#endif
+};
+
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/Semaphore.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,38 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef SEMAPHORE_H
+#define SEMAPHORE_H
+
+#include <stdint.h>
+#include "cmsis_os.h"
+
+namespace rtos {
+
+/*! The Semaphore class is used to manage and protect access to a set of shared resources. */
+class Semaphore {
+public:
+ /*! Create and Initialize a Semaphore object used for managing resources.
+ \param number of available resources; maximum index value is (count-1).
+ */
+ Semaphore(int32_t count);
+
+ /*! Wait until a Semaphore resource becomes available.
+ \param millisec timeout value or 0 in case of no time-out. (default: osWaitForever).
+ \return number of available tokens, or -1 in case of incorrect parameters
+ */
+ int32_t wait(uint32_t millisec=osWaitForever);
+
+ /*! Release a Semaphore resource that was obtain with Semaphore::wait.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus release(void);
+
+private:
+ osSemaphoreId _osSemaphoreId;
+ osSemaphoreDef_t _osSemaphoreDef;
+#ifdef CMSIS_OS_RTX
+ uint32_t _semaphore_data[2];
+#endif
+};
+
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/Thread.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,76 @@
+/* Copyright (c) 2012 mbed.org */
+#ifndef THREAD_H
+#define THREAD_H
+
+#include <stdint.h>
+#include "cmsis_os.h"
+
+namespace rtos {
+
+/*! The Thread class allow defining, creating, and controlling thread functions in the system. */
+class Thread {
+public:
+ /*! Create a new thread, and start it executing the specified function.
+ \param task function to be executed by this thread.
+ \param argument pointer that is passed to the thread function as start argument. (default: NULL).
+ \param priority initial priority of the thread function. (default: osPriorityNormal).
+ \param stacksz stack size (in bytes) requirements for the thread function. (default: DEFAULT_STACK_SIZE).
+ */
+ Thread(void (*task)(void const *argument),
+ void *argument=NULL,
+ osPriority priority=osPriorityNormal,
+ uint32_t stacksize=DEFAULT_STACK_SIZE);
+
+ /*! Terminate execution of a thread and remove it from Active Threads
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus terminate();
+
+ /*! Set priority of an active thread
+ \param priority new priority value for the thread function.
+ \return status code that indicates the execution status of the function.
+ */
+ osStatus set_priority(osPriority priority);
+
+ /*! Get priority of an active thread
+ \ return current priority value of the thread function.
+ */
+ osPriority get_priority();
+
+ /*! Set the specified Signal Flags of an active thread.
+ \param signals specifies the signal flags of the thread that should be set.
+ \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
+ */
+ int32_t signal_set(int32_t signals);
+
+ /*! Wait for one or more Signal Flags to become signaled for the current RUNNING thread.
+ \param signals wait until all specified signal flags set or 0 for any single signal flag.
+ \param millisec timeout value or 0 in case of no time-out. (default: osWaitForever).
+ \return event flag information or error code.
+ */
+ static osEvent signal_wait(int32_t signals, uint32_t millisec=osWaitForever);
+
+
+ /*! Wait for a specified time period in millisec:
+ \param millisec time delay value
+ \return status code that indicates the execution status of the function.
+ */
+ static osStatus wait(uint32_t millisec);
+
+ /*! Pass control to next thread that is in state READY.
+ \return status code that indicates the execution status of the function.
+ */
+ static osStatus yield();
+
+ /*! Get the thread id of the current running thread.
+ \return thread ID for reference by other functions or NULL in case of error.
+ */
+ static osThreadId gettid();
+
+private:
+ osThreadId _tid;
+ osThreadDef_t _thread_def;
+};
+
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtos/cmsis_os.h Mon Jun 11 12:02:30 2012 +0000
@@ -0,0 +1,749 @@
+/* ----------------------------------------------------------------------
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * $Date: 5. March 2012
+ * $Revision: V0.03
+ *
+ * Project: CMSIS-RTOS API
+ * Title: cmsis_os.h RTX header file
+ *
+ * Version 0.02
+ * Initial Proposal Phase
+ * Version 0.03
+ * osKernelStart added, optional feature: main started as thread
+ * osSemaphores have standard behaviour
+ * osTimerCreate does not start the timer, added osTimerStart
+ * osThreadPass is renamed to osThreadYield
+ * -------------------------------------------------------------------- */
+
+/**
+\page cmsis_os_h Header File Template: cmsis_os.h
+
+The file \b cmsis_os.h is a template header file for a CMSIS-RTOS compliant Real-Time Operating System (RTOS).
+Each RTOS that is compliant with CMSIS-RTOS shall provide a specific \b cmsis_os.h header file that represents
+its implementation.
+
+The file cmsis_os.h contains:
+ - CMSIS-RTOS API function definitions
+ - struct definitions for parameters and return types
+ - status and priority values used by CMSIS-RTOS API functions
+ - macros for defining threads and other kernel objects
+
+
+<b>Name conventions and header file modifications</b>
+
+All definitions are prefixed with \b os to give an unique name space for CMSIS-RTOS functions.
+Definitions that are prefixed \b os_ are not used in the application code but local to this header file.
+All definitions and functions that belong to a module are grouped and have a common prefix, i.e. \b osThread.
+
+Definitions that are marked with <b>CAN BE CHANGED</b> can be adapted towards the needs of the actual CMSIS-RTOS implementation.
+These definitions can be specific to the underlying RTOS kernel.
+
+Definitions that are marked with <b>MUST REMAIN UNCHANGED</b> cannot be altered. Otherwise the CMSIS-RTOS implementation is no longer
+compliant to the standard. Note that some functions are optional and need not to be provided by every CMSIS-RTOS implementation.
+
+
+<b>Function calls from interrupt service routines</b>
+
+The following CMSIS-RTOS functions can be called from threads and interrupt service routines (ISR):
+ - \ref osSignalSet
+ - \ref osSemaphoreRelease
+ - \ref osPoolAlloc, \ref osPoolCAlloc, \ref osPoolFree
+ - \ref osMessagePut, \ref osMessageGet
+ - \ref osMailAlloc, \ref osMailCAlloc, \ref osMailGet, \ref osMailPut, \ref osMailFree
+
+Functions that cannot be called from an ISR are verifying the interrupt status and return in case that they are called
+from an ISR context the status code \b osErrorISR. In some implementations this condition might be caught using the HARD FAULT vector.
+
+Some CMSIS-RTOS implementations support CMSIS-RTOS function calls from multiple ISR at the same time.
+If this is impossible, the CMSIS-RTOS rejects calls by nested ISR functions with the status code \b osErrorISRRecursive.
+
+
+<b>Define and reference object definitions</b>
+
+With <b>\#define osObjectsExternal</b> objects are defined as external symbols. This allows to create a consistent header file
+that is used troughtout a project as shown below:
+
+<i>Header File</i>
+\code
+#include <cmsis_os.h> // CMSIS RTOS header file
+
+// Thread definition
+extern void thread_sample (void const *argument); // function prototype
+osThreadDef (thread_sample, osPriorityBelowNormal, 1, 100);
+
+// Pool definition
+osPoolDef(MyPool, 10, long);
+\endcode
+
+
+This header file defines all objects when included in a C/C++ source file. When <b>\#define osObjectsExternal</b> is
+present before the header file, the objects are defined as external symbols. A single consistent header file can therefore be
+used throughout the whole project.
+
+<i>Example</i>
+\code
+#include "osObjects.h" // Definition of the CMSIS-RTOS objects
+\endcode
+
+\code
+#define osObjectExternal // Objects will be defined as external symbols
+#include "osObjects.h" // Reference to the CMSIS-RTOS objects
+\endcode
+
+*/
+
+#ifndef _CMSIS_OS_H
+#define _CMSIS_OS_H
+
+/// \note MUST REMAIN UNCHANGED: \b osCMSIS identifies the CMSIS-RTOS API version
+#define osCMSIS 0x10000 ///< API version (main [31:16] .sub [15:0])
+
+/// \note CAN BE CHANGED: \b osCMSIS_KERNEL identifies the underlaying RTOS kernel and version number.
+#define osCMSIS_RTX 0x0003 ///< RTOS identification and version (main [31:16] .sub [15:0])
+
+/// \note MUST REMAIN UNCHANGED: \b osKernelSystemId shall be consistent in every CMSIS-RTOS.
+#define osKernelSystemId "RTX V0.03" ///< RTOS identification string
+
+
+#define CMSIS_OS_RTX
+
+#ifdef TOOLCHAIN_GCC_ARM
+# define WORDS_STACK_SIZE 512
+#else
+# if defined(TARGET_LPC1768) || defined(TARGET_LPC2368)
+# define WORDS_STACK_SIZE 256
+# elif defined(TARGET_LPC11U24)
+# define WORDS_STACK_SIZE 128
+# endif
+#endif
+
+#define DEFAULT_STACK_SIZE (WORDS_STACK_SIZE*4)
+
+
+/// \note MUST REMAIN UNCHANGED: \b osFeature_xxx shall be consistent in every CMSIS-RTOS.
+#define osFeature_MainThread 1 ///< main thread 1=main can be thread, 0=not available
+#define osFeature_Pool 1 ///< Memory Pools: 1=available, 0=not available
+#define osFeature_MailQ 1 ///< Mail Queues: 1=available, 0=not available
+#define osFeature_MessageQ 1 ///< Message Queues: 1=available, 0=not available
+#define osFeature_Signals 16 ///< maximum number of Signal Flags available per thread
+#define osFeature_Semaphore 8 ///< maximum count for SemaphoreInit function
+#define osFeature_Wait 0 ///< osWait function: 1=available, 0=not available
+
+#if defined (__CC_ARM)
+#define os_InRegs __value_in_regs // Compiler specific: force struct in registers
+#else
+#define os_InRegs
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+
+// ==== Enumeration, structures, defines ====
+
+/// Priority used for thread control.
+/// \note MUST REMAIN UNCHANGED: \b osPriority shall be consistent in every CMSIS-RTOS.
+typedef enum {
+ osPriorityIdle = -3, ///< priority: idle (lowest)
+ osPriorityLow = -2, ///< priority: low
+ osPriorityBelowNormal = -1, ///< priority: below normal
+ osPriorityNormal = 0, ///< priority: normal (default)
+ osPriorityAboveNormal = +1, ///< priority: above normal
+ osPriorityHigh = +2, ///< priority: high
+ osPriorityRealtime = +3, ///< priority: realtime (highest)
+ osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
+} osPriority;
+
+/// Timeout value
+/// \note MUST REMAIN UNCHANGED: \b osWaitForever shall be consistent in every CMSIS-RTOS.
+#define osWaitForever 0xFFFFFFFF ///< wait forever timeout value
+
+/// Status code values returned by CMSIS-RTOS functions
+/// \note MUST REMAIN UNCHANGED: \b osStatus shall be consistent in every CMSIS-RTOS.
+typedef enum {
+ osOK = 0, ///< function completed; no event occurred.
+ osEventSignal = 0x08, ///< function completed; signal event occurred.
+ osEventMessage = 0x10, ///< function completed; message event occurred.
+ osEventMail = 0x20, ///< function completed; mail event occurred.
+ osEventTimeout = 0x40, ///< function completed; timeout occurred.
+ osErrorParameter = 0x80, ///< parameter error: a mandatory parameter was missing or specified an incorrect object.
+ osErrorResource = 0x81, ///< resource not available: a specified resource was not available.
+ osErrorTimeoutResource = 0xC1, ///< resource not available within given time: a specified resource was not available within the timeout period.
+ osErrorISR = 0x82, ///< not allowed in ISR context: the function cannot be called from interrupt service routines.
+ osErrorISRRecursive = 0x83, ///< function called multiple times from ISR with same object.
+ osErrorPriority = 0x84, ///< system cannot determine priority or thread has illegal priority.
+ osErrorNoMemory = 0x85, ///< system is out of memory: it was impossible to allocate or reserve memory for the operation.
+ osErrorValue = 0x86, ///< value of a parameter is out of range.
+ osErrorOS = 0xFF, ///< unspecified RTOS error: run-time error but no other error message fits.
+ os_status_reserved = 0x7FFFFFFF ///< prevent from enum down-size compiler optimization.
+} osStatus;
+
+
+/// Timer type value for the timer definition
+/// \note MUST REMAIN UNCHANGED: \b os_timer_type shall be consistent in every CMSIS-RTOS.
+typedef enum {
+ osTimerOnce = 0, ///< one-shot timer
+ osTimerPeriodic = 1 ///< repeating timer
+} os_timer_type;
+
+/// Entry point of a thread.
+/// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS.
+typedef void (*os_pthread) (void const *argument);
+
+/// Entry point of a timer call back function.
+/// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS.
+typedef void (*os_ptimer) (void const *argument);
+
+// >>> the following data type definitions may shall adapted towards a specific RTOS
+
+/// Thread ID identifies the thread (pointer to a thread control block).
+/// \note CAN BE CHANGED: \b os_thread_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_thread_cb *osThreadId;
+
+/// Timer ID identifies the timer (pointer to a timer control block).
+/// \note CAN BE CHANGED: \b os_timer_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_timer_cb *osTimerId;
+
+/// Mutex ID identifies the mutex (pointer to a mutex control block).
+/// \note CAN BE CHANGED: \b os_mutex_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_mutex_cb *osMutexId;
+
+/// Semaphore ID identifies the semaphore (pointer to a semaphore control block).
+/// \note CAN BE CHANGED: \b os_semaphore_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_semaphore_cb *osSemaphoreId;
+
+/// Pool ID identifies the memory pool (pointer to a memory pool control block).
+/// \note CAN BE CHANGED: \b os_pool_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_pool_cb *osPoolId;
+
+/// Message ID identifies the message queue (pointer to a message queue control block).
+/// \note CAN BE CHANGED: \b os_messageQ_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_messageQ_cb *osMessageQId;
+
+/// Mail ID identifies the mail queue (pointer to a mail queue control block).
+/// \note CAN BE CHANGED: \b os_mailQ_cb is implementation specific in every CMSIS-RTOS.
+typedef struct os_mailQ_cb *osMailQId;
+
+
+/// Thread Definition structure contains startup information of a thread.
+/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_thread_def {
+ os_pthread pthread; ///< start address of thread function
+ osPriority tpriority; ///< initial thread priority
+ uint32_t instances; ///< maximum number of instances of that thread function
+ uint32_t stacksize; ///< stack size requirements in bytes; 0 is default stack size
+} osThreadDef_t;
+
+/// Timer Definition structure contains timer parameters.
+/// \note CAN BE CHANGED: \b os_timer_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_timer_def {
+ os_ptimer ptimer; ///< start address of a timer function
+ void *timer; ///< pointer to internal data
+} osTimerDef_t;
+
+/// Mutex Definition structure contains setup information for a mutex.
+/// \note CAN BE CHANGED: \b os_mutex_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_mutex_def {
+ void *mutex; ///< pointer to internal data
+} osMutexDef_t;
+
+/// Semaphore Definition structure contains setup information for a semaphore.
+/// \note CAN BE CHANGED: \b os_semaphore_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_semaphore_def {
+ void *semaphore; ///< pointer to internal data
+} osSemaphoreDef_t;
+
+/// Definition structure for memory block allocation
+/// \note CAN BE CHANGED: \b os_pool_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_pool_def {
+ uint32_t pool_sz; ///< number of items (elements) in the pool
+ uint32_t item_sz; ///< size of an item
+ void *pool; ///< pointer to memory for pool
+} osPoolDef_t;
+
+/// Definition structure for message queue
+/// \note CAN BE CHANGED: \b os_messageQ_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_messageQ_def {
+ uint32_t queue_sz; ///< number of elements in the queue
+ void *pool; ///< memory array for messages
+} osMessageQDef_t;
+
+/// Definition structure for mail queue
+/// \note CAN BE CHANGED: \b os_mailQ_def is implementation specific in every CMSIS-RTOS.
+typedef struct os_mailQ_def {
+ uint32_t queue_sz; ///< number of elements in the queue
+ uint32_t item_sz; ///< size of an item
+ void *pool; ///< memory array for mail
+} osMailQDef_t;
+
+/// Event structure contains detailed information about an event.
+/// \note MUST REMAIN UNCHANGED: \b os_event shall be consistent in every CMSIS-RTOS.
+/// However the struct may be extended at the end.
+typedef struct {
+ osStatus status; ///< status code: event or error information
+ union {
+ uint32_t v; ///< message as 32-bit value
+ void *p; ///< message or mail as void pointer
+ int32_t signals; ///< signal flags
+ } value; ///< event value
+ union {
+ osMailQId mail_id; ///< mail id obtained by \ref osMailCreate
+ osMessageQId message_id; ///< message id obtained by \ref osMessageCreate
+ } def; ///< event definition
+} osEvent;
+
+
+// ==== Kernel Control Functions ====
+
+/// Start the RTOS Kernel with executing the specified thread.
+/// \param[in] thread_def thread definition referenced with \ref osThread.
+/// \param[in] argument pointer that is passed to the thread function as start argument.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osKernelStart shall be consistent in every CMSIS-RTOS.
+osStatus osKernelStart (osThreadDef_t *thread_def, void *argument);
+
+/// Check if the RTOS kernel is already started.
+/// \note MUST REMAIN UNCHANGED: \b osKernelRunning shall be consistent in every CMSIS-RTOS.
+/// \return 0 RTOS is not started, 1 RTOS is started.
+int32_t osKernelRunning(void);
+
+
+// ==== Thread Management ====
+
+/// Create a Thread Definition with function, priority, and stack requirements.
+/// \param name name of the thread function.
+/// \param priority initial priority of the thread function.
+/// \param instances number of possible thread instances.
+/// \param stacksz stack size (in bytes) requirements for the thread function.
+/// \note CAN BE CHANGED: The parameters to \b osThreadDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osThreadDef(name, priority, instances, stacksz) \
+extern osThreadDef_t os_thread_def_##name
+#else // define the object
+#define osThreadDef(name, priority, instances, stacksz) \
+osThreadDef_t os_thread_def_##name = \
+{ (name), (priority), (instances), (stacksz) }
+#endif
+
+/// Access a Thread defintion.
+/// \param name name of the thread definition object.
+/// \note CAN BE CHANGED: The parameter to \b osThread shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osThread(name) \
+&os_thread_def_##name
+
+
+/// Create a thread and add it to Active Threads and set it to state READY.
+/// \param[in] thread_def thread definition referenced with \ref osThread.
+/// \param[in] argument pointer that is passed to the thread function as start argument.
+/// \return thread ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osThreadCreate shall be consistent in every CMSIS-RTOS.
+osThreadId osThreadCreate (osThreadDef_t *thread_def, void *argument);
+
+/// Return the thread ID of the current running thread.
+/// \return thread ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osThreadGetId shall be consistent in every CMSIS-RTOS.
+osThreadId osThreadGetId (void);
+
+/// Terminate execution of a thread and remove it from Active Threads.
+/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osThreadTerminate shall be consistent in every CMSIS-RTOS.
+osStatus osThreadTerminate (osThreadId thread_id);
+
+/// Pass control to next thread that is in state \b READY.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osThreadYield shall be consistent in every CMSIS-RTOS.
+osStatus osThreadYield (void);
+
+/// Change priority of an active thread.
+/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
+/// \param[in] priority new priority value for the thread function.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osThreadSetPriority shall be consistent in every CMSIS-RTOS.
+osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority);
+
+/// Get current priority of an active thread.
+/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
+/// \return current priority value of the thread function.
+/// \note MUST REMAIN UNCHANGED: \b osThreadGetPriority shall be consistent in every CMSIS-RTOS.
+osPriority osThreadGetPriority (osThreadId thread_id);
+
+
+
+// ==== Generic Wait Functions ====
+
+/// Wait for Timeout (Time Delay)
+/// \param[in] millisec time delay value
+/// \return status code that indicates the execution status of the function.
+osStatus osDelay (uint32_t millisec);
+
+#if (defined (osFeature_Wait) && (osFeature_Wait != 0)) // Generic Wait available
+
+/// Wait for Signal, Message, Mail, or Timeout
+/// \param[in] millisec timeout value or 0 in case of no time-out
+/// \return event that contains signal, message, or mail information or error code.
+/// \note MUST REMAIN UNCHANGED: \b osWait shall be consistent in every CMSIS-RTOS.
+os_InRegs osEvent osWait (uint32_t millisec);
+
+#endif // Generic Wait available
+
+
+// ==== Timer Management Functions ====
+/// Define a Timer object.
+/// \param name name of the timer object.
+/// \param function name of the timer call back function.
+/// \note CAN BE CHANGED: The parameter to \b osTimerDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osTimerDef(name, function) \
+extern osTimerDef_t os_timer_def_##name
+#else // define the object
+#define osTimerDef(name, function) \
+uint32_t os_timer_cb_##name[5]; \
+osTimerDef_t os_timer_def_##name = \
+{ (function), (os_timer_cb_##name) }
+#endif
+
+/// Access a Timer definition.
+/// \param name name of the timer object.
+/// \note CAN BE CHANGED: The parameter to \b osTimer shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osTimer(name) \
+&os_timer_def_##name
+
+/// Create a timer.
+/// \param[in] timer_def timer object referenced with \ref osTimer.
+/// \param[in] type osTimerOnce for one-shot or osTimerPeriodic for periodic behavior.
+/// \param[in] argument argument to the timer call back function.
+/// \return timer ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osTimerCreate shall be consistent in every CMSIS-RTOS.
+osTimerId osTimerCreate (osTimerDef_t *timer_def, os_timer_type type, void *argument);
+
+/// Start or restart a timer.
+/// \param[in] timer_id timer ID obtained by \ref osTimerCreate.
+/// \param[in] millisec time delay value of the timer.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osTimerStart shall be consistent in every CMSIS-RTOS.
+osStatus osTimerStart (osTimerId timer_id, uint32_t millisec);
+
+/// Stop the timer.
+/// \param[in] timer_id timer ID obtained by \ref osTimerCreate.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osTimerStop shall be consistent in every CMSIS-RTOS.
+osStatus osTimerStop (osTimerId timer_id);
+
+
+// ==== Signal Management ====
+
+/// Set the specified Signal Flags of an active thread.
+/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
+/// \param[in] signals specifies the signal flags of the thread that should be set.
+/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
+/// \note MUST REMAIN UNCHANGED: \b osSignalSet shall be consistent in every CMSIS-RTOS.
+int32_t osSignalSet (osThreadId thread_id, int32_t signal);
+
+/// Clear the specified Signal Flags of an active thread.
+/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
+/// \param[in] signals specifies the signal flags of the thread that shall be cleared.
+/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
+/// \note MUST REMAIN UNCHANGED: \b osSignalClear shall be consistent in every CMSIS-RTOS.
+int32_t osSignalClear (osThreadId thread_id, int32_t signal);
+
+/// Get Signal Flags status of an active thread.
+/// \param[in] thread_id thread ID obtained by \ref osThreadCreate or \ref osThreadGetId.
+/// \return previous signal flags of the specified thread or 0x80000000 in case of incorrect parameters.
+/// \note MUST REMAIN UNCHANGED: \b osSignalGet shall be consistent in every CMSIS-RTOS.
+int32_t osSignalGet (osThreadId thread_id);
+
+/// Wait for one or more Signal Flags to become signaled for the current \b RUNNING thread.
+/// \param[in] signals wait until all specified signal flags set or 0 for any single signal flag.
+/// \param[in] millisec timeout value or 0 in case of no time-out.
+/// \return event flag information or error code.
+/// \note MUST REMAIN UNCHANGED: \b osSignalWait shall be consistent in every CMSIS-RTOS.
+os_InRegs osEvent osSignalWait (int32_t signals, uint32_t millisec);
+
+
+// ==== Mutex Management ====
+
+/// Define a Mutex.
+/// \param name name of the mutex object.
+/// \note CAN BE CHANGED: The parameter to \b osMutexDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osMutexDef(name) \
+extern osMutexDef_t os_mutex_def_##name
+#else // define the object
+#define osMutexDef(name) \
+uint32_t os_mutex_cb_##name[3]; \
+osMutexDef_t os_mutex_def_##name = { (os_mutex_cb_##name) }
+#endif
+
+/// Access a Mutex defintion.
+/// \param name name of the mutex object.
+/// \note CAN BE CHANGED: The parameter to \b osMutex shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osMutex(name) \
+&os_mutex_def_##name
+
+/// Create and Initialize a Mutex object
+/// \param[in] mutex_def mutex definition referenced with \ref osMutex.
+/// \return mutex ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osMutexCreate shall be consistent in every CMSIS-RTOS.
+osMutexId osMutexCreate (osMutexDef_t *mutex_def);
+
+/// Wait until a Mutex becomes available
+/// \param[in] mutex_id mutex ID obtained by \ref osMutexCreate.
+/// \param[in] millisec timeout value or 0 in case of no time-out.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osMutexWait shall be consistent in every CMSIS-RTOS.
+osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec);
+
+/// Release a Mutex that was obtained by \ref osMutexWait
+/// \param[in] mutex_id mutex ID obtained by \ref osMutexCreate.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osMutexRelease shall be consistent in every CMSIS-RTOS.
+osStatus osMutexRelease (osMutexId mutex_id);
+
+
+// ==== Semaphore Management Functions ====
+
+#if (defined (osFeature_Semaphore) && (osFeature_Semaphore != 0)) // Semaphore available
+
+/// Define a Semaphore object.
+/// \param name name of the semaphore object.
+/// \note CAN BE CHANGED: The parameter to \b osSemaphoreDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osSemaphoreDef(name) \
+extern osSemaphoreDef_t os_semaphore_def_##name
+#else // define the object
+#define osSemaphoreDef(name) \
+uint32_t os_semaphore_cb_##name[2]; \
+osSemaphoreDef_t os_semaphore_def_##name = { (os_semaphore_cb_##name) }
+#endif
+
+/// Access a Semaphore definition.
+/// \param name name of the semaphore object.
+/// \note CAN BE CHANGED: The parameter to \b osSemaphore shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osSemaphore(name) \
+&os_semaphore_def_##name
+
+/// Create and Initialize a Semaphore object used for managing resources
+/// \param[in] semaphore_def semaphore definition referenced with \ref osSemaphore.
+/// \param[in] count number of available resources.
+/// \return semaphore ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osSemaphoreCreate shall be consistent in every CMSIS-RTOS.
+osSemaphoreId osSemaphoreCreate (osSemaphoreDef_t *semaphore_def, int32_t count);
+
+/// Wait until a Semaphore token becomes available
+/// \param[in] semaphore_id semaphore object referenced with \ref osSemaphore.
+/// \param[in] millisec timeout value or 0 in case of no time-out.
+/// \return number of available tokens, or -1 in case of incorrect parameters.
+/// \note MUST REMAIN UNCHANGED: \b osSemaphoreWait shall be consistent in every CMSIS-RTOS.
+int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec);
+
+/// Release a Semaphore token
+/// \param[in] semaphore_id semaphore object referenced with \ref osSemaphore.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osSemaphoreRelease shall be consistent in every CMSIS-RTOS.
+osStatus osSemaphoreRelease (osSemaphoreId semaphore_id);
+
+#endif // Semaphore available
+
+// ==== Memory Pool Management Functions ====
+
+#if (defined (osFeature_Pool) && (osFeature_Pool != 0)) // Memory Pool Management available
+
+/// \brief Define a Memory Pool.
+/// \param name name of the memory pool.
+/// \param no maximum number of objects (elements) in the memory pool.
+/// \param type data type of a single object (element).
+/// \note CAN BE CHANGED: The parameter to \b osPoolDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osPoolDef(name, no, type) \
+extern osPoolDef_t os_pool_def_##name
+#else // define the object
+#define osPoolDef(name, no, type) \
+uint32_t os_pool_m_##name[3+((sizeof(type)+3)/4)*(no)]; \
+osPoolDef_t os_pool_def_##name = \
+{ (no), sizeof(type), (os_pool_m_##name) }
+#endif
+
+/// \brief Access a Memory Pool definition.
+/// \param name name of the memory pool
+/// \note CAN BE CHANGED: The parameter to \b osPool shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osPool(name) \
+&os_pool_def_##name
+
+/// Create and Initialize a memory pool
+/// \param[in] pool_def memory pool definition referenced with \ref osPool.
+/// \return memory pool ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osPoolCreate shall be consistent in every CMSIS-RTOS.
+osPoolId osPoolCreate (osPoolDef_t *pool_def);
+
+/// Allocate a memory block from a memory pool
+/// \param[in] pool_id memory pool ID obtain referenced with \ref osPoolCreate.
+/// \return address of the allocated memory block or NULL in case of no memory available.
+/// \note MUST REMAIN UNCHANGED: \b osPoolAlloc shall be consistent in every CMSIS-RTOS.
+void *osPoolAlloc (osPoolId pool_id);
+
+/// Allocate a memory block from a memory pool and set memory block to zero
+/// \param[in] pool_id memory pool ID obtain referenced with \ref osPoolCreate.
+/// \return address of the allocated memory block or NULL in case of no memory available.
+/// \note MUST REMAIN UNCHANGED: \b osPoolCAlloc shall be consistent in every CMSIS-RTOS.
+void *osPoolCAlloc (osPoolId pool_id);
+
+/// Return an allocated memory block back to a specific memory pool
+/// \param[in] pool_id memory pool ID obtain referenced with \ref osPoolCreate.
+/// \param[in] block address of the allocated memory block that is returned to the memory pool.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osPoolFree shall be consistent in every CMSIS-RTOS.
+osStatus osPoolFree (osPoolId pool_id, void *block);
+
+#endif // Memory Pool Management available
+
+
+// ==== Message Queue Management Functions ====
+
+#if (defined (osFeature_MessageQ) && (osFeature_MessageQ != 0)) // Message Queues available
+
+/// \brief Create a Message Queue Definition.
+/// \param name name of the queue.
+/// \param queue_sz maximum number of messages in the queue.
+/// \param type data type of a single message element (for debugger).
+/// \note CAN BE CHANGED: The parameter to \b osMessageQDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osMessageQDef(name, queue_sz, type) \
+extern osMessageQDef_t os_messageQ_def_##name
+#else // define the object
+#define osMessageQDef(name, queue_sz, type) \
+uint32_t os_messageQ_q_##name[4+(queue_sz)]; \
+osMessageQDef_t os_messageQ_def_##name = \
+{ (queue_sz), (os_messageQ_q_##name) }
+#endif
+
+/// \brief Access a Message Queue Definition.
+/// \param name name of the queue
+/// \note CAN BE CHANGED: The parameter to \b osMessageQ shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osMessageQ(name) \
+&os_messageQ_def_##name
+
+/// Create and Initialize a Message Queue.
+/// \param[in] queue_def queue definition referenced with \ref osMessageQ.
+/// \param[in] thread_id thread ID (obtained by \ref osThreadCreate or \ref osThreadGetId) or NULL.
+/// \return message queue ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osMessageCreate shall be consistent in every CMSIS-RTOS.
+osMessageQId osMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id);
+
+/// Put a Message to a Queue.
+/// \param[in] queue_id message queue ID obtained with \ref osMessageCreate.
+/// \param[in] info message information.
+/// \param[in] millisec timeout value or 0 in case of no time-out.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osMessagePut shall be consistent in every CMSIS-RTOS.
+osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec);
+
+/// Get a Message or Wait for a Message from a Queue.
+/// \param[in] queue_id message queue ID obtained with \ref osMessageCreate.
+/// \param[in] millisec timeout value or 0 in case of no time-out.
+/// \return event information that includes status code.
+/// \note MUST REMAIN UNCHANGED: \b osMessageGet shall be consistent in every CMSIS-RTOS.
+os_InRegs osEvent osMessageGet (osMessageQId queue_id, uint32_t millisec);
+
+#endif // Message Queues available
+
+
+// ==== Mail Queue Management Functions ====
+
+#if (defined (osFeature_MailQ) && (osFeature_MailQ != 0)) // Mail Queues available
+
+/// \brief Create a Mail Queue Definition
+/// \param name name of the queue
+/// \param queue_sz maximum number of messages in queue
+/// \param type data type of a single message element
+/// \note CAN BE CHANGED: The parameter to \b osMailQDef shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#if defined (osObjectsExternal) // object is external
+#define osMailQDef(name, queue_sz, type) \
+extern osMailQDef_t os_mailQ_def_##name
+#else // define the object
+#define osMailQDef(name, queue_sz, type) \
+uint32_t os_mailQ_q_##name[4+(queue_sz)]; \
+uint32_t os_mailQ_m_##name[3+((sizeof(type)+3)/4)*(queue_sz)]; \
+void * os_mailQ_p_##name[2] = { (os_mailQ_q_##name), os_mailQ_m_##name }; \
+osMailQDef_t os_mailQ_def_##name = \
+{ (queue_sz), sizeof(type), (os_mailQ_p_##name) }
+#endif
+
+/// \brief Access a Mail Queue Definition
+/// \param name name of the queue
+/// \note CAN BE CHANGED: The parameter to \b osMailQ shall be consistent but the
+/// macro body is implementation specific in every CMSIS-RTOS.
+#define osMailQ(name) \
+&os_mailQ_def_##name
+
+/// Create and Initialize mail queue
+/// \param[in] queue_def reference to the mail queue definition obtain with \ref osMailQ
+/// \param[in] thread_id thread ID (obtained by \ref osThreadCreate or \ref osThreadGetId) or NULL.
+/// \return mail queue ID for reference by other functions or NULL in case of error.
+/// \note MUST REMAIN UNCHANGED: \b osMailCreate shall be consistent in every CMSIS-RTOS.
+osMailQId osMailCreate (osMailQDef_t *queue_def, osThreadId thread_id);
+
+/// Allocate a memory block from a mail
+/// \param[in] queue_id mail queue ID obtained with \ref osMailCreate.
+/// \param[in] millisec timeout value or 0 in case of no time-out
+/// \return pointer to memory block that can be filled with mail or NULL in case error.
+/// \note MUST REMAIN UNCHANGED: \b osMailAlloc shall be consistent in every CMSIS-RTOS.
+void *osMailAlloc (osMailQId queue_id, uint32_t millisec);
+
+/// Allocate a memory block from a mail and set memory block to zero
+/// \param[in] queue_id mail queue ID obtained with \ref osMailCreate.
+/// \param[in] millisec timeout value or 0 in case of no time-out
+/// \return pointer to memory block that can shall filled with mail or NULL in case error.
+/// \note MUST REMAIN UNCHANGED: \b osMailCAlloc shall be consistent in every CMSIS-RTOS.
+void *osMailCAlloc (osMailQId queue_id, uint32_t millisec);
+
+/// Put a mail to a queue
+/// \param[in] queue_id mail queue ID obtained with \ref osMailCreate.
+/// \param[in] mail memory block previously allocated with \ref osMailAlloc or \ref osMailCAlloc.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osMailPut shall be consistent in every CMSIS-RTOS.
+osStatus osMailPut (osMailQId queue_id, void *mail);
+
+/// Get a mail from a queue
+/// \param[in] queue_id mail queue ID obtained with \ref osMailCreate.
+/// \param[in] millisec timeout value or 0 in case of no time-out
+/// \return event that contains mail information or error code.
+/// \note MUST REMAIN UNCHANGED: \b osMailGet shall be consistent in every CMSIS-RTOS.
+os_InRegs osEvent osMailGet (osMailQId queue_id, uint32_t millisec);
+
+/// Free a memory block from a mail
+/// \param[in] queue_id mail queue ID obtained with \ref osMailCreate.
+/// \param[in] mail pointer to the memory block that was obtained with \ref osMailGet.
+/// \return status code that indicates the execution status of the function.
+/// \note MUST REMAIN UNCHANGED: \b osMailFree shall be consistent in every CMSIS-RTOS.
+osStatus osMailFree (osMailQId queue_id, void *mail);
+
+#endif // Mail Queues available
+
+/// Set Thread Error (for Create funcions which return IDs)
+extern void sysThreadError(osStatus status);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _CMSIS_OS_H
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rtos/rtos.h Mon Jun 11 12:02:30 2012 +0000 @@ -0,0 +1,17 @@ +/* mbed Microcontroller Library + * Copyright (c) 2006-2012 ARM Limited. All rights reserved. + */ +#ifndef RTOS_H +#define RTOS_H + +#include "Thread.h" +#include "Mutex.h" +#include "RtosTimer.h" +#include "Semaphore.h" +#include "Mail.h" +#include "MemoryPool.h" +#include "Queue.h" + +using namespace rtos; + +#endif