File content as of revision 6:b450b73ff8fe:

/*
MPU9250から9軸データを取得してCAN送信・シリアル送信．
ノルムは5回移動平均から算出．
生データも送る．
姿勢角を出す．
<MPU9250ピン設定>
CS~ : HIGH
ADO : LOW 
SDA/SCL 10kΩでpullup

BMP280から気圧・温度を取得してCAN送信・シリアル送信．
気圧->5回移動平均．
<BMP280ピン設定>
CSB : HIGH
SDO : LOW
*/

#include "SDFileSystem.h"
SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd"); // the pinout on the mbed Cool Components workshop board
#include "mbed.h"
#include "BME280.h"
#include "MPU9250.h"
Serial pc(PA_9,PA_10); //pin19,20 TX,RX
BME280 bmp = BME280(PB_7, PB_6); // pin30,29 SDA,SCL
MPU9250 mpu = MPU9250(PB_7, PB_6); // pin30,29 SDA,SCL​
CAN can(PA_11, PA_12,100000); //pin21,22 rd,td
#define PI 3.14159265358979323846f
#define N 5 // 5回移動平均
#define sampleFreq 100.0f
#define beta 0.33f // gain（大きいと加速度による補正が早い）
Timer timea;
DigitalOut myled(PF_0);

union Float2Byte{
    float _float;
    char _byte[4];
};
typedef union Float2Byte Float2Byte;

union Int2Byte{
    unsigned short _int;
    char _byte[2];
};
typedef union Int2Byte Int2Byte;

float pressure_ave = 0.0f; // 気圧[hPa]
float pressure_new = 0.0f;
float buff_p[N];
float sum = 0.0f;
int cnt = 0;
float tem = 0.0f; // 温度[C]
float psi=0.0f, cta=0.0f, eta=0.0f;
float psi_f=0.0f, cta_f=0.0f, eta_f=0.0f;
float cal_x=0.0f,cal_y=0.0f,cal_z=0.0f;
float world_time;
int fpin = 0; //フライトピン判定

FILE *fp;
CANMessage msg;

void mpu_init(){
    uint8_t whoami_MPU9250, whoami_AK8963;
    float mag_init[3];
    
    whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
    whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
    pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
    pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
    
    if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){  
        pc.printf("MPU9250 is detected.\n\r");
        wait(0.1);
        mpu.resetMPU9250();
        mpu.initMPU9250(); 
        wait(0.1);
        mpu.initAK8963(mag_init);
        mpu.getGres();
        mpu.getAres();
        mpu.getMres();
        wait(0.1);
    }
   else{
        pc.printf("Could not detect MPU9250.\n\r");
        pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n", 
                  whoami_MPU9250, whoami_AK8963);
        while(1);
    }
}

void send2(unsigned short senddata1,unsigned short senddata2,unsigned short senddata3,int id){
    //pc.printf("Master send()\n\r");
    
    /*ID: 0x01*/
    Int2Byte sendInt;
    sendInt._int = senddata1;
    //ここに送りたい値を入れる．
    
    char serialData[6];
    
    serialData[0] = sendInt._byte[0];
    serialData[1] = sendInt._byte[1];
    //pc.printf("send_char: %d\n\r", serialData[i]);
    sendInt._int = senddata2;
    serialData[2] = sendInt._byte[0];
    serialData[3] = sendInt._byte[1];
 
    sendInt._int = senddata3;
    serialData[4] = sendInt._byte[0];
    serialData[5] = sendInt._byte[1];
    //pc.printf("sendFloat: %f\n\r", sendFloat._float);
    if(can.write(CANMessage(id, serialData, 6))){
        //pc.printf("Send.\n\r");
    } 
}

void receive(){
    Float2Byte getFloat;
    Int2Byte getInt;
    myled = 0;
    
    if(can.read(msg)){
        //ID: 0x01
        if(msg.id == 0x01){
            //pc.printf("ID: 0x01\n\r");
            for(int i=0;i<4;++i){
                getFloat._byte[i] = msg.data[i];
                //pc.printf("get_char: %d\n\r", getFloat._byte[i]);
            }
            world_time = getFloat._float;
            //pc.printf("world_time:%.2f\r\n", world_time);
        }
        
        if(msg.id == 0x02){
            //pc.printf("ID: 0x02\n\r");
            getInt._byte[0] = msg.data[0];
            getInt._byte[1] = msg.data[1];
            cal_x=(float)getInt._int /10.0f;
            //pc.printf("Mag:%.2f", cal_x);
            
            getInt._byte[0] = msg.data[2];
            getInt._byte[1] = msg.data[3];
            cal_y=(float)getInt._int /10.0f;
            //pc.printf(",%.2f", cal_y);
            
            getInt._byte[0] = msg.data[4];
            getInt._byte[1] = msg.data[5];
            cal_z=(float)getInt._int /10.0f;
            //pc.printf(",%.2f\r\n", cal_z);
            
            fpin++;
        }
    }
}

int main(){
    myled = 1;
    can.attach(receive, CAN::RxIrq);
    fp = fopen("/sd/log.txt","a");
    if(fp != NULL){
        fprintf(fp,"Start.\n\r");
        fprintf(fp,"time, ax_new, ay_new, az_new, gx_new, gy_new, gz_new, psi, cta, eta, pre, tem\n\r");
        fclose(fp);
    }
    else{
        pc.printf("Failed.\n\r");
    }
    
    int16_t acc[3], gyr[3], mag[3];
    int cntsd = 0;

    float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
    float gx_new = 0.0f, gy_new = 0.0f, gz_new = 0.0f;
    float psi_ax = 0.0f, cta_ay = 0.0f, eta_az = 0.0f;
    float psi_gx = 0.0f, cta_gy = 0.0f, eta_gz = 0.0f;
    float psi_drift = -5.2473f, cta_drift = 5.8882f, eta_drift = -1.8256f;
    bool dt_b = 0;
    float time[3];
    float dt = 0.0f;
    float gRes = 1000.0/32768.0;
    
    int cnt = 0;
    
    wait(0.1);
    mpu_init();
    bmp.initialize();
    
    wait(2.0); // 静止させるまで待つ
    
    // 初期値
    for(int i=0;i<N;i++){
        mpu.readGyroData(gyr);
        mpu.readAccelData(acc);
        mpu.readMagData(mag);
     
        buff_p[i] = bmp.getPressure();
        sum += buff_p[i];
    }
    
    wait(0.1);
    timea.start();
    pc.printf("Start.\n\r");
    int i = 0;
    time[0] = 0.0f;
    
    while(1){
        myled =! myled ;
        mpu.readGyroData(gyr);
        mpu.readAccelData(acc);
        mpu.readMagData(mag);

        ax_new = acc[0] / 2049.81; 
        ay_new = acc[1] / 2049.81;
        az_new = acc[2] / 2049.81;
        gx_new = gyr[0] * gRes; //[deg/s]
        gy_new = gyr[1] * gRes;
        gz_new = gyr[2] * gRes;

        cnt++;
        if(cnt == N) cnt = 0;
        
        /*姿勢計算*/
        dt_b =! dt_b;
        time[dt_b]=timea.read();
        if(fpin == 1){//フライトピンが抜けた瞬間，姿勢０
            psi = 0.0f;
            cta = 0.0f;
            eta = 0.0f;
            time[2]=timea.read();
        }else{
            dt = (time[0]-time[1]);
            if(dt > 0) dt = dt;
            else dt = -dt;
            
            psi_ax = atan2f(ay_new , az_new) * 180/PI; //[deg]
            cta_ay = atan2f(ax_new , sqrt(ay_new*ay_new + az_new*az_new)) * 180/PI;
            psi_gx = psi + gx_new * dt - (time[dt_b] - time[2]) * psi_drift;//[deg]
            cta_gy = cta + gy_new * dt - (time[dt_b] - time[2]) * cta_drift;
            eta_gz = eta + gz_new * dt - (time[dt_b] - time[2]) * eta_drift;
            psi_f = (0.8f * psi  + 0.2f * psi_ax ) ; //[deg]
            cta_f = (0.8f * cta  + 0.2f * cta_ay ) ;
            eta_f = (0.8f * eta  + 0.2f * eta_gz ) ;
            psi = (float)((int)(psi_f*1000000.0f) % 360000000) / 1000000.0f; //0~360[deg]
            cta = (float)((int)(cta_f*1000000.0f) % 360000000) / 1000000.0f;
            eta = (float)((int)(eta_f*1000000.0f) % 360000000) / 1000000.0f;
        }
        
        
        if(i == 5){
                send2((unsigned short)(psi*10.0f),(unsigned short)(cta*10.0f),(unsigned short)(eta*10.0f),4);
                i = 0;
                pc.printf("cansend");
        }
            pc.printf("angle:%f,%f,%f\n\r",psi,cta,eta);
        i++;
        
        /*気圧・気温計算*/
        sum = sum - buff_p[cnt];
        pressure_new = bmp.getPressure();
        buff_p[cnt] = pressure_new;
        sum = sum + pressure_new;
        cnt++;
        if(cnt == N) cnt = 0;
        pressure_ave = sum/N;
        tem = bmp.getTemperature();
        
        /*SD保存*/
        if (cntsd == 0){
        fp = fopen("/sd/log.txt","a");
        }
        
        if(fp != NULL){
            fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r",timea.read(),ax_new, ay_new, az_new, gx_new, gy_new, gz_new, psi, cta, eta, pressure_ave, tem);
            cntsd++;
            
            if(cntsd == 10){
                fclose(fp);
                pc.printf("SDSave.\n\r");
                cntsd = 0;
            }
        }else{
            pc.printf("SDFailed.\n\r");
        }
    }
}