SSLM1 / Mbed 2 deprecated 5_flightmode

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.

Dependencies:   2_MPU9250 mbed 2_GPS_GMS6-CR6 2_LPS33HW 2_EEPROM 2_P7100

main.cpp@0:842b08cc6af5, 2020-07-13 (annotated)

Committer:
rary
Date:
Mon Jul 13 11:51:24 2020 +0000
Revision:
0:842b08cc6af5
add flight mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rary 0:842b08cc6af5 1 #include "mbed.h"
rary 0:842b08cc6af5 2 #include "EEPROM.h"
rary 0:842b08cc6af5 3 #include "LPS33HW.h"
rary 0:842b08cc6af5 4 #include "P7100.h"
rary 0:842b08cc6af5 5 #include "MPU9250.h"
rary 0:842b08cc6af5 6 #include "GMS6_CR6.h"
rary 0:842b08cc6af5 7
rary 0:842b08cc6af5 8
rary 0:842b08cc6af5 9 Timer t; //時間計測
rary 0:842b08cc6af5 10 Serial pc(USBTX, USBRX);
rary 0:842b08cc6af5 11 LPS33HW lps(p9, p10);
rary 0:842b08cc6af5 12 EEPROM rom(p9, p10);
rary 0:842b08cc6af5 13 MPU9250 mpu(p9, p10);
rary 0:842b08cc6af5 14 P7100 p7100(p5, p6, p7, p8);
rary 0:842b08cc6af5 15 Serial twe(p13, p14);
rary 0:842b08cc6af5 16 GMS6_CR6 gps(p28, p27);
rary 0:842b08cc6af5 17
rary 0:842b08cc6af5 18 DigitalIn Flight_Pin(p30);
rary 0:842b08cc6af5 19
rary 0:842b08cc6af5 20 void get_save_low(); //自作関数プロトタイプ宣言
rary 0:842b08cc6af5 21 void get_save_high(); //自作関数プロトタイプ宣言
rary 0:842b08cc6af5 22
rary 0:842b08cc6af5 23 int pointer_high = 0; //EEPROM保存アドレス(high)
rary 0:842b08cc6af5 24 int pointer_low = 0; //EEPROM保存アドレス(low)
rary 0:842b08cc6af5 25
rary 0:842b08cc6af5 26
rary 0:842b08cc6af5 27 //main
rary 0:842b08cc6af5 28 int main()
rary 0:842b08cc6af5 29 {
rary 0:842b08cc6af5 30 pc.baud(460800); //ボーレート変更
rary 0:842b08cc6af5 31 twe.baud(230400); //tweボーレート変更
rary 0:842b08cc6af5 32
rary 0:842b08cc6af5 33 float lat_north; //GPS緯度
rary 0:842b08cc6af5 34 float lon_east; //GPS経度
rary 0:842b08cc6af5 35 float GPS[2]; //GPSデータ格納配列
rary 0:842b08cc6af5 36
rary 0:842b08cc6af5 37 t.start(); //timer Start
rary 0:842b08cc6af5 38
rary 0:842b08cc6af5 39 //sensor Setup
rary 0:842b08cc6af5 40 lps.start(1); //気圧センサスタート
rary 0:842b08cc6af5 41 lps.start(1); //気圧センサスタート
rary 0:842b08cc6af5 42 wait(0.1);
rary 0:842b08cc6af5 43 mpu.start(); //MPU9250 start&setup
rary 0:842b08cc6af5 44 mpu.accelsetup(3);
rary 0:842b08cc6af5 45 mpu.gyrosetup(3);
rary 0:842b08cc6af5 46 mpu.AKsetup(1);
rary 0:842b08cc6af5 47 wait(0.2);
rary 0:842b08cc6af5 48
rary 0:842b08cc6af5 49
rary 0:842b08cc6af5 50 //Wait mode
rary 0:842b08cc6af5 51 pc.printf("waitmode Start!\r\n");
rary 0:842b08cc6af5 52 while(Flight_Pin == 0)
rary 0:842b08cc6af5 53 {
rary 0:842b08cc6af5 54 get_save_low(); //data get&save
rary 0:842b08cc6af5 55 wait_ms(1000);
rary 0:842b08cc6af5 56 }
rary 0:842b08cc6af5 57 pc.printf("waitmode Finish!\r\n");
rary 0:842b08cc6af5 58
rary 0:842b08cc6af5 59
rary 0:842b08cc6af5 60 //Flight mode
rary 0:842b08cc6af5 61 pc.printf("Flightmode Start!\r\n");
rary 0:842b08cc6af5 62 while(1)
rary 0:842b08cc6af5 63 {
rary 0:842b08cc6af5 64 for(int k = 0; k < 200; k++) //2回に一回送信
rary 0:842b08cc6af5 65 {
rary 0:842b08cc6af5 66 for(int n = 0; n < 2;n++)
rary 0:842b08cc6af5 67 {
rary 0:842b08cc6af5 68 get_save_high(); //data get&save
rary 0:842b08cc6af5 69 }
rary 0:842b08cc6af5 70 twe.printf("^"); //送信トリガー
rary 0:842b08cc6af5 71 }
rary 0:842b08cc6af5 72
rary 0:842b08cc6af5 73 gps.read(GPS); //測位データ取得
rary 0:842b08cc6af5 74 lat_north = GPS[0];
rary 0:842b08cc6af5 75 lon_east = GPS[1];
rary 0:842b08cc6af5 76
rary 0:842b08cc6af5 77 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) //緯度が正常な値のときのみ送信
rary 0:842b08cc6af5 78 {
rary 0:842b08cc6af5 79 twe.printf("%f,%f\r\n^", lat_north, lon_east); //送信トリガー
rary 0:842b08cc6af5 80 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
rary 0:842b08cc6af5 81 }
rary 0:842b08cc6af5 82 memset(GPS,'\0', 2); //送信トリガー
rary 0:842b08cc6af5 83 }
rary 0:842b08cc6af5 84 pc.printf("Flightmode Finish!\r\n");
rary 0:842b08cc6af5 85
rary 0:842b08cc6af5 86 }
rary 0:842b08cc6af5 87
rary 0:842b08cc6af5 88
rary 0:842b08cc6af5 89 void get_save_low()
rary 0:842b08cc6af5 90 {
rary 0:842b08cc6af5 91 char s[64]; //データ取得配列
rary 0:842b08cc6af5 92
rary 0:842b08cc6af5 93 float accel[3], gyro[3]; //MPU9250data
rary 0:842b08cc6af5 94 float AX, AY, AZ, GX, GY, GZ; //MPU9250data
rary 0:842b08cc6af5 95
rary 0:842b08cc6af5 96 float T = t.read(); //時刻データ取得
rary 0:842b08cc6af5 97 float P = lps.data_read(); //気圧データ取得
rary 0:842b08cc6af5 98 float V = p7100.v_read(); //電圧取得
rary 0:842b08cc6af5 99 float TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:842b08cc6af5 100
rary 0:842b08cc6af5 101 mpu.accel_read(3, accel);
rary 0:842b08cc6af5 102 mpu.gyro_read(3, gyro);
rary 0:842b08cc6af5 103 AX = accel[0];
rary 0:842b08cc6af5 104 AY = accel[1];
rary 0:842b08cc6af5 105 AZ = accel[2];
rary 0:842b08cc6af5 106 GX = gyro[0];
rary 0:842b08cc6af5 107 GY = gyro[1];
rary 0:842b08cc6af5 108 GZ = gyro[2];
rary 0:842b08cc6af5 109
rary 0:842b08cc6af5 110 sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
rary 0:842b08cc6af5 111
rary 0:842b08cc6af5 112 rom.write_low(pointer_low, s, 64); // write tha data
rary 0:842b08cc6af5 113 pointer_low = pointer_low + 64; //アドレスずらし
rary 0:842b08cc6af5 114
rary 0:842b08cc6af5 115 pc.printf("%s\r\n", s); //check用
rary 0:842b08cc6af5 116
rary 0:842b08cc6af5 117 memset(s, '\0', 64 ); //初期化
rary 0:842b08cc6af5 118 }
rary 0:842b08cc6af5 119
rary 0:842b08cc6af5 120 void get_save_high()
rary 0:842b08cc6af5 121 {
rary 0:842b08cc6af5 122 char s[64]; //データ取得配列
rary 0:842b08cc6af5 123
rary 0:842b08cc6af5 124 float accel[3], gyro[3]; //MPU9250data
rary 0:842b08cc6af5 125 float AX, AY, AZ, GX, GY, GZ; //MPU9250data
rary 0:842b08cc6af5 126
rary 0:842b08cc6af5 127 float T = t.read(); //時刻データ取得
rary 0:842b08cc6af5 128 float P = lps.data_read(); //気圧データ取得
rary 0:842b08cc6af5 129 float V = p7100.v_read(); //電圧取得
rary 0:842b08cc6af5 130 float TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:842b08cc6af5 131
rary 0:842b08cc6af5 132 mpu.accel_read(3, accel);
rary 0:842b08cc6af5 133 mpu.gyro_read(3, gyro);
rary 0:842b08cc6af5 134 AX = accel[0];
rary 0:842b08cc6af5 135 AY = accel[1];
rary 0:842b08cc6af5 136 AZ = accel[2];
rary 0:842b08cc6af5 137 GX = gyro[0];
rary 0:842b08cc6af5 138 GY = gyro[1];
rary 0:842b08cc6af5 139 GZ = gyro[2];
rary 0:842b08cc6af5 140
rary 0:842b08cc6af5 141 sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
rary 0:842b08cc6af5 142
rary 0:842b08cc6af5 143 rom.write_high(pointer_high, s, 64); // write tha data
rary 0:842b08cc6af5 144
rary 0:842b08cc6af5 145 twe.printf("%s", s); //無線機送信
rary 0:842b08cc6af5 146
rary 0:842b08cc6af5 147 pointer_high = pointer_high + 64; //アドレスずらし
rary 0:842b08cc6af5 148
rary 0:842b08cc6af5 149 pc.printf("%s\r\n", s); //check用
rary 0:842b08cc6af5 150
rary 0:842b08cc6af5 151 memset(s, '\0', 64 ); //初期化
rary 0:842b08cc6af5 152 }