File content as of revision 0:a24fb5835b0f:

//==================================================
//Auto pilot(prototype3)
//
//MPU board: mbed LPC1768
//Multiplexer TC74HC157AP
//Accelerometer +Gyro sensor : BMX055
//Pressure sensor BME280
//GPS module AE-GPS
//
//2020/7/24 A.Toda
//mbed os5を使用して、9軸センサとGPS,
//気圧センサのログを取得する。
//姿勢計算や機体制御は行わない
//==================================================

#include "mbed.h"

#include "stdio.h"
#include "math.h"
#include "errno.h"
#include "ctype.h"

// Entry point for the example
#include "SDBlockDevice.h"
#include "FATFileSystem.h"

//Header file for pressure sensor
#include "BME280.h"

//Header file for inertial sensor
#include "test_BMX055.h"

Mutex stdio_mutex;
EventQueue queue1(32 * EVENTS_EVENT_SIZE);

EventQueue queue2(32 * EVENTS_EVENT_SIZE);

EventQueue queue3(32 * EVENTS_EVENT_SIZE);
//==================================================

#define INITIAL_ALTITUDE 0.0
#define DIGIT_TO_VAL(_x)        (_x - '0')
#define RAD_TO_DEG 57.3

#define MAG_OFFSET_X 0.0
#define MAG_OFFSET_Y 0.0
#define MAG_OFFSET_Z 0.0

#define GRAVITY 9.80665
//==================================================
//Port Setting
DigitalOut myled(LED1);

I2C         i2c( p9, p10 );

test_BMX055 inertial_sensor(i2c,(0x76 << 1));//(sda, scl);

Serial pc(USBTX, USBRX); // tx, rx
Serial gps(p13,p14);//tx,rx

SDBlockDevice   blockDevice(p5, p6, p7, p8);  // mosi, miso, sck, cs

DigitalIn gps_sw(p11);

DigitalIn sd_switch(p12);

//=========================================================
//Ticker
Ticker timer1; //for gps notification
Ticker timer2; //for attitude calculation

//Timer
Timer passed_time;
int dummy_counter;
//==================================================
//Timer variables
//==================================================
float time_new,time_old,time_gps_nof;
float feedback_rate = 10000; //usec

//==================================================
//Vetor variables
float quaternion[4][1],pre_quaternion[4][1],damp_1[4][1];//クォータニオン

//==================================================
//GPS variables
//==================================================
int fp_count=0;
int h_time=0,m_time=0,s_time=0;

int notification_from_sub;

float g_hokui,g_tokei;
float gps_azi;
float speed_m;

//==================================================
//Pressure variables
//==================================================
float pressure_1,temperature_1;
float altitude_1,altitude_offset;

//==================================================
//Inertial sonsor  variables
//==================================================
float gx,gy,gz;
float ax,ay,az;

float gx_mean,gy_mean,gz_mean;
int mean_counter=2000;

float limit_acc;

float euler_roll_pitch[2];

//==================================================
//Geomagnetrometer  variables
//==================================================
float mx,my,mz;
float yaw;

//==================================================
//Predeclaration for functions
//==================================================

void gps_receiver();
void gps_receiver_2();
void gps_receiver_3();

void gps_notification();
void inertial_sensor_function();

float get_altitude(float offset);

int gyro_float2int(float gyro_data);
int acc_float2int(float acc_data);
//=========================================================
//SD file 
//=========================================================
// File system declaration
FATFileSystem   fileSystem("fs");
int err;
int sd_flag;


int TimeIndex[1] = {0};
char Six_axis_Index[1] = {'A'};
char GPSIndex[1] = {'G'};

char mag_ir_Index[1] = {'M'};

//ジャイロデータ
int gx_int[1] = {0};
int gy_int[1] = {0};
int gz_int[1] = {0};

//加速度
int ax_int[1] = {0};
int ay_int[1] = {0};
int az_int[1] = {0};

//緯度
int lat_int[1] = {0};
//経度
int longi_int[1] = {0};
//移動方位
int m_azi_int[1] = {0};
//対地速度
int gspeed_int[1] = {0};
//大気圧高度
int altitude_int[1] = {0};

//x軸地磁気
int mx_int[1] = {0};
//y軸地磁気
int my_int[1] = {0};
//z軸地磁気
int mz_int[1] = {0};

//ダミーデータ
char Dump_Index[1] = {'D'};

FILE*   f;

void sd_open_first(){
    
    printf("--- Mbed OS filesystem example ---\n");

    // Try to mount the filesystem
    printf("Mounting the filesystem... ");
    fflush(stdout);

    err = fileSystem.mount(&blockDevice);
    printf("%s\n", (err ? "Fail :(" : "OK"));
    
    if (err) {
        // Reformat if we can't mount the filesystem
        // this should only happen on the first boot
        printf("No filesystem found, formatting... ");
        fflush(stdout);
        err = fileSystem.reformat(&blockDevice);
        printf("%s\n", (err ? "Fail :(" : "OK"));
        if (err) {
            error("error: %s (%d)\n", strerror(-err), err);
        }
    }

    // Open the numbers file
    printf("Opening \"/fs/numbers.dat\"... ");
    fflush(stdout);

    //====================
    //センサ用のファイル
    //====================
    //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
    f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
    
    printf("%s\n", (!f ? "Fail :(" : "OK"));
    if (!f) {
        // Create the numbers file if it doesn't exist
        printf("No file found, creating a new file... ");
        fflush(stdout);
        //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
        f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
        
        printf("%s\n", (!f ? "Fail :(" : "OK"));
        if (!f) {
            error("error: %s (%d)\n", strerror(errno), -errno);
        }
        
        fflush(stdout);
    }
    
}//sd open first


void sd_close(){
    
    // Close the file which also flushes any cached writes
    printf("Closing \"/fs/numbers.dat\"... ");
    fflush(stdout);
    err = fclose(f);
    printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
    if (err < 0) {
        error("error: %s (%d)\n", strerror(errno), -errno);
    }
    
    printf("Mbed OS filesystem example done!\n");
    
}

void sd_open(){
    
    //====================
    //センサ用のファイル
    //====================
    //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
    f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
    
    printf("%s\n", (!f ? "Fail :(" : "OK"));
    if (!f) {
        // Create the numbers file if it doesn't exist
        printf("No file found, creating a new file... ");
        fflush(stdout);
        //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
        f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
        printf("%s\n", (!f ? "Fail :(" : "OK"));
        if (!f) {
            error("error: %s (%d)\n", strerror(errno), -errno);
        }
        
        fflush(stdout);
    }
    
}//sd open first
//=========================================================
//Gps receiver function
//=========================================================
void gps_receiver()
{

    //variables
    int numbers_of_nmea;
    char gps_data[256];



    numbers_of_nmea=0;
    while(gps.getc()!='$') {
    }

    while( (gps_data[numbers_of_nmea]=gps.getc()) != '\r') {

        numbers_of_nmea++;
        if(numbers_of_nmea==256) {
            numbers_of_nmea=255;
            break;
        }
    }

    gps_data[numbers_of_nmea]='\0';

    //char gps_datae[256]="$GNRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
    if( sscanf(gps_data, "GNRMC,%f,%f,%f,%f\r",&g_hokui,&g_tokei,&gps_azi,&speed_m) >= 1) {
        //pc.printf("変換前データ %f,%f,%f\r\n",g_hokui,g_tokei,azi);
    } else {
    }//if ends
}


//=========================================================
//gps notification function
//=========================================================
void gps_notification()
{
    //pc.printf("processing\r\n");
    if(gps_sw==1) {
        notification_from_sub=1;
         time_gps_nof=passed_time.read_ms();
    } else {
        notification_from_sub=0;
    }
    
}

void observation_update()
{
    if((sd_switch==1)&&(sd_flag==0)){
        sd_open();
        sd_flag=1;
        pc.printf("Logging was started.\r\n");
    }else if((sd_switch==1)&&(sd_flag==1)){
        //何もしない
    }else if((sd_switch==0)&&(sd_flag==1)){
        sd_close();
        sd_flag=0;
        pc.printf("Logging was closed.\r\n");
    }else if((sd_switch==0)&&(sd_flag==0)){
        //何もしない
    }
    
    if(notification_from_sub==1) {
        
        //最新の時間の取得
        time_new=passed_time.read_ms();
        //離散時間の更新
        time_old=time_new;
        
        if((time_new-time_gps_nof)<=10.0){
            
            myled=1;
            
            //gpsプロセッサからのシリアル信号を受信
            gps_receiver();
            notification_from_sub=0;
            //高度情報の取得
            altitude_1=get_altitude(altitude_offset);
            
            //処理時間の計算
            //最新の時間の取得
            time_new=passed_time.read_ms();
            //pc.printf("%f\r\n",time_new-time_old);
            //
            //デバッグの為の表示
            //pc.printf("%f,%f,%f,%f,%f\r\n",g_hokui,g_tokei,altitude_1,speed_m,gps_azi);
        
            //地磁気センサの計測
            mx = inertial_sensor.get_mag_x_data();
            my = inertial_sensor.get_mag_y_data();
            mz = inertial_sensor.get_mag_z_data();
            //デバッグの為の表示
            //pc.printf("%f,%f,%f\r\n",mx,my,mz);
            
            myled=0;
            
            if(sd_flag==1){                    
                
                //緯度
                lat_int[0] = int(g_hokui*1000000.0);
                //経度
                longi_int[0] = int(g_tokei*1000000.0);
                //移動方位
                m_azi_int[0] = int(gps_azi*1000000.0);
                //対地速度
                gspeed_int[0] = int(speed_m*100.0);
                //大気圧高度
                altitude_int[0] = int((altitude_1+50000.0)*100.0);
                
                //地磁気センサ x,y軸
                //8192
                //x軸地磁気
                mx_int[0] = int(mx+8192.0);
                //y軸地磁気
                my_int[0] = int(my+8192.0);
                //地磁気センサ z軸
                mz_int[0] = int(mz+32768.0);
                
                //GPSデータ列
                err =fwrite(GPSIndex, 1, 1, f);
                err =fwrite(TimeIndex, 1, 4, f);
    
                err =fwrite(lat_int, 1, 4, f);
                err =fwrite(longi_int, 1, 4, f);
                err =fwrite(m_azi_int, 1, 4, f);
                err =fwrite(gspeed_int, 1, 4, f);
                err =fwrite(altitude_int, 1, 4, f);
                
                //Dump_Index パケットを32byteにするための埋め合わせ
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                
                //地磁気と赤外線センサ
                err =fwrite(mag_ir_Index, 1, 1, f);
                err =fwrite(TimeIndex, 1, 4, f);
    
                err =fwrite(mx_int, 1, 4, f);
                err =fwrite(my_int, 1, 4, f);
                err =fwrite(mz_int, 1, 4, f);

                //Dump_Index パケットを32byteにするための埋め合わせ
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                err =fwrite(Dump_Index, 1, 1, f);
                
            }else{}
        }else{notification_from_sub=0;}
        
    }else{

        }
}

void inertial_sensor_function()
{     
        int counter;

        gx=inertial_sensor.get_gyro_x_data();
        gy=inertial_sensor.get_gyro_y_data();
        gz=inertial_sensor.get_gyro_z_data();
        
        ax = inertial_sensor.get_acc_x_data();
        ay = inertial_sensor.get_acc_y_data();
        az = inertial_sensor.get_acc_z_data();
            
        gx -=gx_mean; 
        gy -=gy_mean;
        gz -=gz_mean;
            
        //最新の時間の取得
        time_new=passed_time.read_ms();
        
        //離散時間の更新
        time_old=time_new;
    
        if(sd_flag==1){

            //バイナリ形式でsdカードに書き込む
            TimeIndex[0] = int(time_new);
            
            //ジャイロのレンジは+-250deg/s
            //ジャイロセンサ計測値をintにする
            
            int gx_pre,gy_pre,gz_pre;
            /*
            gx_pre = gyro_float2int(gx);
            gy_pre = gyro_float2int(gy);
            gz_pre = gyro_float2int(gz);
            */
            gx_pre = int((gx+500.0)*10000.0);//0.1 m deg/sの分解能とする。このレンジでの分解能は7.6m deg/s
            gy_pre = int((gy+500.0)*10000.0);
            gz_pre = int((gz+500.0)*10000.0);
            
            gx_int[0] = gx_pre;
            gy_int[0] = gy_pre;
            gz_int[0] = gz_pre;
            
            //加速度センサ計測値をintにする
                        
            int ax_pre,ay_pre,az_pre;
            /*
            ax_pre = acc_float2int(ax);
            ay_pre = acc_float2int(ay);
            az_pre = acc_float2int(az);
            */
            ax_pre = int((ax+GRAVITY*16.0)*100000.0);//±8g3.91mg/LSB 0.01mg
            ay_pre = int((ay+GRAVITY*16.0)*100000.0);
            az_pre = int((az+GRAVITY*16.0)*100000.0);
            
            ax_int[0] = ax_pre;
            ay_int[0] = ay_pre;
            az_int[0] = az_pre;
            
            //sdカードへの書き込み
            //慣性センサのヘッダ
            //Six_axis_Index[1] = {'A'};
            
            err =fwrite(Six_axis_Index, 1, 1, f);
            err =fwrite(TimeIndex, 1, 4, f);

            err =fwrite(gx_int, 1, 4, f);
            err =fwrite(gy_int, 1, 4, f);
            err =fwrite(gz_int, 1, 4, f);
            
            err =fwrite(ax_int, 1, 4, f);
            err =fwrite(ay_int, 1, 4, f);
            err =fwrite(az_int, 1, 4, f);
            
            //Dump_Index パケットを32byteにするための埋め合わせ
            err =fwrite(Dump_Index, 1, 1, f);
            err =fwrite(Dump_Index, 1, 1, f);
            err =fwrite(Dump_Index, 1, 1, f);
            
            
        }else{}
    
    
}

//gps_notification
void sensor_function(){
    
    inertial_sensor_function();
    gps_notification();
    
    }
//=========================================================
//altitude function
//=========================================================
float get_altitude(float offset)
{

    pressure_1=inertial_sensor.getPressure();
    //pressure_1=pressure_sensor.getPressure();
    
    temperature_1=inertial_sensor.getTemperature();
    //temperature_1=pressure_sensor.getTemperature();
    //pressure_sensor
    
    float altitude=pow((1013.25/pressure_1),0.190);
    altitude-=1.0;
    altitude*=(temperature_1+273.15);
    altitude/=0.0065;
    altitude-=offset;
    altitude+=INITIAL_ALTITUDE;

    //デバッグ用の表示
    pc.printf("pressure=%f,tempreture=%f,altitude=%f\r\n",pressure_1,temperature_1,altitude);
    
    return altitude;

}

//=========================================================
//Timer function
//=========================================================
void timer_reset()
{
    passed_time.reset();//reset timer counter
}

void timer_read()
{
    time_new=passed_time.read_ms();
    //pc.printf("経過時間=%f\r\n",time_new);
}

//float型のgyroデータをintに変換する
int gyro_float2int(float gyro_data){
    
    int gyro_int_data;
    
    if(gyro_data>=0.0){
        //gyro_scale_factor=0.0305f;
        //gyro_scale_factor)/RAD_TO_DEG
        gyro_int_data = int(gyro_data*RAD_TO_DEG/0.0305f);
    }else if(gyro_data<0.0){
        /*
        x_data = -1 * (65536 - x_data);
        */
        gyro_int_data = int((gyro_data*RAD_TO_DEG)/0.0305f);
        gyro_int_data=gyro_int_data+65536;
        
        }
    
    return gyro_int_data;
}

//float型のaccデータをintに変換する
int acc_float2int(float acc_data){
    
    int acc_int_data;
    
    if(acc_data>=0.0){
        //acc_scale_factor=256;
        //x_acc=float(x_data)*GRAVITY/acc_scale_factor;//m/s^2
        acc_int_data = int(acc_data*256/GRAVITY);
        
    }else if(acc_data<0.0){
        //x_data = -1 * (4096 - x_data);  
        
        acc_int_data = int(acc_data*256/GRAVITY);
        acc_int_data=acc_int_data+4096;
        
        }
    
    return acc_int_data;
}


//=========================================================
// main
//=========================================================
int main()
{
    
    //UART initialization
    pc.baud(115200); //115.2 kbps
    gps.baud(115200); //115.2 kbps
    
    blockDevice.frequency(25000000);
    
    /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
    //Mount the filesystem
    sd_flag=0;
    sd_open_first();
    
    //Range setting for inertial sensor
    //慣性センサのレンジ設定
    inertial_sensor.set_gyro_range(3);//±250°/s131.2 LSB/°/s 7.6 m°/s / LSB
    inertial_sensor.set_acc_range(2);//±8g3.91mg/LSB

    //地磁気センサの初期化
    inertial_sensor.initiate_mag();
    
    //Initialization for pressure sensor
    //気圧センサの初期化
    //pressure_sensor.initialize();
    inertial_sensor.initialize();
               
    notification_from_sub=0;//サブマイコンからの通知を初期化
    dummy_counter=0;

    //-------------------------------------------
    //altitude initialization
    /*
    2回目からの計測でまともな高度が得られるので、オフセット計測を2回する
    */
    //-------------------------------------------
    altitude_offset=get_altitude(0.0);//初期高度を取得（オフセットの取得)
    wait_us (100000);//wait 0.1 s
    altitude_offset=get_altitude(0.0);//初期高度を取得（オフセットの取得）

    //------------------------------------------
    //ジャイロオフセットの計算
    gx_mean=gy_mean=gz_mean=0.0;
    
    pc.printf("offset calculation.\r\n");
    
    for(int counter=0;counter<mean_counter;counter++){
        
        gx=inertial_sensor.get_gyro_x_data();
        gy=inertial_sensor.get_gyro_y_data();
        gz=inertial_sensor.get_gyro_z_data();
    
        gx_mean+=gx;
        gy_mean+=gy;
        gz_mean+=gz;
        wait_us(10000);
        }
    
    gx_mean/=mean_counter;
    gy_mean/=mean_counter;
    gz_mean/=mean_counter;
    
    pc.printf("offset calculation has been done.\r\n");
    //-------------------------------------------
    //Thread
    //-------------------------------------------
    //set priority of sensor threads
    Thread thread1(osPriorityAboveNormal);
    Thread thread2(osPriorityNormal);
    
    //set prioority of main
    osThreadSetPriority(osThreadGetId(), osPriorityIdle);
    
    //start threads
    thread1.start(callback(&queue1, &EventQueue::dispatch_forever));
    thread2.start(callback(&queue2, &EventQueue::dispatch_forever));
    
    pc.printf("Ticker setup has been done.\r\n");
    
    //-------------------------------------------
    //Timer starts
    //-------------------------------------------
    passed_time.start();
    
    pc.printf("Activated.\r\n");
    
    //thread are called every 1 or 5ms
    queue1.call_every(10,&observation_update);//observation_update
    queue2.call_every(10,&sensor_function);//sensor_function
    
    while(1) {
        
    }// while(1)ends
}//int main ends