File content as of revision 2:3470f2c07582:

/**
 * 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"
#include "KST_Servo.h"
#include "M-Series.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);
// pins already used for SPI Chip selector pin
//       p21, p23, p29, p30

// Servo Motor Control
PwmOut  pwm_(p22);

// M-Series Random Sequence
Mseries m;


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");


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

    while(1) {
        if(m.update())
            pwm_.pulsewidth_us(KST_SERVO_USEC_MIN);
        else
            pwm_.pulsewidth_us(KST_SERVO_USEC_90);
        wait(.5);

    }
}

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);
            }
        }
    }
}