File content as of revision 11:f23a77c2296d:

/**
 * 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 & Ch3,
                @ 2000DPS, 2G, DLP 5Hz
                @ M-Series Random Sequence Added
                @ Servo Motor PWM (p22),
                @ Text Dump is prioritized
                @ LED Flashing to appeal ERROR State
 *
 * 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"
#include "KST_Servo.h"
#include "M-Series.h"
#include "error_led_flash.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);

long int        smpl_cnt = 0;
Ticker          ticker;
Timer           timer;

#define SampleFreq     200   // [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);
// pins already used for SPI Chip selector pin
//       p21, p23, p29, p30

// Servo Motor Control
PwmOut  pwm_(p22);

// M-Series Random Sequence
Mseries m;

// M-series update flag
#define  M_TERM  200;
int  m_cnt = M_TERM;

#define BINARY_MODE 0
#define ASCII_MODE  1
int send_mode = BINARY_MODE;

#define CSV_TITLE_COLUMN "smpl_cnt,time[sec],M_stat,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,\r\n"

void stop_dump(void);

void servo_test(void)
{
    while(1) {
        if(m.update())
            pwm_.pulsewidth_us(ONDOT_SERVO_KRS2572HV_USEC_MIN);
        else
            pwm_.pulsewidth_us(KONDO_SERVO_KRS2572HV_USEC_90);
        wait(.5);

    }
}
void init_sensor(void)
{
    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);

            if(i==0||i==2) LED_flash_error_notice(i);
            continue;
        }

        printf("Gyro_scale = %u[DPS]\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS));    //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_2G));          //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);

            if(i==0||i==2) LED_flash_error_notice(i);
            continue;
        }

        imu[i]->calib_acc();
        wait_ms(100);
        printf("Calibrated Acc\n");

        imu[i]->AK8963_calib_Magnetometer();
        wait_ms(100);
        printf("Calibrated Magnetrometer\n");
    }

}

void init(void)
{
    // servo requires a 20ms period
    pwm_.period_ms(20);

    pc.baud(baudRate);

    printf("\nrev Feb 1, 2021 for Magnetrometer by Masahiro Furukawa\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);

    init_sensor();

    // servo_test();

    printf("\nHit Key [t] to start.  Hit Key [r] to finish. [s] for binary sending.\n");

    // fixed channel usage (only Ch1, Ch3 is connected)
    imu[0]->select();       // Ch1
    imu[1]->deselect();
    imu[2]->select();       // Ch3
    imu[3]->deselect();

}

void eventFunc(void)
{
    int t_ms = timer.read_ms() - 5; // initial call takes 5ms

    // when M-series counter overflow occurs
    if(--m_cnt < 0) {
        m.update();
        if(m.get())
            pwm_.pulsewidth_us(ONDOT_SERVO_KRS2572HV_USEC_MIN);
        else
            pwm_.pulsewidth_us(KONDO_SERVO_KRS2572HV_USEC_90);
        m_cnt = M_TERM;
    }

    // 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]->read_acc();
//    imu[0]->read_rot();
//    imu[0]->AK8963_read_Magnetometer();
//
//    imu[2]->read_acc();
//    imu[2]->read_rot();
//    imu[2]->AK8963_read_Magnetometer();

    //  measurement step takes 3ms
    imu[0]->read_all();
    imu[2]->read_all();

//    imu[0]->deselect();
//    imu[1]->select();
//    imu[2]->deselect();
//    imu[3]->select();

//    imu[1]->read_acc();
//    imu[1]->read_rot();
//    imu[1]->AK8963_read_Magnetometer();

//    imu[3]->read_acc();
//    imu[3]->read_rot();
//    imu[3]->AK8963_read_Magnetometer();

    switch(send_mode) {

        case ASCII_MODE:
            printf("%ld,",smpl_cnt++);
            printf("%1.3f,",t_ms/1000.0f);
            printf("%d,",m.get());
            for(int i=0; i<3; i+=2) {
                for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->accelerometer_data[j]);
                for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->gyroscope_data[j]);
                for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->Magnetometer[j] / 1000.0f);
            }
//            printf("[mT]");
            break;

        case BINARY_MODE:

            // TO BE IMPLEMENTED ...
//            for(int i=0; i<3; i+=2) {
//                for(int j=0; j<6; j++) putc(imu[i]->accelerometer_response[j], stdout);
//                for(int j=0; j<6; j++) putc(imu[i]->gyroscope_response[j],     stdout);
//            }
            break;
    }

    putc(13, stdout);  //0x0d CR（復帰）
    putc(10, stdout);  //0x0a LF（改行）
    
    // auto-stop when 15sec after
    if(smpl_cnt>3000){stop_dump();}
}

void stop_dump(void)
{
    ticker.detach();
    timer.stop();
    smpl_cnt = 0;
}

int main()
{
    // make instances and check sensors
    init();

    char c;

    while(1) {
        if(pc.readable()) {
            c = pc.getc();

            switch(c) {
                case 'r':
                    stop_dump();
                    break;

                case 'R':
                    smpl_cnt = 0;
                    init_sensor();
                    timer.stop();
                    timer.reset();
                    break;

                case 't':
                    printf(CSV_TITLE_COLUMN);
                    smpl_cnt = 0;
                    m.init();
                    timer.reset();
                    timer.start();
                    send_mode = ASCII_MODE;
                    ticker.attach_us(eventFunc, 1000000/SampleFreq);
                    break;

                case 'c':
                    printf(CSV_TITLE_COLUMN);
                    break;

                case 's':
                    smpl_cnt = 0;
                    timer.reset();
                    timer.start();
                    send_mode = BINARY_MODE;
                    ticker.attach_us(eventFunc, 1000000/SampleFreq);
                    break;
            }
        }
    }
}