CORE
logger
Revision 0:c3e3dc7923c8, committed 2019-12-09
- Comitter:
- takuto003
- Date:
- Mon Dec 09 15:37:51 2019 +0000
- Commit message:
- Logger apd
Changed in this revision
| logger.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r c3e3dc7923c8 logger.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/logger.h Mon Dec 09 15:37:51 2019 +0000
@@ -0,0 +1,505 @@
+#include "mbed.h"
+#include "math.h"
+
+#define BMP280_SLAVE_ADDRESS (0x76 << 1)
+#define AK8963_ADDRESS 0x0C << 1
+#define WHO_AM_I_AK8963 0x00
+#define INFO 0x01
+#define AK8963_ST1 0x02
+#define AK8963_XOUT_L 0x03
+#define AK8963_XOUT_H 0x04
+#define AK8963_YOUT_L 0x05
+#define AK8963_YOUT_H 0x06
+#define AK8963_ZOUT_L 0x07
+#define AK8963_ZOUT_H 0x08
+#define AK8963_ST2 0x09
+#define AK8963_CNTL 0x0A
+#define AK8963_ASTC 0x0C
+#define AK8963_I2CDIS 0x0F
+#define AK8963_ASAX 0x10
+#define AK8963_ASAY 0x11
+#define AK8963_ASAZ 0x12
+#define XG_OFFSET_H 0x13
+#define XG_OFFSET_L 0x14
+#define YG_OFFSET_H 0x15
+#define YG_OFFSET_L 0x16
+#define ZG_OFFSET_H 0x17
+#define ZG_OFFSET_L 0x18
+#define SMPLRT_DIV 0x19
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define ACCEL_CONFIG2 0x1D
+#define LP_ACCEL_ODR 0x1E
+#define WOM_THR 0x1F
+#define FIFO_EN 0x23
+#define I2C_MST_CTRL 0x24
+#define I2C_MST_STATUS 0x36
+#define INT_PIN_CFG 0x37
+#define INT_ENABLE 0x38
+#define DMP_INT_STATUS 0x39
+#define INT_STATUS 0x3A
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCEL_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define MOT_DETECT_STATUS 0x61
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET 0x68
+#define MOT_DETECT_CTRL 0x69
+#define USER_CTRL 0x6A
+#define PWR_MGMT_1 0x6B
+#define PWR_MGMT_2 0x6C
+#define FIFO_COUNTH 0x72
+#define FIFO_COUNTL 0x73
+#define FIFO_R_W 0x74
+#define WHO_AM_I_MPU9250 0x75
+#define XA_OFFSET_H 0x77
+#define XA_OFFSET_L 0x78
+#define YA_OFFSET_H 0x7A
+#define YA_OFFSET_L 0x7B
+#define ZA_OFFSET_H 0x7D
+#define ZA_OFFSET_L 0x7E
+#define P0 1015.0
+#define PI 3.14159265358979323846f
+#define ADO 0
+#define MPU9250_ADDRESS 0x68 << 1
+#define I2C_SDA PB_7
+#define I2C_SCL PB_6
+I2C i2c(I2C_SDA, I2C_SCL);
+
+enum Ascale
+{
+ AFS_2G = 0,
+ AFS_4G,
+ AFS_8G,
+ AFS_16G
+};
+
+enum Gscale
+{
+ GFS_250DPS = 0,
+ GFS_500DPS,
+ GFS_1000DPS,
+ GFS_2000DPS
+};
+
+enum Mscale
+{
+ MFS_14BITS = 0,
+ MFS_16BITS
+};
+uint8_t Ascale;
+uint8_t Gscale;
+uint8_t Mscale;
+uint8_t Mmode;
+float aRes, gRes, mRes;
+
+int16_t accelCount[3];
+int16_t gyroCount[3];
+int16_t magCount[3];
+float q[4];
+
+float ax, ay, az, gx, gy, gz, mx, my, mz;
+float pitch, yaw, roll;
+float deltat;
+int lastUpdate, firstUpdate, Now;
+int delt_t;
+int count;
+int16_t tempCount;
+float temperature;
+
+class LOGGER
+{
+private:
+ char address;
+ uint16_t dig_T1;
+ int16_t dig_T2, dig_T3;
+ uint16_t dig_P1;
+ int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
+ int32_t t_fine;
+
+public:
+ float pressf;
+ float tempf;
+
+ LOGGER(char slave_adr = BMP280_SLAVE_ADDRESS) : address(slave_adr), t_fine(0)
+ {
+ initialize();
+ }
+
+ void initialize()
+ {
+ char cmd[18];
+ cmd[0] = 0xf2;
+ cmd[1] = 0x01;
+ i2c.write(address, cmd, 2);
+ cmd[0] = 0xf4;
+ cmd[1] = 0x27;
+ i2c.write(address, cmd, 2);
+ cmd[0] = 0xf5;
+ cmd[1] = 0xa0;
+ i2c.write(address, cmd, 2);
+ cmd[0] = 0x88;
+ i2c.write(address, cmd, 1);
+ i2c.read(address, cmd, 6);
+ dig_T1 = (cmd[1] << 8) | cmd[0];
+ dig_T2 = (cmd[3] << 8) | cmd[2];
+ dig_T3 = (cmd[5] << 8) | cmd[4];
+ cmd[0] = 0x8E;
+ i2c.write(address, cmd, 1);
+ i2c.read(address, cmd, 18);
+ dig_P1 = (cmd[1] << 8) | cmd[0];
+ dig_P2 = (cmd[3] << 8) | cmd[2];
+ dig_P3 = (cmd[5] << 8) | cmd[4];
+ dig_P4 = (cmd[7] << 8) | cmd[6];
+ dig_P5 = (cmd[9] << 8) | cmd[8];
+ dig_P6 = (cmd[11] << 8) | cmd[10];
+ dig_P7 = (cmd[13] << 8) | cmd[12];
+ dig_P8 = (cmd[15] << 8) | cmd[14];
+ dig_P9 = (cmd[17] << 8) | cmd[16];
+ }
+ float getTemperature()
+ {
+ uint32_t temp_raw;
+ float tempf;
+ char cmd[4];
+ cmd[0] = 0xfa;
+ i2c.write(address, cmd, 1);
+ i2c.read(address, &cmd[1], 3);
+ temp_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
+ int32_t temp;
+ temp =
+ (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) +
+ ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14);
+ t_fine = temp;
+ temp = (temp * 5 + 128) >> 8;
+ tempf = (float)temp;
+ return (tempf / 100.0f);
+ }
+ float getPressure()
+ {
+ uint32_t press_raw;
+ float pressf;
+ char cmd[4];
+ cmd[0] = 0xf7; // press_msb
+ i2c.write(address, cmd, 1);
+ i2c.read(address, &cmd[1], 3);
+ press_raw = (cmd[1] << 12) | (cmd[2] << 4) | (cmd[3] >> 4);
+
+ int32_t var1, var2;
+ uint32_t press;
+
+ var1 = (t_fine >> 1) - 64000;
+ var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
+ var2 = var2 + ((var1 * dig_P5) << 1);
+ var2 = (var2 >> 2) + (dig_P4 << 16);
+ var1 = (((dig_P3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18;
+ var1 = ((32768 + var1) * dig_P1) >> 15;
+ if (var1 == 0)
+ {
+ return 0;
+ }
+ press = (((1048576 - press_raw) - (var2 >> 12))) * 3125;
+ if (press < 0x80000000)
+ {
+ press = (press << 1) / var1;
+ }
+ else
+ {
+ press = (press / var1) * 2;
+ }
+ var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12;
+ var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13;
+ press = (press + ((var1 + var2 + dig_P7) >> 4));
+ pressf = (float)press;
+ return (pressf / 100.0f);
+ }
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+ {
+ char data_write[2];
+ data_write[0] = subAddress;
+ data_write[1] = data;
+ i2c.write(address, data_write, 2, 0);
+ }
+
+ char readByte(uint8_t address, uint8_t subAddress)
+ {
+ char data[1];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1);
+ i2c.read(address, data, 1, 0);
+ return data[0];
+ }
+
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t *dest)
+ {
+ char data[14];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1);
+ i2c.read(address, data, count, 0);
+ for (int ii = 0; ii < count; ii++)
+ {
+ dest[ii] = data[ii];
+ }
+ }
+
+ void getMres()
+ {
+ switch (Mscale)
+ {
+ case MFS_14BITS:
+ mRes = 10.0 * 4219.0 / 8190.0;
+ break;
+ case MFS_16BITS:
+ mRes = 10.0 * 4219.0 / 32760.0;
+ break;
+ }
+ }
+
+ void getGres()
+ {
+ switch (Gscale)
+ {
+ case GFS_250DPS:
+ gRes = 250.0 / 32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0 / 32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0 / 32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0 / 32768.0;
+ break;
+ }
+ }
+
+ void getAres()
+ {
+ switch (Ascale)
+ {
+ case AFS_2G:
+ aRes = 2.0 / 32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0 / 32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0 / 32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0 / 32768.0;
+ break;
+ }
+ }
+
+ void readAccelData(int16_t *destination)
+ {
+ uint8_t rawData[6];
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);
+ destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
+ destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+ }
+
+ void readGyroData(int16_t *destination)
+ {
+ uint8_t rawData[6];
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);
+ destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]);
+ destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]);
+ destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]);
+ }
+
+ void readMagData(int16_t *destination)
+ {
+ uint8_t rawData[7];
+ if (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01)
+ {
+ readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);
+ uint8_t c = rawData[6];
+ if (!(c & 0x08))
+ {
+ destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);
+ destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]);
+ destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]);
+ }
+ }
+ }
+
+ int16_t readTempData()
+ {
+ uint8_t rawData[2];
+ readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);
+ return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]);
+ }
+
+ void resetMPU9250()
+ {
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
+ wait(0.1);
+ }
+
+ void initAK8963(float *destination)
+ {
+ uint8_t rawData[3];
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
+ wait(0.01);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F);
+ wait(0.01);
+ readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);
+ destination[0] = (float)(rawData[0] - 128) / 256.0f + 1.0f;
+ destination[1] = (float)(rawData[1] - 128) / 256.0f + 1.0f;
+ destination[2] = (float)(rawData[2] - 128) / 256.0f + 1.0f;
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00);
+ wait(0.01);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode);
+ wait(0.01);
+ }
+
+ void initMPU9250()
+ {
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
+ wait(0.1);
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);
+ uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3);
+
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3);
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F);
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03);
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);
+ }
+// void calibrateMPU9250(float *dest1, float *dest2)
+// {
+// uint8_t data[12];
+// uint16_t ii, packet_count, fifo_count;
+// int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+//
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
+// wait(0.1);
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
+// wait(0.2);
+// writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);
+// writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);
+// writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);
+// writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00);
+// writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);
+// writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);
+// wait(0.015);
+// writeByte(MPU9250_ADDRESS, CONFIG, 0x01);
+// writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);
+// writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
+// writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
+//
+// uint16_t gyrosensitivity = 131;
+// uint16_t accelsensitivity = 16384;
+// writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);
+// writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);
+// wait(0.04);
+// writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);
+// readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]);
+// fifo_count = ((uint16_t)data[0] << 8) | data[1];
+// packet_count = fifo_count / 12;
+//
+// for (ii = 0; ii < packet_count; ii++)
+// {
+// int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+// readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]);
+// accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1]);
+// accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3]);
+// accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5]);
+// gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7]);
+// gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9]);
+// gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
+//
+// accel_bias[0] += (int32_t)accel_temp[0];
+// accel_bias[1] += (int32_t)accel_temp[1];
+// accel_bias[2] += (int32_t)accel_temp[2];
+// gyro_bias[0] += (int32_t)gyro_temp[0];
+// gyro_bias[1] += (int32_t)gyro_temp[1];
+// gyro_bias[2] += (int32_t)gyro_temp[2];
+// }
+// accel_bias[0] /= (int32_t)packet_count;
+// accel_bias[1] /= (int32_t)packet_count;
+// accel_bias[2] /= (int32_t)packet_count;
+// gyro_bias[0] /= (int32_t)packet_count;
+// gyro_bias[1] /= (int32_t)packet_count;
+// gyro_bias[2] /= (int32_t)packet_count;
+//
+// if (accel_bias[2] > 0L)
+// {
+// accel_bias[2] -= (int32_t)accelsensitivity;
+// }
+// else
+// {
+// accel_bias[2] += (int32_t)accelsensitivity;
+// }
+// data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF;
+// data[1] = (-gyro_bias[0] / 4) & 0xFF;
+// data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
+// data[3] = (-gyro_bias[1] / 4) & 0xFF;
+// data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
+// data[5] = (-gyro_bias[2] / 4) & 0xFF;
+//
+// dest1[0] = (float)gyro_bias[0] / (float)gyrosensitivity;
+// dest1[1] = (float)gyro_bias[1] / (float)gyrosensitivity;
+// dest1[2] = (float)gyro_bias[2] / (float)gyrosensitivity;
+//
+// int32_t accel_bias_reg[3] = {0, 0, 0};
+// readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]);
+// accel_bias_reg[0] = (int16_t)((int16_t)data[0] << 8) | data[1];
+// readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+// accel_bias_reg[1] = (int16_t)((int16_t)data[0] << 8) | data[1];
+// readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+// accel_bias_reg[2] = (int16_t)((int16_t)data[0] << 8) | data[1];
+//
+// uint32_t mask = 1uL;
+// uint8_t mask_bit[3] = {0, 0, 0};
+//
+// for (ii = 0; ii < 3; ii++)
+// {
+// if (accel_bias_reg[ii] & mask)
+// mask_bit[ii] = 0x01;
+// }
+// accel_bias_reg[0] -= (accel_bias[0] / 8);
+// accel_bias_reg[1] -= (accel_bias[1] / 8);
+// accel_bias_reg[2] -= (accel_bias[2] / 8);
+// data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+// data[1] = (accel_bias_reg[0]) & 0xFF;
+// data[1] = data[1] | mask_bit[0];
+// data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+// data[3] = (accel_bias_reg[1]) & 0xFF;
+// data[3] = data[3] | mask_bit[1];
+// data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+// data[5] = (accel_bias_reg[2]) & 0xFF;
+// data[5] = data[5] | mask_bit[2];
+// dest2[0] = (float)accel_bias[0] / (float)accelsensitivity;
+// dest2[1] = (float)accel_bias[1] / (float)accelsensitivity;
+// dest2[2] = (float)accel_bias[2] / (float)accelsensitivity;
+// }
+};
\ No newline at end of file
diff -r 000000000000 -r c3e3dc7923c8 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Dec 09 15:37:51 2019 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "logger.h"
+#include "SDFileSystem.h"
+DigitalOut L1(PA_4);
+DigitalOut L2(PA_6);
+DigitalOut L3(PA_5);
+Serial gps(PA_9,PA_10);
+Serial pc(PA_2,PA_3);
+SDFileSystem sd(PB_5, PB_4, PB_3, PA_15, "sd");
+FILE* fp;
+LOGGER logg;
+Timer t;
+float sum = 0;
+uint32_t sumCount = 0;
+char buffer[14];
+int log_count=0;
+int16_t gyr[3], acc[3], mag[3],Temp;
+float mag_init[3];
+int COU=0;
+int i,rlock,mode;
+char gps_data[256];
+char ns,ew;
+float w_time,hokui,tokei;
+float g_hokui,g_tokei;
+float d_hokui,m_hokui,d_tokei,m_tokei;
+unsigned char c;
+
+void getGPS() {
+ c = gps.getc();
+ if( c=='$' || i == 256){
+ mode = 0;
+ i = 0;
+ for(int j=0; j<256; j++){
+ gps_data[j]=NULL;
+ }
+ }
+ if(mode==0){
+ if((gps_data[i]=c) != '\r'){
+ i++;
+ }else{
+ gps_data[i]='\0';
+
+ if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
+ if(rlock==1){
+ L1=1;
+ d_tokei= int(tokei/100);
+ m_tokei= (tokei-d_tokei*100)/60;
+ g_tokei= d_tokei+m_tokei;
+ //Latitude
+ d_hokui=int(hokui/100);
+ m_hokui=(hokui-d_hokui*100)/60;
+ g_hokui=d_hokui+m_hokui;
+ }
+ else{
+ }
+ sprintf(gps_data, "");
+ }
+ }
+ }
+}
+int main()
+{
+ i2c.frequency(400000);
+ t.start();
+ pc.printf("gps start!\r\n");
+ gps.baud(9600);
+ logg.initialize();
+ mkdir("/sd/mydir", 0777);
+ FILE *fp=fopen("/sd/test.txt","w");
+ fprintf(fp,"start");
+ fclose(fp);
+
+ gps.attach(getGPS,Serial::RxIrq);
+ while(1) {
+ FILE *fp = fopen("/sd/test.txt","a");
+ COU++;
+ fprintf(fp,"ID=%d,",COU);
+ fprintf(fp,"%f", t.read());
+ logg.readGyroData(gyr);
+ logg.readAccelData(acc);
+ logg.readMagData(mag);
+ Temp = logg.readTempData();
+ ax = acc[0] * 2.0 / 32768.0;
+ ay = acc[1] * 2.0 / 32768.0;
+ az = acc[2] * 2.0 / 32768.0;
+ fprintf(fp,"%d, %d, %d,", gyr[0], gyr[1], gyr[2]);
+ fprintf(fp,"%d, %d, %d,", acc[0], acc[1], acc[2]);
+ fprintf(fp,"%f, %f, %f,", ax, ay, az);
+ fprintf(fp,"%d, %d, %d", mag[0], mag[1], mag[2]);
+ fprintf(fp,"%d,",(Temp/100));
+ fprintf(fp,"%2.2f, %04.2f", logg.getTemperature(), logg.getPressure());
+ fprintf(fp,"%.1f,%.6f,%.6f\n",w_time,g_tokei, g_hokui);
+ wait(0.2);
+ fclose(fp);
+ L1=0;
+}
+}