統合プログラム
Dependencies: mbed Servo BMP180
Diff: Landing_Judgement.h
- Revision:
- 4:6e24a1b3edca
- Parent:
- 3:a583276d9fef
- Child:
- 5:e1001bfc423a
diff -r a583276d9fef -r 6e24a1b3edca Landing_Judgement.h --- a/Landing_Judgement.h Thu Oct 28 07:57:02 2021 +0000 +++ b/Landing_Judgement.h Thu Oct 28 08:31:00 2021 +0000 @@ -1,9 +1,96 @@ #include "mbed.h" #include "BMP180.h" -#include "calculate.h" #define PIN_SDA D4 #define PIN_SCL D5 +//nの階乗を計算する関数 +int fact(int n){ + int i, result = 1; + if(n == 0){ + return 1; + } + else + { + for(i = 1;i <= n;i++) + { + result *= i; + } + return result; + } +} + +float my_pow(float x, int n){ + int i; + float pow_result = 1; + + if(n == 0) + return 1; + else + { + for(i = 0;i < n;i++) + { + pow_result *= x; + } + return pow_result; + } +} + +float my_exp(float x) +{ + int i; + float result = 0; + + + for(i = 1;i <= 25; i++) + { + result += my_pow(x, i) / fact(i); + } + + return result + 1; +} + +float my_log(float x) +{ + int i; + float result1, result2; + + x -= 1; + result1 = 0; + result2 = 0; + + for(i = 1;i <= 40;i++) + { + if(i % 2 == 1) + result1 += my_pow(x, i) / i; + else + + result2 += my_pow(x, i) / i; + } + + return result1 - result2; +} + +//累乗 +float mypow(float x, float y) +{ + return my_exp(y * my_log(x)); +} + +//高度計算 +float calculate_h(float dP0FIX,float dp,float dt){ + float dpow = 1.0/5.256; + float dP0 = 1013.25; + float a = (dt+(float)273.15)/(float)0.0065; + float s = (mypow(dP0/dp,dpow)- mypow(dP0/dP0FIX,dpow))*a - 27; + return s; + } + +//海面気圧計算 +float calculate_dp0(float dp,float dt){ + float s = dp*mypow(1 - (0.0065*27)/(dt+0.0065*27+273.15),-5.257); + return s; + } + BMP180 bmp180(PIN_SDA,PIN_SCL); class Landing_Judgement:public BMP180{ @@ -24,11 +111,9 @@ */ //着地判定の計算を開始させる関数 - int landing_judgement(); -}; - -int Landing_Judgement::landing_judgement(){ - + int landing_judgement1(){ + + BMP180 bmp180(PIN_SDA,PIN_SCL); landing_judgement = 0; x = 0; y = 0; @@ -37,7 +122,7 @@ a = 0; b = 0; - bmp180.Initialize(27,int BMP180_OSS_ULTRA_LOW_POWER);//27は府大の海抜高度 + bmp180.Initialize(27,int BMP180_OSS_ULTRA_LOW_POWER); //27は府大の海抜高度 for(int i=0;i<15;i++){ if(bmp180.ReadData(&dt,&dp)){ @@ -83,4 +168,5 @@ //離陸判定後、10秒以上高度10m以下にいた場合着地判定 -} \ No newline at end of file +} +};