Diff: main.cpp

Revision:

0:4656a133ed1a

Child:

1:3bcd844dd707

diff -r 000000000000 -r 4656a133ed1a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 01 17:11:03 2021 +0000
@@ -0,0 +1,188 @@
+/**
+ * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
+ *
+ * Dec 26, 2017
+ * Aug 29, 2018
+ * Dec 17, 2019 @ Digital Low Pass Filter 5Hz -> No LPF
+                @ ACC range 2G -> 4G
+                @ GYRO range 250 -> 500 Degree per second
+                @ Deleted magnet sensor checking
+                @ Set Acc Data Rates, Enable Acc LPF , Bandwidth 218Hz
+                @ Use DLPF set Gyroscope bandwidth 5Hz, temperature bandwidth 5Hz
+ * Feb  1, 2021 @Magnetro Meter Ch1
+ *
+ * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch4)
+ *
+ **/
+
+/*
+   https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
+
+   3.3 Magnetometer Specifications
+
+   Typical Operating Circuit of section 4.2,
+       VDD = 2.5V,
+       VDDIO = 2.5V,
+       TA=25°C, unless otherwise noted.
+
+   PARAMETER CONDITIONS                MIN         TYP         MAX         UNITS
+   MAGNETOMETER SENSITIVITY
+       Full-Scale Range                            ±4800                   µT
+       ADC Word Length                              14                    bits
+       Sensitivity Scale Factor                     0.6                   µT / LSB
+   ZERO-FIELD OUTPUT
+       Initial Calibration Tolerance               ±500                    LSB
+*/
+
+
+#include "mbed.h"
+#include "MPU9250.h"
+
+/* MPU9250 Library
+ *
+ *  https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
+ *
+ *   MOSI (Master Out Slave In)  p5
+ *   MISO (Master In Slave Out   p6
+ *   SCK  (Serial Clock)         p7
+ *   ~CS  (Chip Select)          p8 -> p30
+ */
+
+// define serial objects
+Serial          pc(USBTX, USBRX);
+
+Ticker          ticker;
+Timer           timer;
+
+#define SampleFreq     20   // [Hz]
+#define nCh             4   // number of ch
+#define baudRate     921600 //921600 / 115200
+
+unsigned int counter = 0;
+unsigned int usCycle = 1000000/SampleFreq ;
+
+int errFlag = 0;
+
+//define the mpu9250 object
+mpu9250_spi     *imu[nCh];
+
+// define SPI object for imu objects
+SPI             spi1(p5,  p6,  p7);
+SPI             spi2(p11, p12, p13);
+
+void init(void)
+{
+    pc.baud(baudRate);
+
+    printf("\nrev Feb 1, 2021 for Magnetrometer by Masahiro Furukawa\n\n");
+
+    imu[3] = new mpu9250_spi(spi2, p21);
+    imu[2] = new mpu9250_spi(spi2, p23);
+    imu[1] = new mpu9250_spi(spi1, p29);
+    imu[0] = new mpu9250_spi(spi1, p30);
+    for(int i=0; i<nCh; i++) {
+
+        imu[0]->deselect();
+        imu[1]->deselect();
+        imu[2]->deselect();
+        imu[3]->deselect();
+
+        imu[i]->select();
+
+        //INIT the mpu9250
+        //if(imu[i]->init(1,BITS_DLPF_CFG_188HZ))
+        //if(imu[i]->init(1,BITS_DLPF_CFG_5HZ))
+        if(imu[i]->init(1,BITS_DLPF_CFG_256HZ_NOLPF2)) {
+            printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i+1);
+            wait(90);
+        }
+
+        //output the I2C address to know if SPI is working, it should be 104
+        printf("\nCH %d\nWHOAMI = 0x%2x\n",i+1, imu[i]->whoami());
+
+        if(imu[i]->whoami() != 0x71) {
+            printf(" *** ERROR *** acc and gyro sensor does not respond correctly!\n");
+            errFlag |= 0x01<<(i*2);
+            continue;
+        }
+
+        printf("Gyro_scale = %u[DPS]\n",imu[i]->set_gyro_scale(BITS_FS_500DPS));    //Set 500DPS scale range for gyros    //0706 wada 500to2000
+        wait_ms(20);
+
+        printf("Acc_scale = %u[G]\n",imu[i]->set_acc_scale(BITS_FS_4G));          //Set 4G scale range for accs        //0706 wada 4to16
+        wait_ms(20);
+
+        printf("AK8963 WHIAM = 0x%2x\n",imu[i]->AK8963_whoami());
+
+        if(imu[i]->AK8963_whoami() != 0x48) {
+            printf(" *** ERROR *** magnetrometer does not respond correctly!\n");
+            errFlag |= 0x02<<(i*2);
+            continue;
+        }
+
+        imu[i]->AK8963_calib_Magnetometer();
+        wait_ms(100);
+        printf("Calibrated Magnetrometer\n");
+    }
+    
+    
+    printf("\nHit Key [s] to start.  Hit Key [r] to finish.\n");
+}
+
+void eventFunc(void)
+{
+    // limitation on sending bytes at 921600bps - 92bits(under 100us/sample)
+    // requirement : 1kHz sampling
+    // = 921.5 bits/sample
+    // = 115.1 bytes/sample
+    // = 50 bytes/axis (2byte/axis)
+
+    // 2 byte * 6 axes * 4 ch = 48 bytes/sample
+
+
+    imu[0]->select();
+    imu[1]->deselect();
+    imu[2]->select();
+    imu[3]->deselect();
+
+    imu[0]->AK8963_read_Magnetometer();
+    imu[2]->AK8963_read_Magnetometer();
+
+    imu[0]->deselect();
+    imu[1]->select();
+    imu[2]->deselect();
+    imu[3]->select();
+
+    imu[1]->AK8963_read_Magnetometer();
+    imu[3]->AK8963_read_Magnetometer();
+
+    for(int i=0; i<2; i++) {
+        for(int j=0; j<3; j++) printf("%1.3f,  ",imu[i]->Magnetometer[j] / 1000.0f);
+    }
+    printf("[mT]");
+
+    putc(13, stdout);  //0x0d CR（復帰）
+    putc(10, stdout);  //0x0a LF（改行）
+//    printf("\r\n");
+
+}
+
+int main()
+{
+    // make instances and check sensors
+    init();
+
+    char c;
+
+    while(1) {
+        if(pc.readable()) {
+            c = pc.getc();
+
+            if(c == 'r') {
+                ticker.detach();
+            } else if(c == 's') {
+                ticker.attach_us(eventFunc, 1000000.0f/SampleFreq);
+            }
+        }
+    }
+}