File content as of revision 9:182021b1df79:

#include "mbed.h"
//#include "rtos.h"
#include "MPU9250.h"
#include "SkipperSv2.h"
#include "Estrela.h"
//#include "stm32f4xx_hal_iwdg.h"


#define UPD_MANUAL  0
#define UPD_AUTO    1
#define UPD_LANDING 2

#define SBUS_SIGNAL_OK          0x00
#define SBUS_SIGNAL_LOST        0x01
#define SBUS_SIGNAL_FAILSAFE    0x03

//Serial pc(TX, RX);
RawSerial sbus_(PA_9, PA_10); //SBUS

PwmOut servo1(PC_6); // TIM3_CH1
PwmOut servo2(PC_7); // TIM3_CH2    //PC_7
PwmOut servo3(PB_0); // TIM3_CH3
PwmOut servo4(PB_1); // TIM3_CH4
PwmOut servo5(PB_6); // TIM4_CH1
PwmOut servo6(PB_7); // TIM4_CH2
PwmOut servo7(PB_8); // TIM4_CH3    //PB_8
PwmOut servo8(PB_9); // TIM4_CH4

RawSerial pc(PA_2, PA_3);

DigitalOut led1(PA_0);  //緑
DigitalOut led2(PA_1);  //赤
DigitalOut led3(PB_4);  //赤外線
DigitalOut led4(PB_5);

volatile uint8_t buf_sbus[25]; 
volatile int cnt_sbus = 0;
static int16_t channels[18];
uint8_t  failsafe_status = SBUS_SIGNAL_FAILSAFE;
static bool flg_ch_update = false;
uint8_t i,j,k;
uint8_t update_mode = 0;
static uint16_t pwm[8];
static uint16_t oldTHL;

//9軸センサー関連
float sum = 0;
uint32_t sumCount = 0;
MPU9250 mpu9250;
Timer t;

/*
float g_BefGyro[3] = {0,0,0};
double g_HpfGyro[3] = {0,0,0};
double g_HpfGyro1[3] = {0,0,0};
double g_LpfAccel[3] = {0,0,0};
*/

static int jj = 0;

//IWDG_HandleTypeDef hiwdg;

void disp_clock(){
    pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
    pc.printf("HCLK Clock   = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
    pc.printf("PCLK1 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
    pc.printf("PCLK2 Clock  = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
    pc.printf("\r\n");
    }

//サーボ初期化関数
void init_Servo()
{
    servo1.period_ms(14);
    servo1.pulsewidth_us(1500);
    
    servo2.period_ms(14);
    servo2.pulsewidth_us(1500);
    
    servo3.period_ms(14);
    servo3.pulsewidth_us(1000);
    
    servo4.period_ms(14);
    servo4.pulsewidth_us(1500);
    
    servo5.period_ms(14);
    servo5.pulsewidth_us(1500);
    
    servo6.period_ms(14);
    servo6.pulsewidth_us(1500);
    
    servo7.period_ms(14);
    servo7.pulsewidth_us(1500);
    
    servo8.period_ms(14);
    servo8.pulsewidth_us(1500);
    
    for (j=0; j<8; j++) {pwm[j] = 1500;}
    pc.printf("servo initialized\r\n");
}

//---setall_servo---
//pwmをサーボに出力する関数。
void setall_servo()
{
    
    
    //if(failsafe_status == SBUS_SIGNAL_OK){
    servo1.pulsewidth_us(pwm[0]);
    servo2.pulsewidth_us(pwm[1]);
    servo3.pulsewidth_us(pwm[2]);
    servo4.pulsewidth_us(pwm[3]);
    servo5.pulsewidth_us(pwm[4]);
    servo6.pulsewidth_us(pwm[5]);
    servo7.pulsewidth_us(pwm[6]);
    servo8.pulsewidth_us(pwm[7]);   
      /*  }else{
        servo1.pulsewidth_us(1000);
        servo2.pulsewidth_us(1000);
        servo3.pulsewidth_us(1000);
        servo4.pulsewidth_us(1000);
        servo5.pulsewidth_us(1000);
        servo6.pulsewidth_us(1000);
        servo7.pulsewidth_us(1000);
        servo8.pulsewidth_us(1000);
    }*/
/*
    for(j=0;j<8;j++){
        pc.printf("ch%d=%d ", j+1, pwm[j]);
    }
    //pc.printf("time = %d", frq.read_ms()); 
    pc.printf("\r\n");
*/
    //frq.reset();
}

//---update_pwm---
//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
void update_pwm()
{
    if(flg_ch_update == true){
    switch(update_mode){    //マニュアルモード,自動モード,自動着陸もモードを切替
        case UPD_MANUAL:    //マニュアルモード
            for(j=0;j<8;j++){
                pwm[j] = (channels[j]>>1) + 1000;
            }
            oldTHL = pwm[2];
            jj=0;
            //pc.printf("update_manual\r\n");
            break;
        
        case UPD_AUTO:      //自動モード
            pwm[0] = (channels[0]>>1) + 1000;
            pwm[1] = Auto_ELE;
            pwm[2] = Auto_THR;
            pwm[3] = Auto_RUD;
            pwm[4] = (channels[4]>>1) + 1000;
            pwm[5] = (channels[5]>>1) + 1000;
            pwm[6] = (channels[6]>>1) + 1000;
            pwm[7] = (channels[7]>>1) + 1000;
 
            //pc.printf("update_auto\r\n");
            break;
        
        case UPD_LANDING:   //自動着陸モード
            pwm[0] = (channels[0]>>1) + 1000;
            pwm[1] = Auto_ELE;
            pwm[2] = (channels[2]>>1) + 1000;
            pwm[3] = (channels[3]>>1) + 1000;
            pwm[4] = (channels[4]>>1) + 1000;
            pwm[5] = (channels[5]>>1) + 1000;
            pwm[6] = (channels[6]>>1) + 1000;
            pwm[7] = (channels[7]>>1) + 1000;
            
        default:            //デフォはマニュアルモード
            for(j=0;j<8;j++){
                pwm[j] = (channels[j]>>1) + 1000;
            }

            jj=0;
            //pc.printf("update_manual\r\n");
            break;
    }
    }
    flg_ch_update = false;
    ///oldTHL = pwm[2]; 
          setall_servo();  
}

/*void update_auto()
{
    pwm[0] = (channels[0]>>1) + 1000;
    pwm[1] = Auto_ELE;
    pwm[2] = Auto_THR;
    pwm[3] = Auto_RUD;
    pwm[4] = (channels[4]>>1) + 1000;
    pwm[5] = (channels[5]>>1) + 1000;
    pwm[6] = (channels[6]>>1) + 1000;
    pwm[7] = (channels[7]>>1) + 1000;
    
    setall_servo();
    //pc.printf("update_auto\r\n");
}*/

//---update_Input---
//catch_sbusで取得したSbusの生データから各チャンネルのpwmを抽出する。
void update_Input()
{      /*    for (i=0; i<25; i++){
        pc.printf("%x ", buf_sbus[i]);
        }pc.printf("\n"); 
*/
    // clear channels[]
    for (i=0; i<18; i++) {channels[i] = 0;}

    // reset counters
    uint8_t byte_in_sbus = 1;
    uint8_t bit_in_sbus = 0;
    uint8_t ch = 0;
    uint8_t bit_in_channel = 0;

    // process actual sbus data
    for (i=0; i<176; i++) {
        if (buf_sbus[byte_in_sbus] & (1<<bit_in_sbus)) {
            channels[ch] |= (1<<bit_in_channel);
        }
        bit_in_sbus++;
        bit_in_channel++;

        if (bit_in_sbus == 8) {
            bit_in_sbus =0;
            byte_in_sbus++;
        }
        if (bit_in_channel == 11) {
            bit_in_channel =0;
            ch++;
        }
    }
    // DigiChannel 1
    if (buf_sbus[23] & (1<<0)) {
        channels[16] = 1;
    }else{
        channels[16] = 0;
    }
    // DigiChannel 2
    if (buf_sbus[23] & (1<<1)) {
        channels[17] = 1;
    }else{
        channels[17] = 0;
    }
    // Failsafe
    failsafe_status = SBUS_SIGNAL_OK;
    if (buf_sbus[23] & (1<<2)) {
        failsafe_status = SBUS_SIGNAL_LOST;
    }
    if (buf_sbus[23] & (1<<3)) {
        failsafe_status = SBUS_SIGNAL_FAILSAFE;
    }
    //channels[i] = channels[i]>>1;
    for (i=0; i<25; i++) {buf_sbus[i] = 0;}
    cnt_sbus = 0;
    flg_ch_update = true;
    
    update_pwm();
        

}

//---catch_sbus---
//受信機から送られてくるSBus信号(シリアル)を取得する関数
void catch_sbus()
{
    buf_sbus[cnt_sbus] = sbus_.getc();
    if(buf_sbus[0] == 0x0f)
        {
            cnt_sbus ++;
        }
    if(cnt_sbus >=25){  
        if(buf_sbus[24] == 0x00){
            update_Input();     //update_Input実行
        }else{
            cnt_sbus = 0;
        }
    }
return;
}

//---init_sbus---
//Sbusを初期化する関数
void init_sbus(){
    // Set Baudrate
    sbus_.baud(100000);
    // Set Datalenght & Frame
    sbus_.format(8, Serial::Even, 2);
    //start sbus irq
    sbus_.attach(&catch_sbus, RawSerial::RxIrq);
    }

//---CheckSW_up---
//プロポのスイッチがonかoffか返す関数
//ch 1~8
bool CheckSW_up(int8_t ch)
{
    uint16_t checkpwm;
    checkpwm = (channels[ch-1]>>1) + 1000;
    //pc.printf("ch%d=%d ", ch, checkpwm);
    if(SWITCH_CHECK > checkpwm){
        return true;
    }else{
        return false;
    }
}


        
//---sensing---
//センサーの値を読み込み、各種フィルタをかける(認知)
void sensing()
{
    // If intPin goes high, all data registers have new data
    if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt

        mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
        // Now we'll calculate the accleration value into actual g's
        ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
        ay = (float)accelCount[1]*aRes - accelBias[1];   
        az = (float)accelCount[2]*aRes - accelBias[2];  
   
        mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
        // Calculate the gyro value into actual degrees per second
        gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
        gy = (float)gyroCount[1]*gRes - gyroBias[1];  
        gz = (float)gyroCount[2]*gRes - gyroBias[2];   
  
        mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
        // Calculate the magnetometer values in milliGauss
        // Include factory calibration per data sheet and user environmental corrections
        mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
        my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
        mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
    }
   
    Now = t.read_us();
    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
    lastUpdate = Now;
    
    sum += deltat;
    sumCount++;
    
//  if(lastUpdate - firstUpdate > 10000000.0f) {
//      beta = 0.04;  // decrease filter gain after stabilized
//      zeta = 0.015; // increasey bias drift gain after stabilized
//  }
    
   // Pass gyro rate as rad/s
    mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
    mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

    /*  Auto_Loop()で50ms待ってるから要らないと思う
    // Serial print and/or display at 0.5 s rate independent of data rates
    delt_t = t.read_ms() - count;
    if (delt_t > 500) { // update LCD once per half-second independent of read rate
    */

    //pc.printf("ax = %f", 1000*ax); 
    //pc.printf(" ay = %f", 1000*ay); 
    //pc.printf(" az = %f  mg\n\r", 1000*az); 

    //pc.printf("gx = %f", gx); 
    //pc.printf(" gy = %f", gy); 
    //pc.printf(" gz = %f  deg/s\n\r", gz); 
    
    //pc.printf("mx = %f", mx); 
    //pc.printf(" my = %f", my); 
    //pc.printf(" mz = %f  mG\n\r", mz); 
    
    tempCount = mpu9250.readTempData();  // Read the adc values
    temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
    //pc.printf(" temperature = %f  C\n\r", temperature); 
    
    //pc.printf("q0 = %f\n\r", q[0]);
    //pc.printf("q1 = %f\n\r", q[1]);
    //pc.printf("q2 = %f\n\r", q[2]);
    //pc.printf("q3 = %f\n\r", q[3]);      
    
      
  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
  // In this coordinate system, the positive z-axis is down toward Earth. 
  // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    pitch  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI; 
    yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    roll  *= 180.0f / PI;

    //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100));
    //pc.printf("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
    //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
 
    count = t.read_ms(); 
    sum = 0;
    sumCount = 0; 
    //}           
}


//---CalibrateCompass---
//グラウンドチェック時にスイッチ7だけをonにしておくことで実行 リポ抜いたらやり直ししなくてはならない
void CalibrateCompass(void)
{
    led1 = 1;
    sensing();
    uint8_t ii;
    float mxMAX=mx, mxMIN=mx;
    float myMAX=my, myMIN=my;
    magbias[0] = 0;    //初期化
    magbias[1] = 0;
    //magbias[2] = 0;   
    ii=0;
    while(1){   //キャリブレーション実行   led2が点滅時に機体を一回転させる xy方向のみ
        sensing();
        if(mxMAX < mx) mxMAX = mx;
        if(mxMIN > mx) mxMIN = mx;
        if(myMAX < my) myMAX = my;
        if(myMIN > my) myMIN = my;
        led2 = !led2;
        ++ii;
        wait(50);
        
        if(!CheckSW_up(7)) break;   //スイッチ7を下げて終了
    }
    led2 = 0;
    if(ii<20){  //チャンネル7をすぐ切った場合 元に戻す
        magbias[0] = MAGBIASX;
        magbias[1] = MAGBIASY;
    }else{  //しっかりと値を入れた場合
        magbias[0] = (mxMAX+mxMIN)/2;
        magbias[1] = (myMAX+myMIN)/2;
    }
    led1 = 0;
    t.reset();  //タイマーをリセットして終了
}  

    
//---StabiGyro---
//水平旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
//右旋回
void StabiGyro()
{
    if(jj<=20){     //旋回し始め  20
        Auto_RUD = RUD_N + -105 +int((-30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy        3.5
        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
        Auto_THR = oldTHL + 50;     
        //pc.printf("first%d\r\n",jj);
        jj++;
    }else{          //旋回中
        Auto_RUD = RUD_N + -70 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -100 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = oldTHL;
    }
    
    //pwmの値が最小値と最大値の間に入ってなかったら、強制的に最大値or最小値にする
    if(Auto_RUD > RUD_MAX){
        Auto_RUD = RUD_MAX;
    }else if(Auto_RUD < RUD_MIN){
        Auto_RUD = RUD_MIN;
    }
    if(Auto_ELE > ELE_MAX){
        Auto_ELE = ELE_MAX;
    }else if(Auto_ELE < ELE_MIN){
        Auto_ELE = ELE_MIN;
    }
        
    //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
    //Thread::wait(G_MS);
}

//---StabiGyro_Mobius---
//8の字旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
void StabiGyro_Mobius()
{
    if(jj<=25){     //右旋回導入
        Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy        3.5
        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
        Auto_THR = oldTHL + 50;       
        //pc.printf("first%d\r\n",jj);
        jj++;
    }else if((25<jj) && (jj<=112)){     //右旋回途中
        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = oldTHL;
        jj++;
    }else if((112<jj) && (jj<=117)){    //旋回遷移1
        Auto_RUD = RUD_N + int((0 -roll)*RUDDER_GN - (gy-100)*RUD_DGN);  
        Auto_ELE = ELE_N + -30 + int((-10 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
        Auto_THR = THR_N - 100;      
        //pc.printf("first%d\r\n",jj);
        jj++;
    }else if((117<jj) && (jj<=132)){    //旋回遷移2
        Auto_RUD = RUD_N + 105 + int((30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = oldTHL - 20; 
        jj++;
    }else{                              //左旋回途中
        Auto_RUD = RUD_N + 80 + int((23-roll)*RUDDER_GN - gy*RUD_DGN);  
        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
        Auto_THR = THR_N + 30;   
    }
        
    if(Auto_RUD > RUD_MAX){
        Auto_RUD = RUD_MAX;
    }else if(Auto_RUD < RUD_MIN){
        Auto_RUD = RUD_MIN;
    }
    if(Auto_ELE > ELE_MAX){
        Auto_ELE = ELE_MAX;
    }else if(Auto_ELE < ELE_MIN){
        Auto_ELE = ELE_MIN;
    }
        
    //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
    //Thread::wait(G_MS);
}

//---StabiGyro_Climb---
//上昇旋回のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
void StabiGyro_Climb()
{
    if(jj<=25){     //右旋回導入
        Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy        3.5
        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
        Auto_THR = oldTHL + 50;            
        //pc.printf("first%d\r\n",jj);
        jj++;
    }else if((25<jj) && (jj<=224)){     //右旋回途中
        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = oldTHL;
        jj++;
    /*}else if((224<jj) && (jj<=234)){    //上昇遷移
        Auto_RUD = RUD_N + 160 - int((g_HpfGyro[0]+25)*0.75);
        Auto_ELE = ELE_N + 20 + int((g_HpfGyro[1]-30)*0.8);
        Auto_THR = THR_N + 100;     
        //pc.printf("first%d\r\n",jj);
        jj++;*/
    }else if((224<jj) && (jj<=314)){    //上昇中
        Auto_RUD = RUD_N + -60 + int((-20-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -140 + int((-30 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = oldTHL + 220;
        jj++;
    }else{                              //水平旋回
        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = oldTHL;
    }
        
    if(Auto_RUD > RUD_MAX){
        Auto_RUD = RUD_MAX;
    }else if(Auto_RUD < RUD_MIN){
        Auto_RUD = RUD_MIN;
    }
    if(Auto_ELE > ELE_MAX){
        Auto_ELE = ELE_MAX;
    }else if(Auto_ELE < ELE_MIN){
        Auto_ELE = ELE_MIN;
    }
        
    //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
    //Thread::wait(G_MS);
}

//---StabiGyro_Glide---
//自動滑空のためのラダー、エレベーターの角度(要はpwm)を計算する。(判断)
void StabiGyro_Glide()
{
    //if(jj<=25){
    //    Auto_RUD = RUD_N - 30 - int((g_HpfGyro[0]+130)*0.75);
    //    Auto_ELE = ELE_N + 30+ int((g_HpfGyro[1]-30)*0.8);
    //    Auto_THR = THR_N + 50;     
        //pc.printf("first%d\r\n",jj);
    //    jj++;
    //}else{
        Auto_RUD = RUD_N + -60 + int((-18-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
        Auto_ELE = ELE_N + -110 + int((-24 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
        Auto_THR = 1000;    //スロットルoff
    
    if(Auto_RUD > RUD_MAX){
        Auto_RUD = RUD_MAX;
    }else if(Auto_RUD < RUD_MIN){
        Auto_RUD = RUD_MIN;
    }
    if(Auto_ELE > ELE_MAX){
        Auto_ELE = ELE_MAX;
    }else if(Auto_ELE < ELE_MIN){
        Auto_ELE = ELE_MIN;
    }
        
    //pc.printf("rud %d\tele %5d\r\n", Auto_RUD, Auto_ELE);
    //Thread::wait(G_MS);
}

void StabiGyro_Landing()
{
    Auto_ELE = ELE_N + -80 + int((-20 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
    //Auto_THR = 
}

void Auto_Loop()
{
    //while(true){
    //for(k=0;k<100;++k){
        //sensing();      //センサーの値を読み込む(認知)
        StabiGyro();    //水平旋回のためのラダー、エレベーターのpwmを計算(判断)
        update_mode = UPD_AUTO;     //オートモード
        //StabiAccel();
        //Auto_ELE+=145;
        //Auto_RUD-=80;
        //Servos.Auto2Servo();
        //Thread::wait(50);
    /*
        if(!CheckSW_up(7)){
            pc.printf("go to manual\n");
            break;
        }*/
        //wait_ms(50);        
    //}   
    //k=0; //for debug
    //pwm[6]=1000;
}

void Auto_Mobius()
{
    sensing();
    StabiGyro_Mobius();
    update_mode = UPD_AUTO;
    wait_ms(50);
}

void Auto_Climb()
{
    sensing();
    StabiGyro_Climb();
    update_mode = UPD_AUTO;
    wait_ms(50);   
}

void Auto_Glide()
{
    sensing();
    StabiGyro_Glide();
    update_mode = UPD_AUTO;
    wait_ms(50);
}

void Auto_Landing()
{   
    sensing();
    StabiGyro_Landing();
    update_mode = UPD_LANDING;
    wait_ms(50);
}

int main()
{
    //SBus用バッファ初期化
    int i;
    for (i=0; i<25; i++) {buf_sbus[i] = 0;}
    
    //pc.baud(115200);    //パソコン通信用シリアルのボードレート
           
    //frq.start();     
    init_Servo();
    init_sbus();    
    disp_clock();
   
    //---9軸センサー(MPU9250)初期化--- 
    
    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C  
    pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
    t.start();
    
    // Read the WHO_AM_I register, this is a good test of communication
    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
    pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
  
    if (whoami == 0x71){ // WHO_AM_I should always be 0x68
        pc.printf("MPU9250 is online...\n\r");
        //wait(1);
     
        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
        //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
        //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
        //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
        //pc.printf("x accel bias = %f\n\r", accelBias[0]);
        //pc.printf("y accel bias = %f\n\r", accelBias[1]);
        //pc.printf("z accel bias = %f\n\r", accelBias[2]);
        wait(1);
        
        mpu9250.initMPU9250(); 
        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
        mpu9250.initAK8963(magCalibration);
        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
        pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
        pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
        wait(2);
    
    }else{
        pc.printf("Could not connect to MPU9250: \n\r");
        pc.printf("%#x \n",  whoami); 
        while(1) ; // Loop forever if communication doesn't happen
    }

    mpu9250.getAres(); // Get accelerometer sensitivity
    mpu9250.getGres(); // Get gyro sensitivity
    mpu9250.getMres(); // Get magnetometer sensitivity
    //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
    //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
    //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
    magbias[0] = MAGBIASX;       //+470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
    magbias[1] = MAGBIASY;       //+120.;  // User environmental y-axis correction in milliGauss
    magbias[2] = MAGBIASZ;       //+125.;  // User environmental z-axis correction in milliGauss
    
    t.start();
    pc.printf("mpu9250 initialized\r\n");
    
    //---9軸センサー(MPU9250)初期化終わり---   
          
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    
    pc.printf("All initialized\r\n");
    

    while (true) {      //無限ループ
        //for(i=0;i<8;i++){
        //pc.printf("%x ", buf_sbus[i]);
        //pc.printf("ch%d=%d ", i+1, channels[i]);
        //}
        //pc.printf("\n");
        sensing();
        wait_ms(50);
        
        switch(OperationMode){      //変数OperationModeの値で場合分け
            case GROUNDCHECK:   //グラウンドチェック(スイッチが2つ初期値にちゃんとなってたら水平旋回モードに移行)
                if(!CheckSW_up(7)&&!CheckSW_up(8)){     //チャンネル7,8が両方offなら
                    OperationMode=AUTOLOOP;             //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
                    pc.printf("go to autoloop\r\n");
                }/*else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル７だけonなら
                    CalibrateCompass();
                }*/
                //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
                update_mode = UPD_MANUAL;   //マニュアルモード(そのまま垂れ流す)
               // pc.printf("groundcheck mode\r\n");
                
                break;
            case AUTOLOOP:      //水平旋回モード
                //t.reset();
                if(CheckSW_up(7)){  //チャンネル7がonなら
                    //t.reset();
                    Auto_Loop();    //関数Auto_Loop実行
                    led1 = 0;
                    led2 = 1;
                    //Auto_Loop();
                    //pc.printf("Auto Loop Now\r\n");
                    //autoloop.set_priority(osPriorityAboveNormal);
                    /*
                    while(true){
                        if(!CheckSW_up(7)){
                            autoloop.terminate();
                            break;
                        }
                    }
                    */     
                }else{
                    update_mode = UPD_MANUAL;
                    //pc.printf("manual Now\r\n");
                    led1 = 1;
                    led2 = 0;
                    
                    /*debug*/
                    //k++;
                    //wait(50);
                    //if(k>100) pwm[6]=2000;    
                }  
                
                if(!CheckSW_up(7)&&CheckSW_up(8)){  //チャンネル7がoff、8がonなら
                    OperationMode=AUTOMOBIUS;
                    pc.printf("go to auto8\r\n");
                }
                break;
            case AUTOMOBIUS:    //8の字旋回モード
                if(CheckSW_up(7)){
                    Auto_Mobius();
                    led1 = 0;
                    led2 = 1;
                    pc.printf("Auto Mobius Now\r\n");                    
                }else{
                    update_mode = UPD_MANUAL;
                    pc.printf("manual Now\r\n");
                    led1 = 1;
                    led2 = 1;
                }  
                
                if(!CheckSW_up(7)&&!CheckSW_up(8)){
                    OperationMode=AUTOCLIMB;
                    pc.printf("go to autoclimb\r\n");
                }
                break;
            case AUTOCLIMB:     //上昇旋回モード
                if(CheckSW_up(7)){
                    Auto_Climb();
                    led1 = 0;
                    led2 = 1;
                    pc.printf("Auto Climb Now\r\n");                    
                }else{
                    update_mode = UPD_MANUAL;
                    pc.printf("manual Now\r\n");
                    led1 = 1;
                    led2 = 0;
                }  
                
                if(!CheckSW_up(7)&&CheckSW_up(8)){
                    OperationMode=AUTOGLIDE;
                    pc.printf("go to manual\r\n");
                }
                break;
            
            case AUTOGLIDE:     //自動滑空モード
                if(CheckSW_up(7)){
                    Auto_Glide();
                    led1 = 0;
                    led2 = 1;
                    pc.printf("Auto Glide Now\r\n");                    
                }else{
                    update_mode = UPD_MANUAL;
                    pc.printf("manual Now\r\n");
                    led1 = 1;
                    led2 = 1;
                }  
                
                if(!CheckSW_up(7)&&!CheckSW_up(8)){
                    OperationMode=AUTOLANDING;
                    pc.printf("go to autoloop\r\n");
                }
                break;
            
            case AUTOLANDING:
                led3 = 1; //赤外線
                if(CheckSW_up(7)){
                    Auto_Landing();
                    led1 = 0;
                    led2 = 1;
                    pc.printf("Auto Landing Now\r\n");                    
                }else{
                    update_mode = UPD_MANUAL;
                    pc.printf("manual Now\r\n");
                    led1 = 1;
                    led2 = 0;
                }  
                
                if(!CheckSW_up(7)&&CheckSW_up(8)){
                    //OperationMode=MANUALMODE;
                    //pc.printf("go to manualmade\r\n");
                }
                break;
            /*
            case MANUALMODE:
                update_mode = UPD_MANUAL;
                led1 = 1;
                led2 = 1; 
                
                if(!CheckSW_up(7)&&!CheckSW_up(8)){
                    OperationMode=AUTOLOOP;
                    pc.printf("go to autoloop\r\n");
                }
                break;
            */
        }
                
            
             
                
     /*
    if(CheckSW_up(7)){
        pc.printf("true\r\n");
        Auto_loop();
        
    }else{
        pc.printf("false\r\n");
        update_manual();
    }*/
    }
}