Atsumi Toda / Mbed OS Auto_pilot_prototype_3_2_os_2

This program is for the prototype measurement circuit of UAV.

Dependencies:   test_BMX055

main.cpp@0:a24fb5835b0f, 2020-07-25 (annotated)

Committer:
Joeatsumi
Date:
Sat Jul 25 11:06:13 2020 +0000
Revision:
0:a24fb5835b0f
This program is for the prototype measurement circuit of UAV.; The circuit consisted of mbed LPC1768 ,BMX055,BME280 and microSD slot.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:a24fb5835b0f 1 //==================================================
Joeatsumi 0:a24fb5835b0f 2 //Auto pilot(prototype3)
Joeatsumi 0:a24fb5835b0f 3 //
Joeatsumi 0:a24fb5835b0f 4 //MPU board: mbed LPC1768
Joeatsumi 0:a24fb5835b0f 5 //Multiplexer TC74HC157AP
Joeatsumi 0:a24fb5835b0f 6 //Accelerometer +Gyro sensor : BMX055
Joeatsumi 0:a24fb5835b0f 7 //Pressure sensor BME280
Joeatsumi 0:a24fb5835b0f 8 //GPS module AE-GPS
Joeatsumi 0:a24fb5835b0f 9 //
Joeatsumi 0:a24fb5835b0f 10 //2020/7/24 A.Toda
Joeatsumi 0:a24fb5835b0f 11 //mbed os5を使用して、9軸センサとGPS,
Joeatsumi 0:a24fb5835b0f 12 //気圧センサのログを取得する。
Joeatsumi 0:a24fb5835b0f 13 //姿勢計算や機体制御は行わない
Joeatsumi 0:a24fb5835b0f 14 //==================================================
Joeatsumi 0:a24fb5835b0f 15
Joeatsumi 0:a24fb5835b0f 16 #include "mbed.h"
Joeatsumi 0:a24fb5835b0f 17
Joeatsumi 0:a24fb5835b0f 18 #include "stdio.h"
Joeatsumi 0:a24fb5835b0f 19 #include "math.h"
Joeatsumi 0:a24fb5835b0f 20 #include "errno.h"
Joeatsumi 0:a24fb5835b0f 21 #include "ctype.h"
Joeatsumi 0:a24fb5835b0f 22
Joeatsumi 0:a24fb5835b0f 23 // Entry point for the example
Joeatsumi 0:a24fb5835b0f 24 #include "SDBlockDevice.h"
Joeatsumi 0:a24fb5835b0f 25 #include "FATFileSystem.h"
Joeatsumi 0:a24fb5835b0f 26
Joeatsumi 0:a24fb5835b0f 27 //Header file for pressure sensor
Joeatsumi 0:a24fb5835b0f 28 #include "BME280.h"
Joeatsumi 0:a24fb5835b0f 29
Joeatsumi 0:a24fb5835b0f 30 //Header file for inertial sensor
Joeatsumi 0:a24fb5835b0f 31 #include "test_BMX055.h"
Joeatsumi 0:a24fb5835b0f 32
Joeatsumi 0:a24fb5835b0f 33 Mutex stdio_mutex;
Joeatsumi 0:a24fb5835b0f 34 EventQueue queue1(32 * EVENTS_EVENT_SIZE);
Joeatsumi 0:a24fb5835b0f 35
Joeatsumi 0:a24fb5835b0f 36 EventQueue queue2(32 * EVENTS_EVENT_SIZE);
Joeatsumi 0:a24fb5835b0f 37
Joeatsumi 0:a24fb5835b0f 38 EventQueue queue3(32 * EVENTS_EVENT_SIZE);
Joeatsumi 0:a24fb5835b0f 39 //==================================================
Joeatsumi 0:a24fb5835b0f 40
Joeatsumi 0:a24fb5835b0f 41 #define INITIAL_ALTITUDE 0.0
Joeatsumi 0:a24fb5835b0f 42 #define DIGIT_TO_VAL(_x) (_x - '0')
Joeatsumi 0:a24fb5835b0f 43 #define RAD_TO_DEG 57.3
Joeatsumi 0:a24fb5835b0f 44
Joeatsumi 0:a24fb5835b0f 45 #define MAG_OFFSET_X 0.0
Joeatsumi 0:a24fb5835b0f 46 #define MAG_OFFSET_Y 0.0
Joeatsumi 0:a24fb5835b0f 47 #define MAG_OFFSET_Z 0.0
Joeatsumi 0:a24fb5835b0f 48
Joeatsumi 0:a24fb5835b0f 49 #define GRAVITY 9.80665
Joeatsumi 0:a24fb5835b0f 50 //==================================================
Joeatsumi 0:a24fb5835b0f 51 //Port Setting
Joeatsumi 0:a24fb5835b0f 52 DigitalOut myled(LED1);
Joeatsumi 0:a24fb5835b0f 53
Joeatsumi 0:a24fb5835b0f 54 I2C i2c( p9, p10 );
Joeatsumi 0:a24fb5835b0f 55
Joeatsumi 0:a24fb5835b0f 56 test_BMX055 inertial_sensor(i2c,(0x76 << 1));//(sda, scl);
Joeatsumi 0:a24fb5835b0f 57
Joeatsumi 0:a24fb5835b0f 58 Serial pc(USBTX, USBRX); // tx, rx
Joeatsumi 0:a24fb5835b0f 59 Serial gps(p13,p14);//tx,rx
Joeatsumi 0:a24fb5835b0f 60
Joeatsumi 0:a24fb5835b0f 61 SDBlockDevice blockDevice(p5, p6, p7, p8); // mosi, miso, sck, cs
Joeatsumi 0:a24fb5835b0f 62
Joeatsumi 0:a24fb5835b0f 63 DigitalIn gps_sw(p11);
Joeatsumi 0:a24fb5835b0f 64
Joeatsumi 0:a24fb5835b0f 65 DigitalIn sd_switch(p12);
Joeatsumi 0:a24fb5835b0f 66
Joeatsumi 0:a24fb5835b0f 67 //=========================================================
Joeatsumi 0:a24fb5835b0f 68 //Ticker
Joeatsumi 0:a24fb5835b0f 69 Ticker timer1; //for gps notification
Joeatsumi 0:a24fb5835b0f 70 Ticker timer2; //for attitude calculation
Joeatsumi 0:a24fb5835b0f 71
Joeatsumi 0:a24fb5835b0f 72 //Timer
Joeatsumi 0:a24fb5835b0f 73 Timer passed_time;
Joeatsumi 0:a24fb5835b0f 74 int dummy_counter;
Joeatsumi 0:a24fb5835b0f 75 //==================================================
Joeatsumi 0:a24fb5835b0f 76 //Timer variables
Joeatsumi 0:a24fb5835b0f 77 //==================================================
Joeatsumi 0:a24fb5835b0f 78 float time_new,time_old,time_gps_nof;
Joeatsumi 0:a24fb5835b0f 79 float feedback_rate = 10000; //usec
Joeatsumi 0:a24fb5835b0f 80
Joeatsumi 0:a24fb5835b0f 81 //==================================================
Joeatsumi 0:a24fb5835b0f 82 //Vetor variables
Joeatsumi 0:a24fb5835b0f 83 float quaternion[4][1],pre_quaternion[4][1],damp_1[4][1];//クォータニオン
Joeatsumi 0:a24fb5835b0f 84
Joeatsumi 0:a24fb5835b0f 85 //==================================================
Joeatsumi 0:a24fb5835b0f 86 //GPS variables
Joeatsumi 0:a24fb5835b0f 87 //==================================================
Joeatsumi 0:a24fb5835b0f 88 int fp_count=0;
Joeatsumi 0:a24fb5835b0f 89 int h_time=0,m_time=0,s_time=0;
Joeatsumi 0:a24fb5835b0f 90
Joeatsumi 0:a24fb5835b0f 91 int notification_from_sub;
Joeatsumi 0:a24fb5835b0f 92
Joeatsumi 0:a24fb5835b0f 93 float g_hokui,g_tokei;
Joeatsumi 0:a24fb5835b0f 94 float gps_azi;
Joeatsumi 0:a24fb5835b0f 95 float speed_m;
Joeatsumi 0:a24fb5835b0f 96
Joeatsumi 0:a24fb5835b0f 97 //==================================================
Joeatsumi 0:a24fb5835b0f 98 //Pressure variables
Joeatsumi 0:a24fb5835b0f 99 //==================================================
Joeatsumi 0:a24fb5835b0f 100 float pressure_1,temperature_1;
Joeatsumi 0:a24fb5835b0f 101 float altitude_1,altitude_offset;
Joeatsumi 0:a24fb5835b0f 102
Joeatsumi 0:a24fb5835b0f 103 //==================================================
Joeatsumi 0:a24fb5835b0f 104 //Inertial sonsor variables
Joeatsumi 0:a24fb5835b0f 105 //==================================================
Joeatsumi 0:a24fb5835b0f 106 float gx,gy,gz;
Joeatsumi 0:a24fb5835b0f 107 float ax,ay,az;
Joeatsumi 0:a24fb5835b0f 108
Joeatsumi 0:a24fb5835b0f 109 float gx_mean,gy_mean,gz_mean;
Joeatsumi 0:a24fb5835b0f 110 int mean_counter=2000;
Joeatsumi 0:a24fb5835b0f 111
Joeatsumi 0:a24fb5835b0f 112 float limit_acc;
Joeatsumi 0:a24fb5835b0f 113
Joeatsumi 0:a24fb5835b0f 114 float euler_roll_pitch[2];
Joeatsumi 0:a24fb5835b0f 115
Joeatsumi 0:a24fb5835b0f 116 //==================================================
Joeatsumi 0:a24fb5835b0f 117 //Geomagnetrometer variables
Joeatsumi 0:a24fb5835b0f 118 //==================================================
Joeatsumi 0:a24fb5835b0f 119 float mx,my,mz;
Joeatsumi 0:a24fb5835b0f 120 float yaw;
Joeatsumi 0:a24fb5835b0f 121
Joeatsumi 0:a24fb5835b0f 122 //==================================================
Joeatsumi 0:a24fb5835b0f 123 //Predeclaration for functions
Joeatsumi 0:a24fb5835b0f 124 //==================================================
Joeatsumi 0:a24fb5835b0f 125
Joeatsumi 0:a24fb5835b0f 126 void gps_receiver();
Joeatsumi 0:a24fb5835b0f 127 void gps_receiver_2();
Joeatsumi 0:a24fb5835b0f 128 void gps_receiver_3();
Joeatsumi 0:a24fb5835b0f 129
Joeatsumi 0:a24fb5835b0f 130 void gps_notification();
Joeatsumi 0:a24fb5835b0f 131 void inertial_sensor_function();
Joeatsumi 0:a24fb5835b0f 132
Joeatsumi 0:a24fb5835b0f 133 float get_altitude(float offset);
Joeatsumi 0:a24fb5835b0f 134
Joeatsumi 0:a24fb5835b0f 135 int gyro_float2int(float gyro_data);
Joeatsumi 0:a24fb5835b0f 136 int acc_float2int(float acc_data);
Joeatsumi 0:a24fb5835b0f 137 //=========================================================
Joeatsumi 0:a24fb5835b0f 138 //SD file
Joeatsumi 0:a24fb5835b0f 139 //=========================================================
Joeatsumi 0:a24fb5835b0f 140 // File system declaration
Joeatsumi 0:a24fb5835b0f 141 FATFileSystem fileSystem("fs");
Joeatsumi 0:a24fb5835b0f 142 int err;
Joeatsumi 0:a24fb5835b0f 143 int sd_flag;
Joeatsumi 0:a24fb5835b0f 144
Joeatsumi 0:a24fb5835b0f 145
Joeatsumi 0:a24fb5835b0f 146 int TimeIndex[1] = {0};
Joeatsumi 0:a24fb5835b0f 147 char Six_axis_Index[1] = {'A'};
Joeatsumi 0:a24fb5835b0f 148 char GPSIndex[1] = {'G'};
Joeatsumi 0:a24fb5835b0f 149
Joeatsumi 0:a24fb5835b0f 150 char mag_ir_Index[1] = {'M'};
Joeatsumi 0:a24fb5835b0f 151
Joeatsumi 0:a24fb5835b0f 152 //ジャイロデータ
Joeatsumi 0:a24fb5835b0f 153 int gx_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 154 int gy_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 155 int gz_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 156
Joeatsumi 0:a24fb5835b0f 157 //加速度
Joeatsumi 0:a24fb5835b0f 158 int ax_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 159 int ay_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 160 int az_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 161
Joeatsumi 0:a24fb5835b0f 162 //緯度
Joeatsumi 0:a24fb5835b0f 163 int lat_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 164 //経度
Joeatsumi 0:a24fb5835b0f 165 int longi_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 166 //移動方位
Joeatsumi 0:a24fb5835b0f 167 int m_azi_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 168 //対地速度
Joeatsumi 0:a24fb5835b0f 169 int gspeed_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 170 //大気圧高度
Joeatsumi 0:a24fb5835b0f 171 int altitude_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 172
Joeatsumi 0:a24fb5835b0f 173 //x軸地磁気
Joeatsumi 0:a24fb5835b0f 174 int mx_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 175 //y軸地磁気
Joeatsumi 0:a24fb5835b0f 176 int my_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 177 //z軸地磁気
Joeatsumi 0:a24fb5835b0f 178 int mz_int[1] = {0};
Joeatsumi 0:a24fb5835b0f 179
Joeatsumi 0:a24fb5835b0f 180 //ダミーデータ
Joeatsumi 0:a24fb5835b0f 181 char Dump_Index[1] = {'D'};
Joeatsumi 0:a24fb5835b0f 182
Joeatsumi 0:a24fb5835b0f 183 FILE* f;
Joeatsumi 0:a24fb5835b0f 184
Joeatsumi 0:a24fb5835b0f 185 void sd_open_first(){
Joeatsumi 0:a24fb5835b0f 186
Joeatsumi 0:a24fb5835b0f 187 printf("--- Mbed OS filesystem example ---\n");
Joeatsumi 0:a24fb5835b0f 188
Joeatsumi 0:a24fb5835b0f 189 // Try to mount the filesystem
Joeatsumi 0:a24fb5835b0f 190 printf("Mounting the filesystem... ");
Joeatsumi 0:a24fb5835b0f 191 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 192
Joeatsumi 0:a24fb5835b0f 193 err = fileSystem.mount(&blockDevice);
Joeatsumi 0:a24fb5835b0f 194 printf("%s\n", (err ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 195
Joeatsumi 0:a24fb5835b0f 196 if (err) {
Joeatsumi 0:a24fb5835b0f 197 // Reformat if we can't mount the filesystem
Joeatsumi 0:a24fb5835b0f 198 // this should only happen on the first boot
Joeatsumi 0:a24fb5835b0f 199 printf("No filesystem found, formatting... ");
Joeatsumi 0:a24fb5835b0f 200 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 201 err = fileSystem.reformat(&blockDevice);
Joeatsumi 0:a24fb5835b0f 202 printf("%s\n", (err ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 203 if (err) {
Joeatsumi 0:a24fb5835b0f 204 error("error: %s (%d)\n", strerror(-err), err);
Joeatsumi 0:a24fb5835b0f 205 }
Joeatsumi 0:a24fb5835b0f 206 }
Joeatsumi 0:a24fb5835b0f 207
Joeatsumi 0:a24fb5835b0f 208 // Open the numbers file
Joeatsumi 0:a24fb5835b0f 209 printf("Opening \"/fs/numbers.dat\"... ");
Joeatsumi 0:a24fb5835b0f 210 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 211
Joeatsumi 0:a24fb5835b0f 212 //====================
Joeatsumi 0:a24fb5835b0f 213 //センサ用のファイル
Joeatsumi 0:a24fb5835b0f 214 //====================
Joeatsumi 0:a24fb5835b0f 215 //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
Joeatsumi 0:a24fb5835b0f 216 f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
Joeatsumi 0:a24fb5835b0f 217
Joeatsumi 0:a24fb5835b0f 218 printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 219 if (!f) {
Joeatsumi 0:a24fb5835b0f 220 // Create the numbers file if it doesn't exist
Joeatsumi 0:a24fb5835b0f 221 printf("No file found, creating a new file... ");
Joeatsumi 0:a24fb5835b0f 222 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 223 //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
Joeatsumi 0:a24fb5835b0f 224 f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
Joeatsumi 0:a24fb5835b0f 225
Joeatsumi 0:a24fb5835b0f 226 printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 227 if (!f) {
Joeatsumi 0:a24fb5835b0f 228 error("error: %s (%d)\n", strerror(errno), -errno);
Joeatsumi 0:a24fb5835b0f 229 }
Joeatsumi 0:a24fb5835b0f 230
Joeatsumi 0:a24fb5835b0f 231 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 232 }
Joeatsumi 0:a24fb5835b0f 233
Joeatsumi 0:a24fb5835b0f 234 }//sd open first
Joeatsumi 0:a24fb5835b0f 235
Joeatsumi 0:a24fb5835b0f 236
Joeatsumi 0:a24fb5835b0f 237 void sd_close(){
Joeatsumi 0:a24fb5835b0f 238
Joeatsumi 0:a24fb5835b0f 239 // Close the file which also flushes any cached writes
Joeatsumi 0:a24fb5835b0f 240 printf("Closing \"/fs/numbers.dat\"... ");
Joeatsumi 0:a24fb5835b0f 241 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 242 err = fclose(f);
Joeatsumi 0:a24fb5835b0f 243 printf("%s\n", (err < 0 ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 244 if (err < 0) {
Joeatsumi 0:a24fb5835b0f 245 error("error: %s (%d)\n", strerror(errno), -errno);
Joeatsumi 0:a24fb5835b0f 246 }
Joeatsumi 0:a24fb5835b0f 247
Joeatsumi 0:a24fb5835b0f 248 printf("Mbed OS filesystem example done!\n");
Joeatsumi 0:a24fb5835b0f 249
Joeatsumi 0:a24fb5835b0f 250 }
Joeatsumi 0:a24fb5835b0f 251
Joeatsumi 0:a24fb5835b0f 252 void sd_open(){
Joeatsumi 0:a24fb5835b0f 253
Joeatsumi 0:a24fb5835b0f 254 //====================
Joeatsumi 0:a24fb5835b0f 255 //センサ用のファイル
Joeatsumi 0:a24fb5835b0f 256 //====================
Joeatsumi 0:a24fb5835b0f 257 //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
Joeatsumi 0:a24fb5835b0f 258 f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
Joeatsumi 0:a24fb5835b0f 259
Joeatsumi 0:a24fb5835b0f 260 printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 261 if (!f) {
Joeatsumi 0:a24fb5835b0f 262 // Create the numbers file if it doesn't exist
Joeatsumi 0:a24fb5835b0f 263 printf("No file found, creating a new file... ");
Joeatsumi 0:a24fb5835b0f 264 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 265 //f = fopen("/fs/numbers.txt", "a+");//テキスト形式の書き込み
Joeatsumi 0:a24fb5835b0f 266 f = fopen("/fs/numbers.dat", "ab+");//バイナリ形式の書き込み
Joeatsumi 0:a24fb5835b0f 267 printf("%s\n", (!f ? "Fail :(" : "OK"));
Joeatsumi 0:a24fb5835b0f 268 if (!f) {
Joeatsumi 0:a24fb5835b0f 269 error("error: %s (%d)\n", strerror(errno), -errno);
Joeatsumi 0:a24fb5835b0f 270 }
Joeatsumi 0:a24fb5835b0f 271
Joeatsumi 0:a24fb5835b0f 272 fflush(stdout);
Joeatsumi 0:a24fb5835b0f 273 }
Joeatsumi 0:a24fb5835b0f 274
Joeatsumi 0:a24fb5835b0f 275 }//sd open first
Joeatsumi 0:a24fb5835b0f 276 //=========================================================
Joeatsumi 0:a24fb5835b0f 277 //Gps receiver function
Joeatsumi 0:a24fb5835b0f 278 //=========================================================
Joeatsumi 0:a24fb5835b0f 279 void gps_receiver()
Joeatsumi 0:a24fb5835b0f 280 {
Joeatsumi 0:a24fb5835b0f 281
Joeatsumi 0:a24fb5835b0f 282 //variables
Joeatsumi 0:a24fb5835b0f 283 int numbers_of_nmea;
Joeatsumi 0:a24fb5835b0f 284 char gps_data[256];
Joeatsumi 0:a24fb5835b0f 285
Joeatsumi 0:a24fb5835b0f 286
Joeatsumi 0:a24fb5835b0f 287
Joeatsumi 0:a24fb5835b0f 288 numbers_of_nmea=0;
Joeatsumi 0:a24fb5835b0f 289 while(gps.getc()!='$') {
Joeatsumi 0:a24fb5835b0f 290 }
Joeatsumi 0:a24fb5835b0f 291
Joeatsumi 0:a24fb5835b0f 292 while( (gps_data[numbers_of_nmea]=gps.getc()) != '\r') {
Joeatsumi 0:a24fb5835b0f 293
Joeatsumi 0:a24fb5835b0f 294 numbers_of_nmea++;
Joeatsumi 0:a24fb5835b0f 295 if(numbers_of_nmea==256) {
Joeatsumi 0:a24fb5835b0f 296 numbers_of_nmea=255;
Joeatsumi 0:a24fb5835b0f 297 break;
Joeatsumi 0:a24fb5835b0f 298 }
Joeatsumi 0:a24fb5835b0f 299 }
Joeatsumi 0:a24fb5835b0f 300
Joeatsumi 0:a24fb5835b0f 301 gps_data[numbers_of_nmea]='\0';
Joeatsumi 0:a24fb5835b0f 302
Joeatsumi 0:a24fb5835b0f 303 //char gps_datae[256]="$GNRMC,222219.000,A,3605.4010,N,14006.8240,E,0.11,109.92,191016,,,A";
Joeatsumi 0:a24fb5835b0f 304 if( sscanf(gps_data, "GNRMC,%f,%f,%f,%f\r",&g_hokui,&g_tokei,&gps_azi,&speed_m) >= 1) {
Joeatsumi 0:a24fb5835b0f 305 //pc.printf("変換前データ %f,%f,%f\r\n",g_hokui,g_tokei,azi);
Joeatsumi 0:a24fb5835b0f 306 } else {
Joeatsumi 0:a24fb5835b0f 307 }//if ends
Joeatsumi 0:a24fb5835b0f 308 }
Joeatsumi 0:a24fb5835b0f 309
Joeatsumi 0:a24fb5835b0f 310
Joeatsumi 0:a24fb5835b0f 311 //=========================================================
Joeatsumi 0:a24fb5835b0f 312 //gps notification function
Joeatsumi 0:a24fb5835b0f 313 //=========================================================
Joeatsumi 0:a24fb5835b0f 314 void gps_notification()
Joeatsumi 0:a24fb5835b0f 315 {
Joeatsumi 0:a24fb5835b0f 316 //pc.printf("processing\r\n");
Joeatsumi 0:a24fb5835b0f 317 if(gps_sw==1) {
Joeatsumi 0:a24fb5835b0f 318 notification_from_sub=1;
Joeatsumi 0:a24fb5835b0f 319 time_gps_nof=passed_time.read_ms();
Joeatsumi 0:a24fb5835b0f 320 } else {
Joeatsumi 0:a24fb5835b0f 321 notification_from_sub=0;
Joeatsumi 0:a24fb5835b0f 322 }
Joeatsumi 0:a24fb5835b0f 323
Joeatsumi 0:a24fb5835b0f 324 }
Joeatsumi 0:a24fb5835b0f 325
Joeatsumi 0:a24fb5835b0f 326 void observation_update()
Joeatsumi 0:a24fb5835b0f 327 {
Joeatsumi 0:a24fb5835b0f 328 if((sd_switch==1)&&(sd_flag==0)){
Joeatsumi 0:a24fb5835b0f 329 sd_open();
Joeatsumi 0:a24fb5835b0f 330 sd_flag=1;
Joeatsumi 0:a24fb5835b0f 331 pc.printf("Logging was started.\r\n");
Joeatsumi 0:a24fb5835b0f 332 }else if((sd_switch==1)&&(sd_flag==1)){
Joeatsumi 0:a24fb5835b0f 333 //何もしない
Joeatsumi 0:a24fb5835b0f 334 }else if((sd_switch==0)&&(sd_flag==1)){
Joeatsumi 0:a24fb5835b0f 335 sd_close();
Joeatsumi 0:a24fb5835b0f 336 sd_flag=0;
Joeatsumi 0:a24fb5835b0f 337 pc.printf("Logging was closed.\r\n");
Joeatsumi 0:a24fb5835b0f 338 }else if((sd_switch==0)&&(sd_flag==0)){
Joeatsumi 0:a24fb5835b0f 339 //何もしない
Joeatsumi 0:a24fb5835b0f 340 }
Joeatsumi 0:a24fb5835b0f 341
Joeatsumi 0:a24fb5835b0f 342 if(notification_from_sub==1) {
Joeatsumi 0:a24fb5835b0f 343
Joeatsumi 0:a24fb5835b0f 344 //最新の時間の取得
Joeatsumi 0:a24fb5835b0f 345 time_new=passed_time.read_ms();
Joeatsumi 0:a24fb5835b0f 346 //離散時間の更新
Joeatsumi 0:a24fb5835b0f 347 time_old=time_new;
Joeatsumi 0:a24fb5835b0f 348
Joeatsumi 0:a24fb5835b0f 349 if((time_new-time_gps_nof)<=10.0){
Joeatsumi 0:a24fb5835b0f 350
Joeatsumi 0:a24fb5835b0f 351 myled=1;
Joeatsumi 0:a24fb5835b0f 352
Joeatsumi 0:a24fb5835b0f 353 //gpsプロセッサからのシリアル信号を受信
Joeatsumi 0:a24fb5835b0f 354 gps_receiver();
Joeatsumi 0:a24fb5835b0f 355 notification_from_sub=0;
Joeatsumi 0:a24fb5835b0f 356 //高度情報の取得
Joeatsumi 0:a24fb5835b0f 357 altitude_1=get_altitude(altitude_offset);
Joeatsumi 0:a24fb5835b0f 358
Joeatsumi 0:a24fb5835b0f 359 //処理時間の計算
Joeatsumi 0:a24fb5835b0f 360 //最新の時間の取得
Joeatsumi 0:a24fb5835b0f 361 time_new=passed_time.read_ms();
Joeatsumi 0:a24fb5835b0f 362 //pc.printf("%f\r\n",time_new-time_old);
Joeatsumi 0:a24fb5835b0f 363 //
Joeatsumi 0:a24fb5835b0f 364 //デバッグの為の表示
Joeatsumi 0:a24fb5835b0f 365 //pc.printf("%f,%f,%f,%f,%f\r\n",g_hokui,g_tokei,altitude_1,speed_m,gps_azi);
Joeatsumi 0:a24fb5835b0f 366
Joeatsumi 0:a24fb5835b0f 367 //地磁気センサの計測
Joeatsumi 0:a24fb5835b0f 368 mx = inertial_sensor.get_mag_x_data();
Joeatsumi 0:a24fb5835b0f 369 my = inertial_sensor.get_mag_y_data();
Joeatsumi 0:a24fb5835b0f 370 mz = inertial_sensor.get_mag_z_data();
Joeatsumi 0:a24fb5835b0f 371 //デバッグの為の表示
Joeatsumi 0:a24fb5835b0f 372 //pc.printf("%f,%f,%f\r\n",mx,my,mz);
Joeatsumi 0:a24fb5835b0f 373
Joeatsumi 0:a24fb5835b0f 374 myled=0;
Joeatsumi 0:a24fb5835b0f 375
Joeatsumi 0:a24fb5835b0f 376 if(sd_flag==1){
Joeatsumi 0:a24fb5835b0f 377
Joeatsumi 0:a24fb5835b0f 378 //緯度
Joeatsumi 0:a24fb5835b0f 379 lat_int[0] = int(g_hokui*1000000.0);
Joeatsumi 0:a24fb5835b0f 380 //経度
Joeatsumi 0:a24fb5835b0f 381 longi_int[0] = int(g_tokei*1000000.0);
Joeatsumi 0:a24fb5835b0f 382 //移動方位
Joeatsumi 0:a24fb5835b0f 383 m_azi_int[0] = int(gps_azi*1000000.0);
Joeatsumi 0:a24fb5835b0f 384 //対地速度
Joeatsumi 0:a24fb5835b0f 385 gspeed_int[0] = int(speed_m*100.0);
Joeatsumi 0:a24fb5835b0f 386 //大気圧高度
Joeatsumi 0:a24fb5835b0f 387 altitude_int[0] = int((altitude_1+50000.0)*100.0);
Joeatsumi 0:a24fb5835b0f 388
Joeatsumi 0:a24fb5835b0f 389 //地磁気センサ x,y軸
Joeatsumi 0:a24fb5835b0f 390 //8192
Joeatsumi 0:a24fb5835b0f 391 //x軸地磁気
Joeatsumi 0:a24fb5835b0f 392 mx_int[0] = int(mx+8192.0);
Joeatsumi 0:a24fb5835b0f 393 //y軸地磁気
Joeatsumi 0:a24fb5835b0f 394 my_int[0] = int(my+8192.0);
Joeatsumi 0:a24fb5835b0f 395 //地磁気センサ z軸
Joeatsumi 0:a24fb5835b0f 396 mz_int[0] = int(mz+32768.0);
Joeatsumi 0:a24fb5835b0f 397
Joeatsumi 0:a24fb5835b0f 398 //GPSデータ列
Joeatsumi 0:a24fb5835b0f 399 err =fwrite(GPSIndex, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 400 err =fwrite(TimeIndex, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 401
Joeatsumi 0:a24fb5835b0f 402 err =fwrite(lat_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 403 err =fwrite(longi_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 404 err =fwrite(m_azi_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 405 err =fwrite(gspeed_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 406 err =fwrite(altitude_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 407
Joeatsumi 0:a24fb5835b0f 408 //Dump_Index パケットを32byteにするための埋め合わせ
Joeatsumi 0:a24fb5835b0f 409 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 410 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 411 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 412 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 413 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 414 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 415 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 416
Joeatsumi 0:a24fb5835b0f 417 //地磁気と赤外線センサ
Joeatsumi 0:a24fb5835b0f 418 err =fwrite(mag_ir_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 419 err =fwrite(TimeIndex, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 420
Joeatsumi 0:a24fb5835b0f 421 err =fwrite(mx_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 422 err =fwrite(my_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 423 err =fwrite(mz_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 424
Joeatsumi 0:a24fb5835b0f 425 //Dump_Index パケットを32byteにするための埋め合わせ
Joeatsumi 0:a24fb5835b0f 426 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 427 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 428 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 429 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 430 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 431 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 432 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 433 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 434 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 435 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 436 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 437 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 438 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 439 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 440 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 441
Joeatsumi 0:a24fb5835b0f 442 }else{}
Joeatsumi 0:a24fb5835b0f 443 }else{notification_from_sub=0;}
Joeatsumi 0:a24fb5835b0f 444
Joeatsumi 0:a24fb5835b0f 445 }else{
Joeatsumi 0:a24fb5835b0f 446
Joeatsumi 0:a24fb5835b0f 447 }
Joeatsumi 0:a24fb5835b0f 448 }
Joeatsumi 0:a24fb5835b0f 449
Joeatsumi 0:a24fb5835b0f 450 void inertial_sensor_function()
Joeatsumi 0:a24fb5835b0f 451 {
Joeatsumi 0:a24fb5835b0f 452 int counter;
Joeatsumi 0:a24fb5835b0f 453
Joeatsumi 0:a24fb5835b0f 454 gx=inertial_sensor.get_gyro_x_data();
Joeatsumi 0:a24fb5835b0f 455 gy=inertial_sensor.get_gyro_y_data();
Joeatsumi 0:a24fb5835b0f 456 gz=inertial_sensor.get_gyro_z_data();
Joeatsumi 0:a24fb5835b0f 457
Joeatsumi 0:a24fb5835b0f 458 ax = inertial_sensor.get_acc_x_data();
Joeatsumi 0:a24fb5835b0f 459 ay = inertial_sensor.get_acc_y_data();
Joeatsumi 0:a24fb5835b0f 460 az = inertial_sensor.get_acc_z_data();
Joeatsumi 0:a24fb5835b0f 461
Joeatsumi 0:a24fb5835b0f 462 gx -=gx_mean;
Joeatsumi 0:a24fb5835b0f 463 gy -=gy_mean;
Joeatsumi 0:a24fb5835b0f 464 gz -=gz_mean;
Joeatsumi 0:a24fb5835b0f 465
Joeatsumi 0:a24fb5835b0f 466 //最新の時間の取得
Joeatsumi 0:a24fb5835b0f 467 time_new=passed_time.read_ms();
Joeatsumi 0:a24fb5835b0f 468
Joeatsumi 0:a24fb5835b0f 469 //離散時間の更新
Joeatsumi 0:a24fb5835b0f 470 time_old=time_new;
Joeatsumi 0:a24fb5835b0f 471
Joeatsumi 0:a24fb5835b0f 472 if(sd_flag==1){
Joeatsumi 0:a24fb5835b0f 473
Joeatsumi 0:a24fb5835b0f 474 //バイナリ形式でsdカードに書き込む
Joeatsumi 0:a24fb5835b0f 475 TimeIndex[0] = int(time_new);
Joeatsumi 0:a24fb5835b0f 476
Joeatsumi 0:a24fb5835b0f 477 //ジャイロのレンジは+-250deg/s
Joeatsumi 0:a24fb5835b0f 478 //ジャイロセンサ計測値をintにする
Joeatsumi 0:a24fb5835b0f 479
Joeatsumi 0:a24fb5835b0f 480 int gx_pre,gy_pre,gz_pre;
Joeatsumi 0:a24fb5835b0f 481 /*
Joeatsumi 0:a24fb5835b0f 482 gx_pre = gyro_float2int(gx);
Joeatsumi 0:a24fb5835b0f 483 gy_pre = gyro_float2int(gy);
Joeatsumi 0:a24fb5835b0f 484 gz_pre = gyro_float2int(gz);
Joeatsumi 0:a24fb5835b0f 485 */
Joeatsumi 0:a24fb5835b0f 486 gx_pre = int((gx+500.0)*10000.0);//0.1 m deg/sの分解能とする。このレンジでの分解能は7.6m deg/s
Joeatsumi 0:a24fb5835b0f 487 gy_pre = int((gy+500.0)*10000.0);
Joeatsumi 0:a24fb5835b0f 488 gz_pre = int((gz+500.0)*10000.0);
Joeatsumi 0:a24fb5835b0f 489
Joeatsumi 0:a24fb5835b0f 490 gx_int[0] = gx_pre;
Joeatsumi 0:a24fb5835b0f 491 gy_int[0] = gy_pre;
Joeatsumi 0:a24fb5835b0f 492 gz_int[0] = gz_pre;
Joeatsumi 0:a24fb5835b0f 493
Joeatsumi 0:a24fb5835b0f 494 //加速度センサ計測値をintにする
Joeatsumi 0:a24fb5835b0f 495
Joeatsumi 0:a24fb5835b0f 496 int ax_pre,ay_pre,az_pre;
Joeatsumi 0:a24fb5835b0f 497 /*
Joeatsumi 0:a24fb5835b0f 498 ax_pre = acc_float2int(ax);
Joeatsumi 0:a24fb5835b0f 499 ay_pre = acc_float2int(ay);
Joeatsumi 0:a24fb5835b0f 500 az_pre = acc_float2int(az);
Joeatsumi 0:a24fb5835b0f 501 */
Joeatsumi 0:a24fb5835b0f 502 ax_pre = int((ax+GRAVITY*16.0)*100000.0);//±8g3.91mg/LSB 0.01mg
Joeatsumi 0:a24fb5835b0f 503 ay_pre = int((ay+GRAVITY*16.0)*100000.0);
Joeatsumi 0:a24fb5835b0f 504 az_pre = int((az+GRAVITY*16.0)*100000.0);
Joeatsumi 0:a24fb5835b0f 505
Joeatsumi 0:a24fb5835b0f 506 ax_int[0] = ax_pre;
Joeatsumi 0:a24fb5835b0f 507 ay_int[0] = ay_pre;
Joeatsumi 0:a24fb5835b0f 508 az_int[0] = az_pre;
Joeatsumi 0:a24fb5835b0f 509
Joeatsumi 0:a24fb5835b0f 510 //sdカードへの書き込み
Joeatsumi 0:a24fb5835b0f 511 //慣性センサのヘッダ
Joeatsumi 0:a24fb5835b0f 512 //Six_axis_Index[1] = {'A'};
Joeatsumi 0:a24fb5835b0f 513
Joeatsumi 0:a24fb5835b0f 514 err =fwrite(Six_axis_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 515 err =fwrite(TimeIndex, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 516
Joeatsumi 0:a24fb5835b0f 517 err =fwrite(gx_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 518 err =fwrite(gy_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 519 err =fwrite(gz_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 520
Joeatsumi 0:a24fb5835b0f 521 err =fwrite(ax_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 522 err =fwrite(ay_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 523 err =fwrite(az_int, 1, 4, f);
Joeatsumi 0:a24fb5835b0f 524
Joeatsumi 0:a24fb5835b0f 525 //Dump_Index パケットを32byteにするための埋め合わせ
Joeatsumi 0:a24fb5835b0f 526 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 527 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 528 err =fwrite(Dump_Index, 1, 1, f);
Joeatsumi 0:a24fb5835b0f 529
Joeatsumi 0:a24fb5835b0f 530
Joeatsumi 0:a24fb5835b0f 531 }else{}
Joeatsumi 0:a24fb5835b0f 532
Joeatsumi 0:a24fb5835b0f 533
Joeatsumi 0:a24fb5835b0f 534 }
Joeatsumi 0:a24fb5835b0f 535
Joeatsumi 0:a24fb5835b0f 536 //gps_notification
Joeatsumi 0:a24fb5835b0f 537 void sensor_function(){
Joeatsumi 0:a24fb5835b0f 538
Joeatsumi 0:a24fb5835b0f 539 inertial_sensor_function();
Joeatsumi 0:a24fb5835b0f 540 gps_notification();
Joeatsumi 0:a24fb5835b0f 541
Joeatsumi 0:a24fb5835b0f 542 }
Joeatsumi 0:a24fb5835b0f 543 //=========================================================
Joeatsumi 0:a24fb5835b0f 544 //altitude function
Joeatsumi 0:a24fb5835b0f 545 //=========================================================
Joeatsumi 0:a24fb5835b0f 546 float get_altitude(float offset)
Joeatsumi 0:a24fb5835b0f 547 {
Joeatsumi 0:a24fb5835b0f 548
Joeatsumi 0:a24fb5835b0f 549 pressure_1=inertial_sensor.getPressure();
Joeatsumi 0:a24fb5835b0f 550 //pressure_1=pressure_sensor.getPressure();
Joeatsumi 0:a24fb5835b0f 551
Joeatsumi 0:a24fb5835b0f 552 temperature_1=inertial_sensor.getTemperature();
Joeatsumi 0:a24fb5835b0f 553 //temperature_1=pressure_sensor.getTemperature();
Joeatsumi 0:a24fb5835b0f 554 //pressure_sensor
Joeatsumi 0:a24fb5835b0f 555
Joeatsumi 0:a24fb5835b0f 556 float altitude=pow((1013.25/pressure_1),0.190);
Joeatsumi 0:a24fb5835b0f 557 altitude-=1.0;
Joeatsumi 0:a24fb5835b0f 558 altitude*=(temperature_1+273.15);
Joeatsumi 0:a24fb5835b0f 559 altitude/=0.0065;
Joeatsumi 0:a24fb5835b0f 560 altitude-=offset;
Joeatsumi 0:a24fb5835b0f 561 altitude+=INITIAL_ALTITUDE;
Joeatsumi 0:a24fb5835b0f 562
Joeatsumi 0:a24fb5835b0f 563 //デバッグ用の表示
Joeatsumi 0:a24fb5835b0f 564 pc.printf("pressure=%f,tempreture=%f,altitude=%f\r\n",pressure_1,temperature_1,altitude);
Joeatsumi 0:a24fb5835b0f 565
Joeatsumi 0:a24fb5835b0f 566 return altitude;
Joeatsumi 0:a24fb5835b0f 567
Joeatsumi 0:a24fb5835b0f 568 }
Joeatsumi 0:a24fb5835b0f 569
Joeatsumi 0:a24fb5835b0f 570 //=========================================================
Joeatsumi 0:a24fb5835b0f 571 //Timer function
Joeatsumi 0:a24fb5835b0f 572 //=========================================================
Joeatsumi 0:a24fb5835b0f 573 void timer_reset()
Joeatsumi 0:a24fb5835b0f 574 {
Joeatsumi 0:a24fb5835b0f 575 passed_time.reset();//reset timer counter
Joeatsumi 0:a24fb5835b0f 576 }
Joeatsumi 0:a24fb5835b0f 577
Joeatsumi 0:a24fb5835b0f 578 void timer_read()
Joeatsumi 0:a24fb5835b0f 579 {
Joeatsumi 0:a24fb5835b0f 580 time_new=passed_time.read_ms();
Joeatsumi 0:a24fb5835b0f 581 //pc.printf("経過時間=%f\r\n",time_new);
Joeatsumi 0:a24fb5835b0f 582 }
Joeatsumi 0:a24fb5835b0f 583
Joeatsumi 0:a24fb5835b0f 584 //float型のgyroデータをintに変換する
Joeatsumi 0:a24fb5835b0f 585 int gyro_float2int(float gyro_data){
Joeatsumi 0:a24fb5835b0f 586
Joeatsumi 0:a24fb5835b0f 587 int gyro_int_data;
Joeatsumi 0:a24fb5835b0f 588
Joeatsumi 0:a24fb5835b0f 589 if(gyro_data>=0.0){
Joeatsumi 0:a24fb5835b0f 590 //gyro_scale_factor=0.0305f;
Joeatsumi 0:a24fb5835b0f 591 //gyro_scale_factor)/RAD_TO_DEG
Joeatsumi 0:a24fb5835b0f 592 gyro_int_data = int(gyro_data*RAD_TO_DEG/0.0305f);
Joeatsumi 0:a24fb5835b0f 593 }else if(gyro_data<0.0){
Joeatsumi 0:a24fb5835b0f 594 /*
Joeatsumi 0:a24fb5835b0f 595 x_data = -1 * (65536 - x_data);
Joeatsumi 0:a24fb5835b0f 596 */
Joeatsumi 0:a24fb5835b0f 597 gyro_int_data = int((gyro_data*RAD_TO_DEG)/0.0305f);
Joeatsumi 0:a24fb5835b0f 598 gyro_int_data=gyro_int_data+65536;
Joeatsumi 0:a24fb5835b0f 599
Joeatsumi 0:a24fb5835b0f 600 }
Joeatsumi 0:a24fb5835b0f 601
Joeatsumi 0:a24fb5835b0f 602 return gyro_int_data;
Joeatsumi 0:a24fb5835b0f 603 }
Joeatsumi 0:a24fb5835b0f 604
Joeatsumi 0:a24fb5835b0f 605 //float型のaccデータをintに変換する
Joeatsumi 0:a24fb5835b0f 606 int acc_float2int(float acc_data){
Joeatsumi 0:a24fb5835b0f 607
Joeatsumi 0:a24fb5835b0f 608 int acc_int_data;
Joeatsumi 0:a24fb5835b0f 609
Joeatsumi 0:a24fb5835b0f 610 if(acc_data>=0.0){
Joeatsumi 0:a24fb5835b0f 611 //acc_scale_factor=256;
Joeatsumi 0:a24fb5835b0f 612 //x_acc=float(x_data)*GRAVITY/acc_scale_factor;//m/s^2
Joeatsumi 0:a24fb5835b0f 613 acc_int_data = int(acc_data*256/GRAVITY);
Joeatsumi 0:a24fb5835b0f 614
Joeatsumi 0:a24fb5835b0f 615 }else if(acc_data<0.0){
Joeatsumi 0:a24fb5835b0f 616 //x_data = -1 * (4096 - x_data);
Joeatsumi 0:a24fb5835b0f 617
Joeatsumi 0:a24fb5835b0f 618 acc_int_data = int(acc_data*256/GRAVITY);
Joeatsumi 0:a24fb5835b0f 619 acc_int_data=acc_int_data+4096;
Joeatsumi 0:a24fb5835b0f 620
Joeatsumi 0:a24fb5835b0f 621 }
Joeatsumi 0:a24fb5835b0f 622
Joeatsumi 0:a24fb5835b0f 623 return acc_int_data;
Joeatsumi 0:a24fb5835b0f 624 }
Joeatsumi 0:a24fb5835b0f 625
Joeatsumi 0:a24fb5835b0f 626
Joeatsumi 0:a24fb5835b0f 627 //=========================================================
Joeatsumi 0:a24fb5835b0f 628 // main
Joeatsumi 0:a24fb5835b0f 629 //=========================================================
Joeatsumi 0:a24fb5835b0f 630 int main()
Joeatsumi 0:a24fb5835b0f 631 {
Joeatsumi 0:a24fb5835b0f 632
Joeatsumi 0:a24fb5835b0f 633 //UART initialization
Joeatsumi 0:a24fb5835b0f 634 pc.baud(115200); //115.2 kbps
Joeatsumi 0:a24fb5835b0f 635 gps.baud(115200); //115.2 kbps
Joeatsumi 0:a24fb5835b0f 636
Joeatsumi 0:a24fb5835b0f 637 blockDevice.frequency(25000000);
Joeatsumi 0:a24fb5835b0f 638
Joeatsumi 0:a24fb5835b0f 639 /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
Joeatsumi 0:a24fb5835b0f 640 //Mount the filesystem
Joeatsumi 0:a24fb5835b0f 641 sd_flag=0;
Joeatsumi 0:a24fb5835b0f 642 sd_open_first();
Joeatsumi 0:a24fb5835b0f 643
Joeatsumi 0:a24fb5835b0f 644 //Range setting for inertial sensor
Joeatsumi 0:a24fb5835b0f 645 //慣性センサのレンジ設定
Joeatsumi 0:a24fb5835b0f 646 inertial_sensor.set_gyro_range(3);//±250°/s131.2 LSB/°/s 7.6 m°/s / LSB
Joeatsumi 0:a24fb5835b0f 647 inertial_sensor.set_acc_range(2);//±8g3.91mg/LSB
Joeatsumi 0:a24fb5835b0f 648
Joeatsumi 0:a24fb5835b0f 649 //地磁気センサの初期化
Joeatsumi 0:a24fb5835b0f 650 inertial_sensor.initiate_mag();
Joeatsumi 0:a24fb5835b0f 651
Joeatsumi 0:a24fb5835b0f 652 //Initialization for pressure sensor
Joeatsumi 0:a24fb5835b0f 653 //気圧センサの初期化
Joeatsumi 0:a24fb5835b0f 654 //pressure_sensor.initialize();
Joeatsumi 0:a24fb5835b0f 655 inertial_sensor.initialize();
Joeatsumi 0:a24fb5835b0f 656
Joeatsumi 0:a24fb5835b0f 657 notification_from_sub=0;//サブマイコンからの通知を初期化
Joeatsumi 0:a24fb5835b0f 658 dummy_counter=0;
Joeatsumi 0:a24fb5835b0f 659
Joeatsumi 0:a24fb5835b0f 660 //-------------------------------------------
Joeatsumi 0:a24fb5835b0f 661 //altitude initialization
Joeatsumi 0:a24fb5835b0f 662 /*
Joeatsumi 0:a24fb5835b0f 663 2回目からの計測でまともな高度が得られるので、オフセット計測を2回する
Joeatsumi 0:a24fb5835b0f 664 */
Joeatsumi 0:a24fb5835b0f 665 //-------------------------------------------
Joeatsumi 0:a24fb5835b0f 666 altitude_offset=get_altitude(0.0);//初期高度を取得（オフセットの取得)
Joeatsumi 0:a24fb5835b0f 667 wait_us (100000);//wait 0.1 s
Joeatsumi 0:a24fb5835b0f 668 altitude_offset=get_altitude(0.0);//初期高度を取得（オフセットの取得）
Joeatsumi 0:a24fb5835b0f 669
Joeatsumi 0:a24fb5835b0f 670 //------------------------------------------
Joeatsumi 0:a24fb5835b0f 671 //ジャイロオフセットの計算
Joeatsumi 0:a24fb5835b0f 672 gx_mean=gy_mean=gz_mean=0.0;
Joeatsumi 0:a24fb5835b0f 673
Joeatsumi 0:a24fb5835b0f 674 pc.printf("offset calculation.\r\n");
Joeatsumi 0:a24fb5835b0f 675
Joeatsumi 0:a24fb5835b0f 676 for(int counter=0;counter<mean_counter;counter++){
Joeatsumi 0:a24fb5835b0f 677
Joeatsumi 0:a24fb5835b0f 678 gx=inertial_sensor.get_gyro_x_data();
Joeatsumi 0:a24fb5835b0f 679 gy=inertial_sensor.get_gyro_y_data();
Joeatsumi 0:a24fb5835b0f 680 gz=inertial_sensor.get_gyro_z_data();
Joeatsumi 0:a24fb5835b0f 681
Joeatsumi 0:a24fb5835b0f 682 gx_mean+=gx;
Joeatsumi 0:a24fb5835b0f 683 gy_mean+=gy;
Joeatsumi 0:a24fb5835b0f 684 gz_mean+=gz;
Joeatsumi 0:a24fb5835b0f 685 wait_us(10000);
Joeatsumi 0:a24fb5835b0f 686 }
Joeatsumi 0:a24fb5835b0f 687
Joeatsumi 0:a24fb5835b0f 688 gx_mean/=mean_counter;
Joeatsumi 0:a24fb5835b0f 689 gy_mean/=mean_counter;
Joeatsumi 0:a24fb5835b0f 690 gz_mean/=mean_counter;
Joeatsumi 0:a24fb5835b0f 691
Joeatsumi 0:a24fb5835b0f 692 pc.printf("offset calculation has been done.\r\n");
Joeatsumi 0:a24fb5835b0f 693 //-------------------------------------------
Joeatsumi 0:a24fb5835b0f 694 //Thread
Joeatsumi 0:a24fb5835b0f 695 //-------------------------------------------
Joeatsumi 0:a24fb5835b0f 696 //set priority of sensor threads
Joeatsumi 0:a24fb5835b0f 697 Thread thread1(osPriorityAboveNormal);
Joeatsumi 0:a24fb5835b0f 698 Thread thread2(osPriorityNormal);
Joeatsumi 0:a24fb5835b0f 699
Joeatsumi 0:a24fb5835b0f 700 //set prioority of main
Joeatsumi 0:a24fb5835b0f 701 osThreadSetPriority(osThreadGetId(), osPriorityIdle);
Joeatsumi 0:a24fb5835b0f 702
Joeatsumi 0:a24fb5835b0f 703 //start threads
Joeatsumi 0:a24fb5835b0f 704 thread1.start(callback(&queue1, &EventQueue::dispatch_forever));
Joeatsumi 0:a24fb5835b0f 705 thread2.start(callback(&queue2, &EventQueue::dispatch_forever));
Joeatsumi 0:a24fb5835b0f 706
Joeatsumi 0:a24fb5835b0f 707 pc.printf("Ticker setup has been done.\r\n");
Joeatsumi 0:a24fb5835b0f 708
Joeatsumi 0:a24fb5835b0f 709 //-------------------------------------------
Joeatsumi 0:a24fb5835b0f 710 //Timer starts
Joeatsumi 0:a24fb5835b0f 711 //-------------------------------------------
Joeatsumi 0:a24fb5835b0f 712 passed_time.start();
Joeatsumi 0:a24fb5835b0f 713
Joeatsumi 0:a24fb5835b0f 714 pc.printf("Activated.\r\n");
Joeatsumi 0:a24fb5835b0f 715
Joeatsumi 0:a24fb5835b0f 716 //thread are called every 1 or 5ms
Joeatsumi 0:a24fb5835b0f 717 queue1.call_every(10,&observation_update);//observation_update
Joeatsumi 0:a24fb5835b0f 718 queue2.call_every(10,&sensor_function);//sensor_function
Joeatsumi 0:a24fb5835b0f 719
Joeatsumi 0:a24fb5835b0f 720 while(1) {
Joeatsumi 0:a24fb5835b0f 721
Joeatsumi 0:a24fb5835b0f 722 }// while(1)ends
Joeatsumi 0:a24fb5835b0f 723 }//int main ends