Revision 3:6811c0ce95f6, committed 2018-12-04

Comitter:

altb

Date:

Tue Dec 04 15:49:48 2018 +0000

Parent:

1:36bbe04e1f6f

Commit message:

AHRS Klasse mit Mahony filter etc

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.cpp	Tue Dec 04 15:49:48 2018 +0000
@@ -0,0 +1,49 @@
+#include "AHRS.h"
+#include "Mahony.h"
+#include "MadgwickAHRS.h"
+#define PI 3.141592653589793
+
+using namespace std;
+
+AHRS::AHRS(uint8_t filtertype,float TS) : RPY_filter(TS), csAG(PA_15),csM(PD_2), spi(PC_12, PC_11, PC_10), imu(&spi, &csM, &csAG), thread(osPriorityBelowNormal, 4096){  
+    
+    thread.start(callback(this, &AHRS::update));
+    ticker.attach(callback(this, &AHRS::sendSignal), TS);
+    LinearCharacteristics raw_ax2ax(40.0/65536.0,-418.0);       // use gain and offset here
+    LinearCharacteristics raw_ay2ay(-40.0/65536.0,-307.0);      // y-axis reversed
+    LinearCharacteristics raw_az2az(-16350.0,16350,-10.0, 10.0);
+    LinearCharacteristics raw_gx2gx(-32767,32768,-500*PI/180.0, 500*PI/180.0);
+    LinearCharacteristics raw_gy2gy(-32767,32768, 500*PI/180.0,-500*PI/180.0);  // y-axis reversed (lefthanded system)
+    LinearCharacteristics raw_gz2gz(-32767,32768,-500*PI/180.0, 500*PI/180.0);
+    LinearCharacteristics int2magx( -32767,32768,100.0,-100.0);     // x-axis reversed
+    LinearCharacteristics int2magy( -32767,32768,100.0,-100.0);     // y-axis reversed
+    LinearCharacteristics int2magz( -32767,32768,-100.0,100.0);
+    while (!imu.begin()) {
+        wait(1);
+        printf("Failed to communicate with LSM9DS1.\r\n");
+    }
+
+}
+
+AHRS::~AHRS() {}
+
+void AHRS::update(void)
+{
+    while(1){
+        thread.signal_wait(signal);
+        imu.readAccel();
+        imu.readMag_calibrated();
+        imu.readGyro();
+
+        //Perform Madgwick-filter update
+        RPY_filter.update(raw_gx2gx(imu.gx), raw_gy2gy(imu.gy),  raw_gz2gz(imu.gz) ,
+                      raw_ax2ax(imu.ax), raw_ay2ay(imu.ay),  raw_az2az(imu.az),
+                      int2magx(imu.mx), int2magy(imu.my),  int2magz(imu.mz)); 
+       
+ 
+        }       // while(1)
+}
+void AHRS::sendSignal() {
+    
+    thread.signal_set(signal);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.h	Tue Dec 04 15:49:48 2018 +0000
@@ -0,0 +1,45 @@
+#include "Mahony.h"
+#include "MadgwickAHRS.h"
+#include "LinearCharacteristics.h"
+#include "LSM9DS1.h"
+#include "Signal.h"
+
+class AHRS{
+public:
+    AHRS(uint8_t,float);
+    virtual ~AHRS();
+    
+    float getRollRadians() {
+        if (!RPY_filter.anglesComputed) RPY_filter.computeAngles();
+        return RPY_filter.getRoll();
+    }
+    float getPitchRadians() {
+        if (!RPY_filter.anglesComputed) RPY_filter.computeAngles();
+        return RPY_filter.getPitch();
+    }
+    float getYawRadians() {
+        if (!RPY_filter.anglesComputed) RPY_filter.computeAngles();
+        return RPY_filter.getYaw();
+    }
+    LSM9DS1 imu;
+    Mahony RPY_filter;
+    LinearCharacteristics raw_gx2gx;
+    LinearCharacteristics raw_gy2gy;
+    LinearCharacteristics raw_gz2gz;
+private:
+    Signal signal;
+    Thread thread;
+    Ticker ticker;
+    Mutex mutex;      // mutex to lock critical sections 
+    void sendSignal();
+    void update();
+    LinearCharacteristics raw_ax2ax;
+    LinearCharacteristics raw_ay2ay;
+    LinearCharacteristics raw_az2az;
+    LinearCharacteristics int2magx;     
+    LinearCharacteristics int2magy;
+    LinearCharacteristics int2magz;
+    SPI spi;
+    DigitalOut csAG;         // for spi
+    DigitalOut csM;           //  "
+};

--- a/MadgwickAHRS.cpp	Fri Oct 26 05:56:25 2018 +0000
+++ b/MadgwickAHRS.cpp	Tue Dec 04 15:49:48 2018 +0000
@@ -26,7 +26,6 @@
 //-------------------------------------------------------------------------------------------
 // Definitions
 
-#define sampleFreqDef   100.0f          // sample frequency in Hz
 #define betaDef         0.25f            // 2 * proportional gain 0.1 - 0.5 - 5; 8
 
 
@@ -36,13 +35,13 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Madgwick::Madgwick() {
+Madgwick::Madgwick(float Ts) {
     beta = betaDef;
     q0 = 1.0f;
     q1 = 0.0f;
     q2 = 0.0f;
     q3 = 0.0f;
-    invSampleFreq = 1.0f / sampleFreqDef;
+    invSampleFreq = Ts;
     anglesComputed = 0;
 }
 

--- a/MadgwickAHRS.h	Fri Oct 26 05:56:25 2018 +0000
+++ b/MadgwickAHRS.h	Tue Dec 04 15:49:48 2018 +0000
@@ -39,7 +39,7 @@
 // Function declarations
 public:
     float invSampleFreq;
-    Madgwick(void);
+    Madgwick(float);
     void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
     void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
     void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);

--- a/Mahony.cpp	Fri Oct 26 05:56:25 2018 +0000
+++ b/Mahony.cpp	Tue Dec 04 15:49:48 2018 +0000
@@ -27,7 +27,6 @@
 //-------------------------------------------------------------------------------------------
 // Definitions
 
-#define DEFAULT_SAMPLE_FREQ 100.0f  // sample frequency in Hz
 #define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
 #define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain
 
@@ -38,7 +37,7 @@
 //-------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-Mahony::Mahony()
+Mahony::Mahony(float Ts)
 {
     twoKp = twoKpDef;   // 2 * proportional gain (Kp)
     twoKi = twoKiDef;   // 2 * integral gain (Ki)
@@ -50,7 +49,7 @@
     integralFBy = 0.0f;
     integralFBz = 0.0f;
     anglesComputed = 0;
-    invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
+    invSampleFreq = Ts;
 }
 
 void Mahony::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)

--- a/Mahony.h	Fri Oct 26 05:56:25 2018 +0000
+++ b/Mahony.h	Tue Dec 04 15:49:48 2018 +0000
@@ -26,18 +26,19 @@
     float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
     float invSampleFreq;
     float roll, pitch, yaw;
-    char anglesComputed;
     static float invSqrt(float x);
-    void computeAngles();
+
 
 //-------------------------------------------------------------------------------------------
 // Function declarations
 
 public:
-    Mahony();
+    Mahony(float);
     void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
     void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
     void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+    char anglesComputed;
+    void computeAngles();
     float getRoll() {
         if (!anglesComputed) computeAngles();
         return roll * 57.29578f;