File content as of revision 1:f383708a5a52:

//mbed
#include "mbed.h"
#include "FATFileSystem.h"
#include "SDFileSystem.h"
//C
#include "math.h"
//sensor
#include "MPU6050_DMP6.h"
//#include "MPU9250.h"
//#include "BMP280.h"
#include "hcsr04.h"
//device
#include "sbus.h"
//config
#include "SkipperSv2.h"
#include "falfalla.h"
//other
#include "pid.h"

#define DEBUG_SEMIAUTO 0
#define DEBUG_PRINT_INLOOP 1

#define KP_ELE 2.0
#define KI_ELE 0.0
#define KD_ELE 0.0
#define KP_RUD 3.0
#define KI_RUD 0.0
#define KD_RUD 0.0
#define KP_AIL 3.0
#define KI_AIL 0.0
#define KD_AIL 0.0




#define GAIN_CONTROLVALUE_TO_PWM 3.0

#define RIGHT_ROLL -12.0
#define RIGHT_PITCH -5.0
#define LEFT_ROLL 12.0
#define LEFT_PITCH -5.0
#define STRAIGHT_ROLL 4.0
#define STRAIGHT_PITCH 3.0
#define TAKEOFF_THR 0.8
#define LOOP_THR 0.6

#define RIGHT_ROLL_SHORT -12.0
#define RIGHT_PITCH_SHORT -5.0
#define LEFT_ROLL_SHORT 12.0
#define LEFT_PITCH_SHORT -5.0

#define GLIDE_ROLL -12.0
#define GLIDE_PITCH -3.0

//コンパスキャリブレーション
//SkipperS2基板
/*
#define MAGBIAS_X -35.0
#define MAGBIAS_Y 535.0
#define MAGBIAS_Z -50.0
*/
//S2v2 1番基板
#define MAGBIAS_X 395.0
#define MAGBIAS_Y 505.0
#define MAGBIAS_Z -725.0
//S2v2 2番基板
/*
#define MAGBIAS_X 185.0
#define MAGBIAS_Y 220.0
#define MAGBIAS_Z -350.0
*/

#define ELEMENT 1
#define LIMIT_STRAIGHT_YAW 5.0 
#define THRESHOLD_TURNINGRADIUS_YAW 60.0 
#define ALLOWHEIGHT 15

#ifndef PI
#define PI 3.14159265358979
#endif

const int16_t lengthdivpwm = 320; 
const int16_t changeModeCount = 6;

const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]};
const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]};
const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]};
const int16_t reverce[4] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};

SBUS sbus(PA_9, PA_10); //SBUS

PwmOut servo1(PC_6); // TIM3_CH1  //old echo
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  //old trigger
//PwmOut servo7(PB_8); // TIM4_CH3    //PB_8 new echo
//PwmOut servo8(PB_9); // TIM4_CH4   //new trigger

RawSerial pc(PA_2,PA_3, 115200);    //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
//RawSerial pc2(PB_6,PB_7, 115200);   //sbus確認用
SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");

DigitalOut led1(PA_0);  //黄色のコネクタ
DigitalOut led2(PA_1);
DigitalOut led3(PB_4); 
DigitalOut led4(PB_5);

//InterruptIn switch2(PC_14);
MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8 

PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);

enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8};
enum Angle{ROLL, PITCH, YAW};   //yaw:北を0とした絶対角度
enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
enum BombingMode{Takeoff, Chicken, Transition, Approach};
enum OutputStatus{Manual, Auto};

static OutputStatus output_status = Manual;
OperationMode operation_mode = StartUp;
BombingMode bombing_mode = Takeoff;
static int16_t autopwm[8] = {1500,1500,1180,1500,1392,1500};
static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1500};
int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820};
int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
int16_t oldTHR = 1000;

static float nowAngle[3] = {0,0,0};
const float trimAngle[3] = {0.0, 0.0, 0.0};
const float maxAngle[2] = {90, 90};
const float minAngle[2] = {-90, -90};

float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;

unsigned int g_distance;
Ticker USsensor;
static char g_buf[16];
char g_landingcommand;
float g_SerialTargetYAW;

Timer t;
Timeout RerurnChickenServo1;
Timeout RerurnChickenServo2;

/*-----関数のプロトタイプ宣言-----*/
void setup();
void loop();

void Init_PWM();
void Init_servo();              //サーボ初期化
void Init_sbus();               //SBUS初期化
void Init_sensors();
void DisplayClock();            //クロック状態確認

//センサの値取得
void SensingMPU();
void UpdateDist();

//void offsetRollPitch(float FirstROLL, float FirstPITCH);
//void TransYaw(float FirstYAW);
float TranslateNewYaw(float beforeYaw,  float newzeroYaw);
void UpdateTargetAngle(float targetAngle[3]);
void CalculateControlValue(float targetAngle[3], float controlValue[3]);
void UpdateAutoPWM(float controlValue[3]);
void ConvertPWMintoRAD(float targetAngle[3]);
inline float CalcRatio(float value, float trim, float limit);
bool CheckSW_Up(Channel ch);
int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
inline int16_t SetTHRinRatio(float ratio);

//sbus割り込み
void Update_PWM();              //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
void Output_PWM(int16_t pwm[6]);    //pwmをサーボへ出力

//シリアル割り込み
void SendSerial();   //1文字きたら送り返す
void SendArray();
void getSF_Serial();
float ConvertByteintoFloat(char high, char low);
void ISR_Serial_Rx();

//SD設定
int GetParameter(FILE *fp, const char *paramName,char parameter[]);
int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE,
               float *g_kpRUD, float *g_kiRUD, float *g_kdRUD,
               float *g_rightloopROLL, float *g_rightloopPITCH,
               float *g_leftloopROLL, float *g_leftloopPITCH,
               float *g_gostraightROLL, float *g_gostraightPITCH,
               float *g_takeoffTHR, float *g_loopTHR,
               float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
               float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
               float *g_glideloopROLL, float *g_glideloopPITCH);
//switch2割り込み
void ResetTrim();

//自動操縦
void UpdateTargetAngle_GoStraight(float targetAngle[3]);
void UpdateTargetAngle_Rightloop(float targetAngle[3]);
void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
void UpdateTargetAngle_Leftloop(float targetAngle[3]);
void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
void UpdateTargetAngle_Moebius(float targetAngle[3]);
void UpdateTargetAngle_Glide(float targetAngle[3]);
void UpdateTargetAngle_Takeoff(float targetAngle[3]);
void UpdateTargetAngle_Approach(float targetAngle[3]);
void Take_off_and_landing(float targetAngle[3]);

int Rotate(float targetAngle[3], float TargetYAW);

//投下
void Chicken_Drop();
void ReturnChickenServo1();
void ReturnChickenServo2();

//超音波による高度補正
void checkHeight(float targetAngle[3]);
void UpdateTargetAngle_NoseUP(float targetAngle[3]);
void UpdateTargetAngle_NoseDOWN(float targetAngle[3]);

//デバッグ用
void DebugPrint();

/*---関数のプロトタイプ宣言終わり---*/

int main()
{   
    setup();
    
    
    while(1){
        
        loop();
        
        if(!CheckSW_Up(Ch7)){
            led3=0;
        }else{
            led3=1;
        }
    }
    
}

void setup(){
    //buzzer = 0;
    led1 = 1;
    led2 = 1;
    led3 = 1;
    led4 = 1;
    
    SetOptions(&g_kpELE, &g_kiELE, &g_kdELE,
               &g_kpRUD, &g_kiRUD, &g_kdRUD,
               &g_rightloopROLL, &g_rightloopPITCH,
               &g_leftloopROLL, &g_leftloopPITCH,
               &g_gostraightROLL, &g_gostraightPITCH,
               &g_takeoffTHR, &g_loopTHR,
               &g_rightloopROLLshort, &g_rightloopPITCHshort,
               &g_leftloopROLLshort, &g_leftloopPITCHshort,
               &g_glideloopROLL, &g_glideloopPITCH);    
    
        
    Init_PWM();
    Init_servo();
    Init_sbus();    
    Init_sensors();
    //switch2.rise(ResetTrim);
    pc.attach(getSF_Serial, Serial::RxIrq);
    USsensor.attach(&UpdateDist, 0.05);
    
    NVIC_SetPriority(USART1_IRQn,0);
    NVIC_SetPriority(EXTI0_IRQn,1);
    NVIC_SetPriority(TIM5_IRQn,2);
    NVIC_SetPriority(EXTI9_5_IRQn,3);
    NVIC_SetPriority(USART2_IRQn,4);
    DisplayClock();
    t.start();
    
    
    pc.printf("MPU calibration start\r\n");
    
    float offsetstart = t.read();
    while(t.read() - offsetstart < 26){
        SensingMPU();
        for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
        pc.printf("\r\n");
        led1 = !led1;
        led2 = !led2;
        led3 = !led3;
        led4 = !led4;
    }
    
    FirstROLL = nowAngle[ROLL];
    FirstPITCH = nowAngle[PITCH];
    nowAngle[ROLL] -=FirstROLL;
    nowAngle[PITCH] -=FirstPITCH;
    
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    wait(0.2);
    
    
    pc.printf("All initialized\r\n");
}

void loop(){
    static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};

    SensingMPU();
    UpdateTargetAngle(targetAngle);
    //Rotate(targetAngle, 30.0);
    CalculateControlValue(targetAngle, controlValue);
    UpdateAutoPWM(controlValue);
    wait_ms(23);
#if DEBUG_PRINT_INLOOP
    DebugPrint();
#endif
}

//サーボ初期化関数
void Init_servo(){
    
    servo1.period_ms(14);
    servo1.pulsewidth_us(trimpwm[AIL_R]);
    
    servo2.period_ms(14);
    servo2.pulsewidth_us(trimpwm[ELE]);
    
    servo3.period_ms(14);
    servo3.pulsewidth_us(trimpwm[THR]);
    
    servo4.period_ms(14);
    servo4.pulsewidth_us(trimpwm[RUD]);
    
    servo5.period_ms(14);
    servo5.pulsewidth_us(1392);

    servo6.period_ms(14);
    servo6.pulsewidth_us(trimpwm[AIL_L]);

    pc.printf("servo initialized\r\n");
}

//Sbus初期化
void Init_sbus(){
    sbus.initialize();
    sbus.setLastfuncPoint(Update_PWM);
    sbus.startInterrupt();
}

void Init_sensors(){
    if(mpu6050.setup() == -1){
        pc.printf("failed initialize\r\n");
        while(1){
            led1 = 1; led2 = 0; led3 = 1; led4 = 0;
            wait(1);
            led1 = 0; led2 = 1; led3 = 0; led4 = 1;
            wait(1);
        }
    }
}

void Init_PWM(){
    for (uint8_t i = 0; i < 4; ++i){
        trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100));
        maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100));
        minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100));
    }
    pc.printf("PWM initialized\r\n");
}

void DisplayClock(){
    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 UpdateTargetAngle(float targetAngle[3]){
    static int16_t count_op = 0;
#if DEBUG_SEMIAUTO    
    switch(operation_mode){
        case StartUp:
            if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = SemiAuto;
                    pc.printf("Goto SemiAuto mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            break;

        case SemiAuto:
        /*  大会用では以下のif文を入れてoperation_modeを変える
            if(CheckSW_Up(Ch6)){
                count_op++;
                if(count_op>changeModeCount){
                    output_status = XXX;
                    led2 = 0;
                    pc.printf("Goto XXX mode\r\n");
                    count_op = 0;
                }else count_op = 0;
                ConvertPWMintoRAD(targetAngle);
            }
        */
            ConvertPWMintoRAD(targetAngle);
            break;

        default:
            operation_mode = SemiAuto;
            break;
    }

#else

    switch(operation_mode){
        case StartUp:
            if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){        //ch7;自動・手動切り替え　ch8;自動操縦モード切替
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = RightLoop;
                    pc.printf("Goto RightLoop mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            break;

        case RightLoop:
            if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = LeftLoop;
                    pc.printf("Goto LeftLoop mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            UpdateTargetAngle_Rightloop(targetAngle);
            
            break;

        case LeftLoop:
            if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = GoStraight;
                    pc.printf("Goto GoStraight mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            UpdateTargetAngle_Leftloop(targetAngle);
            break;
        
        case GoStraight:
            if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = Moebius;
                    pc.printf("Goto Moebius mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            UpdateTargetAngle_GoStraight(targetAngle);
            break;

        case Moebius:
            if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = Glide;
                    pc.printf("Goto Glide mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            UpdateTargetAngle_Moebius(targetAngle);
            break;

        case Glide:
            if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = BombwithPC;
                    pc.printf("Goto Bombing mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            UpdateTargetAngle_Glide(targetAngle);
            break;

        case BombwithPC:
            if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
                count_op++;
                if(count_op > changeModeCount){
                    operation_mode = RightLoop;
                    pc.printf("Goto RightLoop mode\r\n");
                    count_op = 0;
                }
            }else count_op = 0;
            Take_off_and_landing(targetAngle);
            break;

        default:
            operation_mode = StartUp;
            break;
    }
#endif

    if(CheckSW_Up(Ch7)){
        output_status = Auto;
        led1 = 1;
    }else{
        output_status = Manual;
        led1 = 0;
    }
}

int GetParameter(FILE *fp, const char *paramName,char parameter[]){
    int i=0, j=0;
    int strmax = 200;
    char str[strmax];

    rewind(fp); //ファイル位置を先頭に
    while(1){
        if (fgets(str, strmax, fp) == NULL) {
            return 0;
        }
        if (!strncmp(str, paramName, strlen(paramName))) {
            while (str[i++] != '=') {}
            while (str[i] != '\n') {
                    parameter[j++] = str[i++];
            }
            parameter[j] = '\0';
            return 1;
        }
    }
    return 0;
}


//sdによる設定
int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE,
               float *g_kpRUD, float *g_kiRUD, float *g_kdRUD,
               float *g_rightloopROLL, float *g_rightloopPITCH,
               float *g_leftloopROLL, float *g_leftloopPITCH,
               float *g_gostraightROLL, float *g_gostraightPITCH,
               float *g_takeoffTHR, float *g_loopTHR,
               float *g_rightloopROLLshort, float *g_rightloopPITCHshort,
               float *g_leftloopROLLshort, float *g_leftloopPITCHshort,
               float *g_glideloopROLL, float *g_glideloopPITCH
               ){

    pc.printf("SDsetup start.\r\n");    
    
    FILE *fp;
    char parameter[30]; //文字列渡す用の配列
    int SDerrorcount = 0;  //取得できなかった数を返す
    const char *paramNames[] = { 
        "KP_ELEVATOR",
        "KI_ELEVATOR",
        "KD_ELEVATOR",
        "KP_RUDDER",
        "KI_RUDDER",
        "KD_RUDDER",
        "RIGHTLOOP_ROLL",
        "RIGHTLOOP_PITCH",
        "LEFTLOOP_ROLL",
        "LEFTLOOP_PITCH",
        "GOSTRAIGHT_ROLL",
        "GOSTRAIGHT_PITCH",
        "TAKEOFF_THR_RATE",
        "LOOP_THR_RATE",
        "RIGHTLOOP_ROLL_SHORT",
        "RIGHTLOOP_PITCH_SHORT",
        "LEFTLOOP_ROLL_SHORT",
        "LEFTLOOP_PITCH_SHORT",
        "AUTOGLIDE_ROLL",
        "AUTOGLIDE PITCH"        
    };

    fp = fopen("/sd/option.txt","r");
    
    if(fp != NULL){ //開けたら
        pc.printf("File was openned.\r\n");
        if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter);
        else{                                        *g_kpELE = KP_ELE;
                                                     SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter);
        else{                                        *g_kiELE = KI_ELE;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter);
        else{                                        *g_kdELE = KD_ELE;
                                                     SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter);
        else{                                        *g_kpRUD = KP_RUD;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter);
        else{                                        *g_kiRUD = KI_RUD;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter);
        else{                                        *g_kdRUD = KD_RUD;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter);
        else{                                        *g_rightloopROLL = RIGHT_ROLL;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter);
        else{                                        *g_rightloopPITCH = RIGHT_PITCH;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter);
        else{                                        *g_leftloopROLL = LEFT_ROLL;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter);
        else{                                         *g_leftloopPITCH = LEFT_PITCH;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter);
        else{                                         *g_gostraightROLL = STRAIGHT_ROLL;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter);
        else{                                         *g_gostraightPITCH = STRAIGHT_PITCH;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter);
        else{                                         *g_takeoffTHR = TAKEOFF_THR;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter);
        else{                                         *g_loopTHR = LOOP_THR;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter);
        else{                                         *g_rightloopROLLshort = RIGHT_ROLL_SHORT;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter);
        else{                                         *g_rightloopPITCHshort = RIGHT_PITCH_SHORT;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter);
        else{                                         *g_leftloopROLLshort = LEFT_ROLL_SHORT;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter);
        else{                                         *g_leftloopPITCHshort = LEFT_PITCH_SHORT;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter);
        else{                                         *g_glideloopROLL = GLIDE_ROLL;
                                                      SDerrorcount++;
        }
        if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter);
        else{                                         *g_glideloopPITCH = GLIDE_PITCH;
                                                      SDerrorcount++;
        }
        fclose(fp);

    }else{  //ファイルがなかったら
        pc.printf("fp was null.\r\n");
        *g_kpELE = KP_ELE;
        *g_kiELE = KI_ELE;
        *g_kdELE = KD_ELE;
        *g_kpRUD = KP_RUD;
        *g_kiRUD = KI_RUD;
        *g_kdRUD = KD_RUD;
        *g_rightloopROLL = RIGHT_ROLL;
        *g_rightloopPITCH = RIGHT_PITCH;
        *g_leftloopROLL = LEFT_ROLL;
        *g_leftloopPITCH = LEFT_PITCH;
        *g_gostraightROLL = STRAIGHT_ROLL;
        *g_gostraightPITCH = STRAIGHT_PITCH;
        *g_takeoffTHR = TAKEOFF_THR;
        *g_loopTHR = LOOP_THR;
        SDerrorcount = -1;
    }
    pc.printf("SDsetup finished.\r\n");
    if(SDerrorcount == 0)  pc.printf("setting option is success\r\n");
    else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
    else if(SDerrorcount > 0)  pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); 
    
    pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD);
    pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE);
    pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH);
    pc.printf("leftloopROLL = %f, g_leftloopPITCH =  %f\r\n", *g_leftloopROLL, *g_leftloopPITCH);
    pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH);
    pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR);
    pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort);
    pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort =  %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort);
    pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH);
    
    return SDerrorcount;
}                  

void CalculateControlValue(float targetAngle[3], float controlValue[3]){
    static int t_last;
    int t_now;
    float dt;

    t_now = t.read_us();
    dt = (float)((t_now - t_last)/1000000.0f) ;
    t_last = t_now;


    //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
    controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);       //エルロンでロール制御   
    controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
}

void UpdateAutoPWM(float controlValue[3]){    
    int16_t addpwm[2]; //-500~500
    addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH];    //センサ：機首下げ正    レバー:機首上げ正
    addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ：右回転正（8月13日時点；左回転が正！）     レバー:右回転正
    
    //autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];
    autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[AIL_R];
    //autopwm[THR] = oldTHR;

    autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
    autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
}

inline float CalcRatio(float value, float trim, float limit){
    return  (value - trim) / (limit - trim);
}

bool CheckSW_Up(Channel ch){
    if(SWITCH_CHECK < sbus.manualpwm[ch]){
        return true;
    }else{
        return false;
    }
}

int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){
    if(value > max) return max;
    if(value < min) return min;
    return value;
}

inline int16_t SetTHRinRatio(float ratio){
    return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
}

/*---SBUS割り込み処理---*/

//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
void Update_PWM()
{    
    static int16_t pwm[6];
    static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
    if(sbus.flg_ch_update == true){
        switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
            case Manual: 
                for(uint8_t i=0;i<6;i++){
                    pwm[i] = sbus.manualpwm[i];  
                }
                pc.printf("%d ,",pwm[0]);//R
                pc.printf("%d ,\r\n",pwm[5]);//L
                oldTHR = sbus.manualpwm[THR];
                //pc.printf("update_manual\r\n");
                break;
        
            case Auto:
                pwm[AIL_R] = autopwm[AIL_R];               //sbus.manualpwm[AIL];
                pwm[ELE] = autopwm[ELE];
                pwm[THR] = autopwm[THR];
                pwm[RUD] = autopwm[RUD];
                pwm[DROP] = autopwm[DROP];
                pwm[AIL_L] = autopwm[AIL_L];
                //pc.printf("update_auto\r\n");
                break;
                
            default:
                for(uint8_t i=0;i<6;i++){
                    pwm[i] = sbus.manualpwm[i];
                } //pc.printf("update_manual\r\n");
                break;
        }
        for(uint8_t i=0;i<6;i++){
        if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
        temppwm[i]=pwm[i];
        }
    }else{
        pc.printf("0\r\n");
    }
    sbus.flg_ch_update = false;   
    Output_PWM(pwm);  
}


//pwmをサーボに出力。
void Output_PWM(int16_t pwm[5])
{
    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]);

}

void ResetTrim(){
    for(uint8_t i=0; i<6; i++){            //i=4から書き換え＿投下サーボは入ってない模様
        trimpwm[i] = sbus.manualpwm[i];
    }
    pc.printf("reset PWM trim\r\n");
}


void SensingMPU(){
    //static int16_t deltaT = 0, t_start = 0;
    //t_start = t.read_us();
    
    float rpy[3] = {0}, oldrpy[3] = {0};
    static uint16_t count_changeRPY = 0;
    static bool flg_checkoutlier = false;
    NVIC_DisableIRQ(USART1_IRQn);
    NVIC_DisableIRQ(USART2_IRQn);
    NVIC_DisableIRQ(TIM5_IRQn);
    NVIC_DisableIRQ(EXTI0_IRQn);
    NVIC_DisableIRQ(EXTI9_5_IRQn);

    mpu6050.getRollPitchYaw_Skipper(rpy);
 
    NVIC_EnableIRQ(USART1_IRQn);
    NVIC_EnableIRQ(USART2_IRQn);
    NVIC_EnableIRQ(TIM5_IRQn);
    NVIC_EnableIRQ(EXTI0_IRQn);
    NVIC_EnableIRQ(EXTI9_5_IRQn);
    
    
    //外れ値対策
    for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
    rpy[ROLL] -= FirstROLL;
    rpy[PITCH] -= FirstPITCH;
    rpy[YAW] -= FirstYAW;
    
    for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
    if(!flg_checkoutlier || count_changeRPY >= 2){
        for(uint8_t i=0; i<3; i++){
            nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
        }
        count_changeRPY = 0;
    }else   count_changeRPY++;
    flg_checkoutlier = false;
    
}

float TranslateNewYaw(float beforeYaw,  float newzeroYaw){
    float newYaw = beforeYaw - newzeroYaw;
    
    if(newYaw<-180.0f) newYaw += 360.0f;
    else if(newYaw>180.0f) newYaw -= 360.0f;
    return newYaw;
}


void getSF_Serial(){
    
        static char SFbuf[16];
        static int bufcounter=0;
        
        SFbuf[bufcounter]=pc.getc();
        if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
            
        if(bufcounter==5 && SFbuf[4]=='F'){
            g_landingcommand = SFbuf[1];
            if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
            bufcounter = 0;
            memset(SFbuf, 0, strlen(SFbuf));
            pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
        }
            else if(bufcounter>=5 && g_buf[4]!='F'){
                pc.printf("Communication Falsed.\r\n");
                bufcounter = 0;
            }
    }
    
float ConvertByteintoFloat(char high, char low){

        //int16_t intvalue = (int16_t)high*256 + (int16_t)low;
        int16_t intvalue = (int16_t)(((int16_t)high << 8) | low);  // Turn the MSB and LSB into a signed 16-bit value
        float floatvalue = (float)intvalue;
        return floatvalue;
}


//超音波割り込み
void UpdateDist(){
    g_distance = usensor.get_dist_cm();
    usensor.start();
}

//8の字旋回
void UpdateTargetAngle_Moebius(float targetAngle[3]){
    static uint8_t RotateCounter=0;
    static bool flg_setInStartAuto = false;
    static float FirstYAW_Moebius = 0.0;
    float newYaw_Moebius;

    if(!flg_setInStartAuto && CheckSW_Up(Ch7)){
        FirstYAW_Moebius = nowAngle[YAW];
        RotateCounter = 0;
        flg_setInStartAuto = true;
    }else if(!CheckSW_Up(Ch7)){
        flg_setInStartAuto = false;
        led2 = 0;
    }
    
    newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);

    if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0)    {RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n");}
    if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0)  {RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n");}
    if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius < 10.0f)   {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");}
    if(RotateCounter == 3 && newYaw_Moebius >20.0 && newYaw_Moebius < 180.0f)     {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");}


    if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle);    
    else UpdateTargetAngle_Leftloop_short(targetAngle);   //左旋回

}

//自動滑空
void UpdateTargetAngle_Glide(float targetAngle[3]){
    static int THRcount = 0;
    static int t_start = 0;
    static bool flg_tstart = false;
    int t_diff = 0;
    static int groundcount = 0;
    
    targetAngle[ROLL] = g_glideloopROLL;
    targetAngle[PITCH] = g_glideloopPITCH;
    
    autopwm[THR]=oldTHR;
    //シリアル通信受信の割り込みイベント登録
    pc.attach(ISR_Serial_Rx, Serial::RxIrq);
    
/*    
    //時間計測開始設定
    if(!flg_tstart && CheckSW_Up(Ch7)){
        t_start = t.read();
        flg_tstart = true;
        pc.printf("timer start\r\n");
    }else if(!CheckSW_Up(Ch7)){
        t_start = 0;
        flg_tstart = false;
    }


    //フラグが偽であれば計測は行わない    
    if(flg_tstart){
        t_diff = t.read() - t_start;
        //一定高度or15秒でled点灯
        if((groundcount>5 && g_distance>0) || t_diff > 15){
            led2 = 1;
            //pc.printf("Call [Stop!] calling!\r\n");
        }
        if(g_distance<180 && g_distance > 0) groundcount++;
    }else{
        t_diff = 0;
        groundcount = 0;
        led2 = 0;
    }
    
    if(t_diff > 17){
        autopwm[THR] = SetTHRinRatio(0.5);
    }else{
        if(g_distance<150 && g_distance>0 ){
            THRcount++;
            if(THRcount>5){
                autopwm[THR] = SetTHRinRatio(0.6);
                //pc.printf("throttle ON\r\n");
            }
        }else{
            autopwm[THR] = 1180;
            THRcount = 0;
        }
    }*/
}
//離陸-投下-着陸一連
void Take_off_and_landing(float targetAngle[3]){
    if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
    
    switch(bombing_mode){
        case Takeoff:
            static bool flg_setFirstYaw = false;
            static int TakeoffCount = 0;

            if(!flg_setFirstYaw && CheckSW_Up(Ch7)){
                FirstYAW = nowAngle[YAW];
                flg_setFirstYaw = true;
            }else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){
                flg_setFirstYaw = false;
            }
            
            UpdateTargetAngle_Takeoff(targetAngle);
            
            if(g_distance>150) TakeoffCount++;
            else TakeoffCount = 0;
            if(TakeoffCount>5){
                autopwm[THR] = 1180+320*2*0.5;
                pc.printf("Now go to Approach mode!!");
                bombing_mode = Approach;
            }
            break;
        /*    
        case Chicken:    
            break;
        */    
        case Transition:
            static int ApproachCount = 0;
            targetAngle[YAW]=180.0;
            int Judge = Rotate(targetAngle, g_SerialTargetYAW);
            
            if(Judge==0) ApproachCount++;
            if(ApproachCount>5) bombing_mode = Approach;
            break;
            
        case Approach:
            UpdateTargetAngle_Approach(targetAngle);
            break;
            
        default:
            bombing_mode = Takeoff;
            break;    
    }
}

//離陸モード
void UpdateTargetAngle_Takeoff(float targetAngle[3]){
    //pc.printf("%d \r\n",g_distance);
    targetAngle[ROLL] = g_gostraightROLL;
    targetAngle[PITCH] = g_loopTHR;
    autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
}

//ヨーを目標値にして許容角度になるまで水平旋回
int Rotate(float targetAngle[3], float TargetYAW){
    float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);

    if(diffYaw > LIMIT_STRAIGHT_YAW){
        /*
        if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
        else UpdateTargetAngle_Rightloop(targetAngle);
        */
        UpdateTargetAngle_Rightloop_short(targetAngle);
        return 1;
    }else if(diffYaw < -LIMIT_STRAIGHT_YAW){
        UpdateTargetAngle_Leftloop_short(targetAngle);
        /*
        if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
        else UpdateTargetAngle_Leftloop(targetAngle);
        */
        return 1;
    }else{
        UpdateTargetAngle_GoStraight(targetAngle);
        return 0;
    }
}

//チキラー投下
void Chicken_Drop(){
    if(CheckSW_Up(Ch7)){
        autopwm[DROP] = 1512;
        pc.printf("Bombed!\r\n");
        RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
        //operation_mode = Approach;
        //buzzer = 0;
        pc.printf("Goto LeftLoop mode\r\n");
    }
}

void ReturnChickenServo1(){
    autopwm[DROP] = 1344;
    pc.printf("first reverse\r\n");
    RerurnChickenServo2.attach(&ReturnChickenServo2, 1);
}

void ReturnChickenServo2(){
    autopwm[DROP] = 1392;
    pc.printf("second reverse\r\n");
}

//着陸モード（PCからの指令に従う）
void UpdateTargetAngle_Approach(float targetAngle[3]){
    pc.putc(g_buf[0]);
    switch(g_buf[0]){
        case 'R':   //右旋回セヨ
            UpdateTargetAngle_Rightloop(targetAngle);
            break;
            
        case 'L':   //左旋回セヨ
            UpdateTargetAngle_Leftloop(targetAngle);
            break;
            
        case 'G':   //直進セヨ
            UpdateTargetAngle_GoStraight(targetAngle);
            break;
        
        case 'Y':   //指定ノヨー方向ニ移動セヨ
            Rotate(targetAngle, g_SerialTargetYAW);
            break;
        
        case 'B':   //ブザーヲ鳴ラセ
            //buzzer = 1;
            break;
            
        case 'D':   //物資ヲ落トセ
            Chicken_Drop();
            break;
                    
        case 'C':   //停止セヨ
            targetAngle[ROLL] = 0.0;
            targetAngle[PITCH] = -3.0;
            autopwm[THR] = minpwm[THR];
            break;
    }
}

void checkHeight(float targetAngle[3]){
    
    static int targetHeight = 200; 
    
    if(g_distance < targetHeight + ALLOWHEIGHT){
        UpdateTargetAngle_NoseUP(targetAngle);
        if(CheckSW_Up(Ch7)) led2 = 1;
    }
    else if(g_distance > targetHeight - ALLOWHEIGHT){
        UpdateTargetAngle_NoseDOWN(targetAngle);
        if(CheckSW_Up(Ch7)) led2 = 1;
    }
    else led2=0;
}

void UpdateTargetAngle_NoseUP(float targetAngle[3]){
    
    //targetAngle[PITCH] += 2.0f;
    //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
    autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05);
    //pc.printf("nose UP");
}

void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){
    
    //targetAngle[PITCH] -= 2.0f;
    autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05);
    //pc.printf("nose DOWN");
}

//直進
void UpdateTargetAngle_GoStraight(float targetAngle[3]){

    targetAngle[ROLL] = g_gostraightROLL;
    targetAngle[PITCH] = g_gostraightPITCH;
    autopwm[THR] = SetTHRinRatio(g_loopTHR);
    
    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}

//右旋回
void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回

    targetAngle[ROLL] = g_rightloopROLL;
    targetAngle[PITCH] = g_rightloopPITCH ;
    autopwm[THR] = SetTHRinRatio(g_loopTHR);
    //checkHeight(targetAngle);
    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}

void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回

    targetAngle[ROLL] = g_rightloopROLLshort;
    targetAngle[PITCH] = g_rightloopPITCHshort;
    autopwm[THR] = SetTHRinRatio(g_loopTHR);
    
    
    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}

//左旋回
void UpdateTargetAngle_Leftloop(float targetAngle[3]){
    
    targetAngle[ROLL] = g_leftloopROLL;
    targetAngle[PITCH] = g_leftloopPITCH;
    autopwm[THR] = SetTHRinRatio(g_loopTHR);
    //checkHeight(targetAngle);

    //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}

void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){
    
    targetAngle[ROLL] = g_leftloopROLLshort;
    targetAngle[PITCH] = g_leftloopPITCHshort;
    autopwm[THR] = SetTHRinRatio(g_loopTHR);
    
    //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
}
void ISR_Serial_Rx()
{
    // シリアルの受信処理
    char data = pc.getc();
    
    if(data=='C'){
    autopwm[THR]=minpwm[2];
    wait(60.0);
    }
    
}

//デバッグ用
void DebugPrint(){    
    /*
    static int16_t deltaT = 0, t_start = 0;
    deltaT = t.read_u2s() - t_start;
    pc.printf("t:%d us, ",deltaT);
    pc.printf("\r\n");
    t_start = t.read_us();
    */
    //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
    //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]);
    //pc.printf("\r\n");
    for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
    //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
    //pc.printf("%d\t",autopwm[ELE]);  pc.printf("%d\t",autopwm[RUD]);
    //pc.printf("%d",g_distance);

    //pc.printf("Mode: %c: ",g_buf[0]);
    //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
    pc.printf("\r\n");
}