Diff: main.cpp

Revision:

1:c54fc6087d0d

Parent:

0:2c66000e0791

Child:

3:8cbf1e2122d4

--- a/main.cpp	Wed Feb 01 09:15:27 2017 +0000
+++ b/main.cpp	Tue Feb 07 14:13:36 2017 +0000
@@ -1,6 +1,8 @@
 /*
     STM32 - Magnetic light - Updated version to work with ST components. (MCU ,  Magnetic sensor)
-    Arkadiraf@gmail.com  - 25/01/2017
+    Arkadiraf@gmail.com  - 07/02/2017
+    
+    Based on my previous arduino version published at:
 */
 
 /*
@@ -10,8 +12,12 @@
  D11 -  Led Din (through voltage converter 3v3->5V)
  GND -  GND
 
- Magnetic sensor LIS
- i2c
+ Magnetic sensor LIS3MDL I2C : https://www.pololu.com/product/2737
+ 3V3 - Vin
+ GND - GND
+ D5  - SCL
+ D4  - SDA 
+ 
 */
 
 ///////////////
@@ -19,7 +25,7 @@
 ///////////////
 #include "mbed.h"
 #include "PololuLedStrip.h"
-
+#include "x_nucleo_iks01a1.h"
 //////////////
 // Defines: //
 //////////////
@@ -33,6 +39,12 @@
 PololuLedStrip ledStrip(D11);
 Timer timer;
 Serial pc(SERIAL_TX, SERIAL_RX,57600);
+
+// Magnetic sensor:
+/* Instantiate the expansion board */
+static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D4, D5);
+static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
+
 ///////////////
 // variables //
 ///////////////
@@ -123,6 +135,7 @@
 
 
 // Magnetic sensor:
+int16_t axesRaw[3]; // raw data from magnetic sensor
 float Mag_raw[3]= {1,0,0}; //Magnetometer
 float Mag[3]= {0,0,0};     //Magnetometer
 float Mag_Bias[3]= {0,0,0};     //Magnetometer bias values
@@ -146,6 +159,10 @@
 ///////////////
 // Functions //
 ///////////////
+// HPF filter:
+void HighPassFilter();
+// Update Pixel array based on mag sensor
+void Update_Pixels_Mag();
 // move dot throught the strip
 void dotMove(rgb_color dotColor);
 // update strip colors
@@ -159,10 +176,11 @@
     // init timer
     timer.start();
 
-    // init magnetometer
-
-
-    //
+    // init magnetometer - Rewrite library or add options to change magnetic full scale and sample rate
+    // read magnetic sensor ID
+    uint8_t id;
+    magnetometer->ReadID(&id);
+    pc.printf("LIS3MDL magnetometer              = 0x%X\r\n", id);
 
     ///////////////////////
     //  Main Code Loop : //
@@ -182,13 +200,17 @@
             sampleMillis=timeMillis;
 
             // Read magnetometer values
-            Mag_raw[0]=0;
-            Mag_raw[1]=1000;
-            Mag_raw[2]=0;
+            magnetometer->Get_M_AxesRaw(axesRaw);
+            // debbug messages
+            //pc.printf("LIS3MDL [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", axesRaw[0], axesRaw[1], axesRaw[2]);
+            
+            // update float ariables
+            // axis adjustment to fit the previous setup, (visual aligment).
+            Mag_raw[0]=axesRaw[1];
+            Mag_raw[1]=axesRaw[0];
+            Mag_raw[2]=axesRaw[2];
 
-            ////////////////////////////
-            // bias samples and scale //
-            ////////////////////////////
+            // bias samples and scale
             Mag[0]=Mag_raw[0]-Mag_Bias[0];
             Mag[1]=Mag_raw[1]-Mag_Bias[1];
             Mag[2]=Mag_raw[2]-Mag_Bias[2];
@@ -198,60 +220,18 @@
             Mag_Norm[1]=Mag[1]/Mag_ABS;
             Mag_Norm[2]=Mag[2]/Mag_ABS;
 
-            /////////////////
-            // HPF filter: //
-            /////////////////
-//            LMagIn[0]=MagIn[0];
-//            LMagIn[1]=MagIn[1];
-//            LMagIn[2]=MagIn[2];
-//            LMagOut[0]=MagOut[0];
-//            LMagOut[1]=MagOut[1];
-//            LMagOut[2]=MagOut[2];
-//            // update reading
-//            MagIn[0]=Mag_raw[0];
-//            MagIn[1]=Mag_raw[1];
-//            MagIn[2]=Mag_raw[2];
-//            // update filter
-//            MagOut[0]=AHPF*(LMagOut[0]+MagIn[0]-LMagIn[0]);
-//            MagOut[1]=AHPF*(LMagOut[1]+MagIn[1]-LMagIn[1]);
-//            MagOut[2]=AHPF*(LMagOut[2]+MagIn[2]-LMagIn[2]);
-//
-//            // Normalize vector and calculate ABS value
-//            Mag_ABS=sqrt(MagOut[0]*MagOut[0]+MagOut[1]*MagOut[1]+MagOut[2]*MagOut[2]);
-//            Mag_Norm[0]=MagOut[0]/Mag_ABS;
-//            Mag_Norm[1]=MagOut[1]/Mag_ABS;
-//            Mag_Norm[2]=MagOut[2]/Mag_ABS;
+            // HPF filter:
+            HighPassFilter();
 
-
-            // Calculate angle between magnetic vector and LED vectors
-            for (uint16_t ii=0 ; ii<LED_COUNT ; ii++) {
-                Pixel_Vect_Float[0]=((float)Pixel_Vect[ii][0])/100;
-                Pixel_Vect_Float[1]=((float)Pixel_Vect[ii][1])/100;
-                Pixel_Vect_Float[2]=((float)Pixel_Vect[ii][2])/100;
-                CosAngle=Mag_Norm[0]*Pixel_Vect_Float[0] + Mag_Norm[1]*Pixel_Vect_Float[1] + Mag_Norm[2]*Pixel_Vect_Float[2];
-                //LedPower=Mag_ABS*CosAngle*CosAngle*CosAngle*CosAngle*CosAngle;
-                LedPower=Mag_ABS*((float)pow(CosAngle,5));
-                if (LedPower>=0) {
-                    if (LedPower>255) LedPower=255;
-                    LedPower_Byte=(uint8_t)(LedPower);
-                    colors[ii] = (rgb_color) {
-                        LedPower_Byte, 0, 0
-                    };
-                }
-                if (LedPower<0) {
-                    if (LedPower<-255) LedPower=-255;
-                    LedPower_Byte=(uint8_t)(-LedPower);
-                    colors[ii] = (rgb_color) {
-                        0, 0, LedPower_Byte
-                    };
-                }
-            }
+            // Update Pixel array based on mag sensor
+            Update_Pixels_Mag();
+            
             // Send the colors to the LED strip.
             ledStrip.write(colors, LED_COUNT);
-            
+
             // debug messages:
-            pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag[0],Mag[1],Mag[2]);    
-            pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag_Norm[0],Mag_Norm[1],Mag_Norm[2]);       
+            //pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag[0],Mag[1],Mag[2]);
+            //pc.printf("DBG_1: mill: %d , MAG: %.2f , %.2f , %.2f \r\n",sampleMillis,Mag_Norm[0],Mag_Norm[1],Mag_Norm[2]);
         }// end update pixels based on mag
 
 
@@ -273,6 +253,60 @@
 ///////////////
 // Functions //
 ///////////////
+// HPF filter:
+void HighPassFilter()
+{
+    LMagIn[0]=MagIn[0];
+    LMagIn[1]=MagIn[1];
+    LMagIn[2]=MagIn[2];
+    LMagOut[0]=MagOut[0];
+    LMagOut[1]=MagOut[1];
+    LMagOut[2]=MagOut[2];
+    // update reading
+    MagIn[0]=Mag_raw[0];
+    MagIn[1]=Mag_raw[1];
+    MagIn[2]=Mag_raw[2];
+    // update filter
+    MagOut[0]=AHPF*(LMagOut[0]+MagIn[0]-LMagIn[0]);
+    MagOut[1]=AHPF*(LMagOut[1]+MagIn[1]-LMagIn[1]);
+    MagOut[2]=AHPF*(LMagOut[2]+MagIn[2]-LMagIn[2]);
+
+    // Normalize vector and calculate ABS value
+    Mag_ABS=sqrt(MagOut[0]*MagOut[0]+MagOut[1]*MagOut[1]+MagOut[2]*MagOut[2]);
+    Mag_Norm[0]=MagOut[0]/Mag_ABS;
+    Mag_Norm[1]=MagOut[1]/Mag_ABS;
+    Mag_Norm[2]=MagOut[2]/Mag_ABS;
+}// end high pass filter
+
+// Update Pixel array based on mag sensor
+void Update_Pixels_Mag()
+{
+    // Calculate angle between magnetic vector and LED vectors
+    for (uint16_t ii=0 ; ii<LED_COUNT ; ii++) {
+        Pixel_Vect_Float[0]=((float)Pixel_Vect[ii][0])/100;
+        Pixel_Vect_Float[1]=((float)Pixel_Vect[ii][1])/100;
+        Pixel_Vect_Float[2]=((float)Pixel_Vect[ii][2])/100;
+        CosAngle=Mag_Norm[0]*Pixel_Vect_Float[0] + Mag_Norm[1]*Pixel_Vect_Float[1] + Mag_Norm[2]*Pixel_Vect_Float[2];
+        //LedPower=Mag_ABS*CosAngle*CosAngle*CosAngle*CosAngle*CosAngle;
+        LedPower=Mag_ABS*((float)pow(CosAngle,9));
+        if (LedPower>=0) {
+            if (LedPower>255) LedPower=255;
+            LedPower_Byte=(uint8_t)(LedPower);
+            colors[ii] = (rgb_color) {
+                LedPower_Byte, 0, 0
+            };
+        }
+        if (LedPower<0) {
+            if (LedPower<-255) LedPower=-255;
+            LedPower_Byte=(uint8_t)(-LedPower);
+            colors[ii] = (rgb_color) {
+                0, 0, LedPower_Byte
+            };
+        }
+    }
+}// end pixel update based on magnetic field
+
+
 // move dot throught the strip
 void dotMove(rgb_color dotColor)
 {