File content as of revision 1:bb89b58cfa0e:

#include "mbed.h"
#include "BMP180.h"
#include "getGPS.h"
#include "Servo.h"
#include "Movement.h"
#include "direction.h"
#define PIN_SDA D4
#define PIN_SCL D5

GPS gps(D1,D0);
DigitalOut Nichrome(A6);
BMP180 bmp180(PIN_SDA,PIN_SCL);
double bias_la=0,bias_lo=0;

//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;
    }
    
int landing_judgement(){
    
    int x ,y ,n1 , n2 ;
    int landing_judgement=0 ;
    float h,dp,dt,dp0;
    float a ,b ,dp_ave,dt_ave;
    
    bmp180.Initialize(27,BMP180_OSS_ULTRA_LOW_POWER);//27は府大の海抜高度
    
    for(int i=0;i<15;i++){
        if(bmp180.ReadData(&dt,&dp)){
            a = a + dp;
            b = b + dt;
            n1 = n1 + 1;
            n2 = n2 + 1;
            wait(1);
            }
        }
    
    dp_ave = a / n1;
    dt_ave = b / n2;    
    dp0 = calculate_dp0(dp_ave,dt_ave);
    
    while(x<10){
            if(bmp180.ReadData(&dt,&dp)){
                h = calculate_h(dp0,dp,dt);
                if(h >= 30){
                x = x + 1;
                    }
                wait(1);
            }
    }
    //10秒以上高度30mにいた場合離陸判定
    
    wait(10);
      
    while(y<10){
            if(bmp180.ReadData(&dt,&dp)){
                h = calculate_h(dp0,dp,dt); 
                if(h <= 10){
                y = y + 1;
                    }
                wait(1);
            }
    }
    
    wait(5);
    
    landing_judgement = landing_judgement + 1;
    return landing_judgement;
    
    //離陸判定後、10秒以上高度10m以下にいた場合着地判定
    
}        
             
int parachute_separation()
{
        Nichrome=1;
        wait(10);
        Nichrome=0;
    return 0;
    }

int main(void)
{   
int land_judgement_1=0,sep_judge=1;

//着地判定
while(1)
{
    land_judgement_1 = landing_judgement();
    if(land_judgement_1==1)
    {
        break;
    }  
}

wait(30);

//中間地点を経由してゴール地点まで自律移動
direction.walk();

return 0;

}