Atsumi Toda
/
Auto_pilot_prototype_3_2_os_2
This program is for the prototype measurement circuit of UAV.
main.cpp
- Committer:
- Joeatsumi
- Date:
- 2020-07-25
- Revision:
- 0:a24fb5835b0f
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