PQ_Hybrid_Electrical_Equipment_Team
新歓2018の新入生モデロケ体験でデモフライトとして打ち上げた電装モデロケの電装モジュールのプログラム
Dependencies: GPS_interrupt LPS25H_lib MadgwickFilter mbed mpu9250_i2c
Revision 0:6a155e0d095d, committed 2018-04-24
- Comitter:
- Sigma884
- Date:
- Tue Apr 24 12:25:58 2018 +0000
- Commit message:
- ??2018?????????????????????????????????????????????????
Changed in this revision
diff -r 000000000000 -r 6a155e0d095d GPS_interrupt.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS_interrupt.lib Tue Apr 24 12:25:58 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/PQ_Hybrid_Electrical_Equipment_Team/code/GPS_interrupt/#23611bb30bc8
diff -r 000000000000 -r 6a155e0d095d LPS25H_lib.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LPS25H_lib.lib Tue Apr 24 12:25:58 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/PQ_Hybrid_Electrical_Equipment_Team/code/LPS25H_lib/#4b9b28d45e51
diff -r 000000000000 -r 6a155e0d095d MadgwickFilter.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MadgwickFilter.lib Tue Apr 24 12:25:58 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/PQ_Hybrid_Electrical_Equipment_Team/code/MadgwickFilter/#c20656d96585
diff -r 000000000000 -r 6a155e0d095d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 24 12:25:58 2018 +0000
@@ -0,0 +1,423 @@
+#include "mbed.h"
+#include "mpu9250_i2c.h"
+#include "MadgwickFilter.hpp"
+#include "LPS25H_lib.h"
+#include "GPS_interrupt.h"
+#include "string.h"
+
+/**************************************
+コンストラクタ
+**************************************/
+Serial pc(USBTX, USBRX, 115200);
+Serial GPS(p9, p10, 115200);
+
+LocalFileSystem local("local");
+
+DigitalOut led_lps(LED1);
+DigitalOut led_mpu(LED2);
+DigitalOut led_gps(LED3);
+DigitalOut led_log(LED4);
+AnalogIn vinread(p15);
+PwmOut buzzer(p21);
+I2C m_i2c(p28, p27);
+
+mpu9250 nine(m_i2c, AD0_HIGH);
+MadgwickFilter attitude(1.0);
+LPS25H_lib LPS25H(LPS25H_lib::AD0_HIGH, m_i2c);
+GPS_interrupt mGPS(&GPS, 115200);
+
+Ticker tick_print;
+Ticker tick_rec;
+Ticker tick_pipi;
+Ticker tick_gps;
+Ticker tick_lps;
+InterruptIn myButton(p18);
+Timer timer_button;
+
+/**************************************
+変数の宣言
+**************************************/
+float gyro[3], gyro_rad[3], acc[3], mag[3], euler[3];
+Quaternion quart(1.0, 0.0, 0.0, 0.0);
+float pres, temp, alt, pres_0, temp_0, speed, pres_old;
+float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
+int gps_sat;
+float vin;
+
+bool pipi_now, save, button, top_flight, sec_flight;
+bool flag_test_gps = true; //GPSの設定をするか
+
+FILE *fp;
+int time_button;
+
+/**************************************
+関数のプロトタイプ宣言
+**************************************/
+void setup();
+void printData();
+void resetPT();
+void startRec();
+void stopRec();
+void recData();
+void pipi();
+void lvup();
+void tone(float freq);
+
+void readGPS();
+void readLPS();
+void readMPU();
+float deg2rad(float deg);
+float rad2deg(float rad);
+
+void buttonPush();
+void buttonRelease();
+
+/**************************************
+メイン関数
+**************************************/
+int main() {
+ setup();
+ lvup();
+
+ myButton.mode(PullUp);
+ myButton.fall(&buttonPush);
+ myButton.rise(&buttonRelease);
+
+ tick_print.attach(&printData, 1.0f);
+ tick_gps.attach(&readGPS, 0.1f);
+ tick_lps.attach(&readLPS, 0.05f);
+
+ while(1) {
+ readMPU();
+ }
+}
+
+/**************************************
+データの表示
+**************************************/
+void printData(){
+ for(int i = 0; i < 20; i ++){
+ pc.printf("*");
+ }
+ pc.printf("\r\n");
+ pc.printf("TIME -> %2d:%2d:%02.2f\r\n", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]);
+ pc.printf("GPS -> %3.7f(N), %3.7f(E), %.2f[m]\r\n", gps_lat, gps_lon, gps_alt);
+ pc.printf("SPEED -> %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
+ pc.printf("SAT -> %d\r\n", gps_sat);
+ pc.printf("LPS25H -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
+ pc.printf("SPEED -> %.4f[m/s]\r\n", speed);
+ pc.printf("BATTERY -> %.2f[V]\r\n", vin);
+ pc.printf("GYRO -> %.4f, %.4f, %.4f\r\n", gyro[0], gyro[1], gyro[2]);
+ pc.printf("ACC -> %.4f, %.4f, %.4f\r\n", acc[0], acc[1], acc[2]);
+ pc.printf("MAG -> %.2f, %.2f, %.2f\r\n", mag[0], mag[1], mag[2]);
+ pc.printf("QUART -> %f, %f, %f, %f\r\n", quart.w, quart.x, quart.y, quart.z);
+ pc.printf("EULER -> %f, %f, %f\r\n", euler[0], euler[1], euler[2]);
+ pc.printf("TOP -> %d, %d\r\n", top_flight, sec_flight);
+ pc.printf("SAVE -> %d\r\n\n\n", save);
+}
+
+/**************************************
+高度のリセット
+**************************************/
+void resetPT(){
+ pres_0 = pres;
+ temp_0 = temp;
+ lvup();
+}
+
+/**************************************
+記録開始
+**************************************/
+void startRec(){
+ fp = fopen("/local/rocketlog.txt", "a");
+ pc.printf("START SAVING\r\n");
+ led_log = 1;
+ wait(1.0f);
+}
+
+/**************************************
+記録終了
+**************************************/
+void stopRec(){
+ fprintf(fp, "\r\n\n");
+ fclose(fp);
+ pc.printf("STOP SAVING\r\n");
+ led_log = 0;
+ wait(1.0f);
+}
+
+/**************************************
+記録書き込み
+**************************************/
+void recData(){
+ fprintf(fp, "%d/%d/%d,", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付
+ fprintf(fp, "%2d:%2d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
+ fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
+ fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
+ fprintf(fp, "%d,", gps_sat); //GPS衛星数
+ fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt); //気圧センサ
+ fprintf(fp, "%.4f,", speed); //Z軸速度
+ fprintf(fp, "%.2f,", vin); //電源電圧
+ fprintf(fp, "%.4f,%.4f,%.4f,", gyro[0], gyro[1], gyro[2]); //ジャイロ
+ fprintf(fp, "%.4f,%.4f,%.4f,", acc[0], acc[1], acc[2]); //加速度
+ fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]); //地磁気
+ fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z); //クオータニオン
+ fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]); //姿勢角
+ fprintf(fp, "%d,%d\r\n", top_flight, sec_flight); //頂点検知
+}
+
+/**************************************
+ブザー
+**************************************/
+void pipi(){
+ if(pipi_now){
+ buzzer = 0.0f;
+ pipi_now = !pipi_now;
+ }
+ else{
+ float T = 1.0f / (4.0*440.000f);
+ buzzer.period(T);
+ buzzer = 0.5f;
+ pipi_now = !pipi_now;
+ }
+}
+
+/**************************************
+レベルアップ
+**************************************/
+void lvup(){
+ for(int i = 1; i <= 4; i ++){
+ tone(4.0*349.228f);
+ wait(0.15f);
+ }
+ tone(0.0f);
+ wait(0.15f);
+ tone(4.0*311.127f);
+ wait(0.15f);
+ tone(0.0f);
+ wait(0.15f);
+ tone(4.0*391.995f);
+ wait(0.15f);
+ tone(0.0f);
+ wait(0.15f);
+ tone(4.0*349.228f);
+ wait(1.0f);
+ tone(0.0f);
+ wait(1.0f);
+}
+
+/**************************************
+音を鳴らす
+**************************************/
+void tone(float freq){
+ float T = 0;
+
+ if(freq == 0.0f){
+ buzzer = 0.0f;
+ }
+
+ T = 1.0f / freq;
+ buzzer.period(T);
+ buzzer = 0.5f;
+}
+
+/**************************************
+GPS読み取り
+**************************************/
+void readGPS(){
+ gps_lat = mGPS.Latitude();
+ gps_lon = mGPS.Longitude();
+ gps_alt = mGPS.Height();
+ mGPS.getUTC(gps_utc);
+ gps_utc[3] += 9;
+ if(gps_utc[3] >= 24){
+ gps_utc[3] -= 24;
+ }
+ gps_knot = mGPS.Knot();
+ gps_deg = mGPS.Degree();
+ gps_sat = mGPS.Number();
+
+ vin = vinread.read() * 8.4;
+}
+
+/**************************************
+気圧センサー読み取り・速度計算
+**************************************/
+void readLPS(){
+ pres_old = pres;
+ pres = LPS25H.getPres();
+ temp = LPS25H.getTemp();
+ alt = LPS25H.getAlt(pres_0, temp_0);
+ speed = (pres - pres_old) / 0.05;
+
+ int top_c = 0;
+ if(alt > 70 && speed < 3.0f && !top_flight){
+ top_c ++;
+ if(top_c > 10){
+ top_c = 0;
+ top_flight = true;
+ }
+ }
+ if(alt < 30 && speed < 0.0f && top_flight && !sec_flight){
+ top_c ++;
+ if(top_c > 10){
+ top_c = 0;
+ sec_flight = true;
+ }
+ }
+}
+
+/**************************************
+9軸センサー読み取り・姿勢計算
+**************************************/
+void readMPU(){
+ nine.getGyro(gyro);
+ nine.getAcc(acc);
+ nine.getMag(mag);
+
+ for(int i = 0; i < 3; i ++){
+ gyro_rad[i] = deg2rad(gyro[i]);
+ }
+ attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
+ attitude.getAttitude(&quart);
+ attitude.getEulerAngle(euler);
+ for(int i = 0; i < 3; i ++){
+ euler[i] = rad2deg(euler[i]);
+ }
+}
+
+/**************************************
+度数法 -> 弧度法
+**************************************/
+float deg2rad(float deg){
+ return deg * 0.017453292519943; //* 3.1415926535 / 180;
+}
+
+/**************************************
+弧度法 -> 度数法
+**************************************/
+float rad2deg(float rad){
+ return rad * 57.29577951308232; //* 180 / 3.145926535;
+}
+
+/**************************************
+ボタンが押されたとき
+**************************************/
+void buttonPush(){
+ if(!button){
+ button = true;
+ timer_button.reset();
+ timer_button.start();
+ }
+}
+
+/**************************************
+ボタンが離されたとき
+**************************************/
+void buttonRelease(){
+ if(button){
+ button = false;
+ time_button = timer_button.read();
+ timer_button.stop();
+ if(time_button >= 2){
+ if(!save){
+ save = true;
+ startRec();
+ tick_rec.detach();
+ tick_rec.attach(&recData, 0.05f);
+ tick_pipi.attach(&pipi, 0.5f);
+ }
+ else{
+ save = false;
+ stopRec();
+ tick_rec.detach();
+ tick_pipi.detach();
+ tone(0.0f);
+ }
+ }
+ else{
+ resetPT();
+ }
+ }
+}
+
+/**************************************
+セットアップ（最初に1回実行）
+**************************************/
+void setup(){
+ wait(0.5f);
+ char setup_string[1024];
+
+ pc.printf("\r\n\nSetting Start\r\n");//
+ strcat(setup_string, "\r\n\nSetting Start\r\n");
+
+ LPS25H.begin(25);
+ LPS25H.setFIFO(16);
+ if(LPS25H.whoAmI() == 0){
+ pc.printf("LPS25H OK!!\r\n");//
+ strcat(setup_string, "LPS25H OK!!\r\n");
+ led_lps = 1;
+ }
+ else{
+ pc.printf("LPS25H NG...\r\n");//
+ strcat(setup_string, "LPS25H NG...\r\n");
+ led_lps = 0;
+ }
+
+ nine.setAcc(_16G);
+ nine.setGyro(_2000DPS);
+ nine.setOffset(-2.17859, -0.11247, 0.924360,
+ 0.005295, 0.019147, 0.056065,
+ 49.425, -5.55, 75.15);
+ int check_nine = 0;
+ if(nine.senserTest()){
+ pc.printf("MPU9250 OK!!\r\n");//
+ strcat(setup_string, "MPU9250 OK!!\r\n");
+ check_nine ++;
+ }
+ else{
+ pc.printf("MPU9250 NG...\r\n");//
+ strcat(setup_string, "MPU9250 NG...\r\n");
+ }
+ if(nine.mag_senserTest()){
+ pc.printf("MPU9250 MAG OK!!\r\n");//
+ strcat(setup_string, "MPU9250 MAG OK!!\r\n");
+ check_nine ++;
+ }
+ else{
+ pc.printf("MPU9250 MAG NG...\r\n");//
+ strcat(setup_string, "MPU9250 MAG NG...\r\n");
+ }
+ if(check_nine == 2){
+ led_mpu = 1;
+ }
+ else{
+ led_mpu = 0;
+ }
+
+ if(flag_test_gps){
+ pc.printf("GPS setting");//
+ strcat(setup_string, "GPS setting");
+ led_gps = 0;
+
+ while(!mGPS.gps_readable){
+ pc.printf(".");//
+ strcat(setup_string, ".");
+ wait(1.0f);
+ }
+
+ pc.printf(" Finished!!\r\n");//
+ strcat(setup_string, " Finished!!\r\n");
+ led_gps = 1;
+ }
+
+ pc.printf("All setting finished!! -> Start!!\r\n");//
+ strcat(setup_string, "All setting finished!! -> Start!!\r\n");
+
+ led_log = 1;
+ fp = fopen("/local/rocketlog.txt", "a");
+ fprintf(fp, setup_string);
+ fclose(fp);
+ setup_string[0] = NULL;
+ led_log = 0;
+}
diff -r 000000000000 -r 6a155e0d095d mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Apr 24 12:25:58 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file
diff -r 000000000000 -r 6a155e0d095d mpu9250_i2c.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpu9250_i2c.lib Tue Apr 24 12:25:58 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/PQ_Hybrid_Electrical_Equipment_Team/code/mpu9250_i2c/#cf5787acc101