Atsumi Toda
/
Auto_pilot_prototype_3_2_os_2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 0:a24fb5835b0f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jul 25 11:06:13 2020 +0000
@@ -0,0 +1,723 @@
+//==================================================
+//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