aaa

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Files at this revision

API Documentation at this revision

Comitter:
TUATBM
Date:
Tue Aug 28 07:12:16 2018 +0000
Parent:
0:92024886c0be
Commit message:
aaa

Changed in this revision

HCSR04.lib Show annotated file Show diff for this revision Revisions of this file
MPU6050.lib Show annotated file Show diff for this revision Revisions of this file
MPU9250/MPU9250.cpp Show annotated file Show diff for this revision Revisions of this file
MPU9250/MPU9250.h Show annotated file Show diff for this revision Revisions of this file
Rawserial.bld Show annotated file Show diff for this revision Revisions of this file
SDFileSystem.lib Show annotated file Show diff for this revision Revisions of this file
config/Estrela.h Show diff for this revision Revisions of this file
config/falfalla.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 92024886c0be -r f31e17341659 HCSR04.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Tue Aug 28 07:12:16 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/TUATBM/code/HCSR04_2/#f077c49e84f6
diff -r 92024886c0be -r f31e17341659 MPU6050.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.lib	Tue Aug 28 07:12:16 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/1519026547/code/MPU6050_2/#e1e51291024e
diff -r 92024886c0be -r f31e17341659 MPU9250/MPU9250.cpp
--- a/MPU9250/MPU9250.cpp	Tue Aug 01 12:27:13 2017 +0000
+++ b/MPU9250/MPU9250.cpp	Tue Aug 28 07:12:16 2018 +0000
@@ -3,7 +3,7 @@
 #include "MPU9250.h"
 
 
-MPU9250::MPU9250(PinName sda, PinName scl, Serial* serial_p)
+MPU9250::MPU9250(PinName sda, PinName scl, RawSerial* serial_p)
     :
     i2c_p(new I2C(sda,scl)),
     i2c(*i2c_p),
@@ -90,6 +90,21 @@
     degree[2] = yaw;
 }
 
+
+float MPU9250::calculateYawByMg(){
+    transformCoordinateFromCompassToMPU();
+    lpmag[0] = LPGAIN_MAG *lpmag[0] + (1 - LPGAIN_MAG)*mx;
+    lpmag[1] = LPGAIN_MAG *lpmag[1] + (1 - LPGAIN_MAG)*my;
+    lpmag[2] = LPGAIN_MAG *lpmag[2] + (1 - LPGAIN_MAG)*mz;
+    
+    float radroll = PI/180.0f * roll;
+    float radpitch = PI/180.0f * pitch;
+
+    return 180.0f/PI * atan2(lpmag[2]*sin(radpitch) - lpmag[1]*cos(radpitch),
+                                         lpmag[0]*cos(radroll) - lpmag[1]*sin(radroll)*sin(radpitch) + lpmag[2]*sin(radroll)*cos(radpitch));
+}
+
+
 // Accelerometer and gyroscope self test; check calibration wrt factory settings
 void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
 {
@@ -295,6 +310,8 @@
         magbias[i] = 0;
 
         eInt[i] = 0.0f; 
+    
+        lpmag[i] = 0.0f;
     }
 
     q[0] = 1.0f;
@@ -874,4 +891,11 @@
   yaw   *= 180.0f / PI; 
   yaw   -= DECLINATION;
   roll  *= 180.0f / PI; 
+}
+
+void MPU9250::transformCoordinateFromCompassToMPU(){
+    float buf = mx;
+    mx = my;
+    my = buf;
+    mz = -mz;
 }
\ No newline at end of file
diff -r 92024886c0be -r f31e17341659 MPU9250/MPU9250.h
--- a/MPU9250/MPU9250.h	Tue Aug 01 12:27:13 2017 +0000
+++ b/MPU9250/MPU9250.h	Tue Aug 28 07:12:16 2018 +0000
@@ -27,6 +27,8 @@
 #define DECLINATION -7.45f //Declination at Tokyo is -7.45 degree 2017/06/14
 
 const float PI = 3.14159265358979323846f;
+const float LPGAIN_MAG = 0.0;
+
 
 enum Ascale {
   AFS_2G = 0,
@@ -53,13 +55,13 @@
  
 public:
     //MPU9250();
-    MPU9250(PinName sda, PinName scl, Serial* serial_p);
+    MPU9250(PinName sda, PinName scl, RawSerial* serial_p);
     ~MPU9250();
 
     bool Initialize(void);
     bool sensingAcGyMg(void);
     void calculatePostureAngle(float degree[3]);
-
+    float calculateYawByMg(void);
     
     void MPU9250SelfTest(float * destination);
 
@@ -82,7 +84,7 @@
     I2C *i2c_p;
     I2C &i2c;
 
-    Serial* pc_p;
+    RawSerial* pc_p;
 
     Timer timer;
 
@@ -114,6 +116,7 @@
     float q[4];           // vector to hold quaternion
     float eInt[3];              // vector to hold integral error for Mahony method
 
+    float lpmag[3];
 
   void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
   char readByte(uint8_t address, uint8_t subAddress);
@@ -148,6 +151,7 @@
     void translateQuaternionToDeg(float quaternion[4]);
     void calibrateDegree(void);
 
+    void transformCoordinateFromCompassToMPU();
 protected:
 
 };
diff -r 92024886c0be -r f31e17341659 Rawserial.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Rawserial.bld	Tue Aug 28 07:12:16 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/fd96258d940d
\ No newline at end of file
diff -r 92024886c0be -r f31e17341659 SDFileSystem.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Tue Aug 28 07:12:16 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/1519026547/code/SDFileSystem3/#85fbe134acff
diff -r 92024886c0be -r f31e17341659 config/Estrela.h
--- a/config/Estrela.h	Tue Aug 01 12:27:13 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,111 +0,0 @@
-#ifndef ESTRELA_H_
-#define ESTRELA_H_
-
-#include "mbed.h"
-
-//===========OUT channel======================
-/*
-Servo servo_AIL1(SERVO8);
-Servo servo_ELE(SERVO7);
-Servo servo_THR(SERVO6);
-Servo servo_RUD(SERVO5);
-Servo servo_O(SERVO4);
-Servo servo_AIL2(SERVO3);
-*/
-//===========IN channel======================
-
-//===========machine setting=================
-
-/*<<<<<<<<<< Tips >>>>>>>>>
-サーボ入出力は0~1000(us) これはPWMのパルス幅である
-だけど機体によってトリムがあるので、サーボ初期位置は必ず500とは限らない
-あたりまえだけど、プロポのほうの設定変更(舵角やミキシング量等)はここには反映されない
-反映したけりゃ自分でこのファイルを書き直すこと
-
-AIL エルロン
-ELE エレベーター
-THR スロットル
-RUD ラダー
-SW1 (7ch)
-SW2 (8CH)
-
-*/
-//1ch   AIL1
-//2ch   ELE
-//3ch   THR
-//4ch   RUD
-//6cH   AIL2
-
-
-/*自動操縦スイッチのしきい値 ch7のトリムやEPAを変えたらかならずチェックすること*/
-#define SWITCH_CHECK 1400    //下1052 上1972
-/*各舵のニュートラル値*/
-#define RUD_N 1573           //  L   1790   N   1332    R   1032    width   822
-#define THR_N 1508           //  low 1184   N   1507    hi  1832    width   813
-#define ELE_N 1461           //  up  1192   N   1412    dow 1690    width   739
-/*各舵の最大値*/
-#define RUD_MAX 1836
-#define ELE_MAX 1628
-/*各舵の最小値*/
-#define RUD_MIN 1262
-#define ELE_MIN 1304
-#define THR_MIN 1416
-/*各舵のリバース設定 1かー1のみ 直接掛け算するので*/
-#define THROTTLE_REV 1
-#define ELEVATOR_REV 1 //1
-#define RUDDER_REV 1   //1
-/*各舵の比例ゲイン値*/
-#define THROTTLE_GN 0.2
-#define ELEVATOR_GN 1   //0.9
-#define RUDDER_GN 1  //0.6
-/*各舵の微分ゲイン*/
-#define ELE_DGN 0.5
-#define RUD_DGN 0.5
-/*旋回時のラダー中心位置*/
-#define RUDDER_TURN 164
-/*旋回時のロール角度*/
-#define SENKAI_D 10
-/*通常迎角*/
-#define GEIKAKU 31
-/*機体設置状態ピッチ方向の傾き*/
-#define LAND_ANGLE 5
-/*旋回 積分量設定*/
-#define SENKAIDDD 10000000 //旋回時間 傾きの積分値と比較
-/*落下機構の動作位置*/
-#define OTEDAMA1 710
-#define OTEDAMA2 500
-#define OTEDAMA3 250
-
-#define CHECKSW1_t 0.1
-
-#define MANUAL_t 0.5
-#define SCALE_t 0.05
-#define GHPF 512
-#define ALPF 32
-#define ALPF_ 528
-
-#define MANUAL_MS 25
-#define CHECKMODE_MS 0
-#define SENSING_MS 0
-#define AUTO_MS 25
-//#define G_MS 50
-//#define A_MS 50
-#define MOBIUS_T 144
-#define MOBIUS_NN 12
-#define MOBIUS_LO 60
-
-//=====================================================
-
-//===============global===============
-int Auto_RUD=RUD_N;
-int Auto_ELE=ELE_N;
-int Auto_THR=THR_N;
-
-
-#endif /* ESTRELA_H_ */
-
-/*
-x軸回り    ロール
-y軸回り    ピッチ
-z軸回り    ヨー
-*/
\ No newline at end of file
diff -r 92024886c0be -r f31e17341659 config/falfalla.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config/falfalla.h	Tue Aug 28 07:12:16 2018 +0000
@@ -0,0 +1,55 @@
+#ifndef FALFALLA_H
+#define FALFALLA_H
+
+#define SWITCH_CHECK 1400
+#define NUMBER_FALFALLA 2 //何号機かを入れるとそれに応じて#if以下が変わって設定が変わる
+
+static float g_kpELE=2.0;
+static float g_kiELE=0.0;
+static float g_kdELE=0.0;
+static float g_kpRUD=3.0;
+static float g_kiRUD=0.0;
+static float g_kdRUD=0.0;
+
+static float g_rightloopROLL=-17.0;
+static float g_rightloopPITCH=-6.3;
+static float g_leftloopROLL=16.0;
+static float g_leftloopPITCH=-6.0;
+static float g_gostraightROLL=2.0;
+static float g_gostraightPITCH=-3.0;
+static float g_takeoffTHR=1.0;
+static float g_loopTHR=0.58;
+
+static float g_rightloopROLLshort=-20.0;
+static float g_rightloopPITCHshort=-7.0;
+static float g_leftloopROLLshort=22.0;
+static float g_leftloopPITCHshort=-6.0;
+
+static float g_glideloopROLL = -16.0;
+static float g_glideloopPITCH = 0.0;
+
+#if NUMBER_FALFALLA == 1    //1号機
+
+const int16_t Trim_Falfalla[4] = {0,14,-100,-10};
+const float ExpMax_Falfalla[4] = {100,115,100,89};
+const float ExpMin_Falfalla[4] = {100,47,100,110};
+const int8_t Reverce_falfalla[4] = {1,-1,1,1};   //Nutral:1 , Reverce:-1
+
+#elif NUMBER_FALFALLA == 2  //2号機
+
+const int16_t Trim_Falfalla[4] = {0,12,-100,2};
+const float ExpMax_Falfalla[4] = {100,115,100,103};
+const float ExpMin_Falfalla[4] = {100,97,100,97};
+const int16_t Reverce_falfalla[4] = {1,-1,1,-1};   //Nutral:1 , Reverce:-1
+
+
+#endif
+
+
+#endif /* ESTRELA_H_ */
+
+/*
+x軸回り    ロール
+y軸回り    ピッチ
+z軸回り    ヨー
+*/
\ No newline at end of file
diff -r 92024886c0be -r f31e17341659 main.cpp
--- a/main.cpp	Tue Aug 01 12:27:13 2017 +0000
+++ b/main.cpp	Tue Aug 28 07:12:16 2018 +0000
@@ -1,85 +1,144 @@
+//mbed
 #include "mbed.h"
-//#include "rtos.h"
-
+#include "FATFileSystem.h"
+#include "SDFileSystem.h"
+//C
 #include "math.h"
-#include "MPU9250.h"
-#include "BMP280.h"
+//sensor
+#include "MPU6050_DMP6.h"
+//#include "MPU9250.h"
+//#include "BMP280.h"
+#include "hcsr04.h"
+//device
+#include "sbus.h"
+//config
 #include "SkipperSv2.h"
-#include "Estrela.h"
-#include "sbus.h"
+#include "falfalla.h"
+//other
 #include "pid.h"
 
-#define DEBUG_SEMIAUTO 1  
-#define DEBUG_PRINT_INLOOP 1
+#define DEBUG_SEMIAUTO 0
+#define DEBUG_PRINT_INLOOP
+
+#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 GAIN_CONTROLVALUE_TO_PWM 3.0
+
+#define RIGHT_ROLL -17.0
+#define RIGHT_PITCH -6.3
+#define LEFT_ROLL 16.0
+#define LEFT_PITCH -6.0
+#define STRAIGHT_ROLL 2.0
+#define STRAIGHT_PITCH -3.0
+#define TAKEOFF_THR 1.0
+#define LOOP_THR 0.58
 
-#define KP_ELE 2
-#define KI_ELE 0
-#define KD_ELE 0
-#define KP_RUD 2
-#define KI_RUD 0
-#define KD_RUD 0
+#define RIGHT_ROLL_SHORT -20.0
+#define RIGHT_PITCH_SHORT -7.0
+#define LEFT_ROLL_SHORT 22.0
+#define LEFT_PITCH_SHORT -6.0
+
+#define GLIDE_ROLL 16.0
+#define GLIDE_PITCH 0.0
 
-#define GAIN_CONTROLVALUE_TO_PWM 3
-
+//コンパスキャリブレーション
+//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
+*/
 
-const uint16_t changeModeCount = 50; 
-const int16_t lengthdivpwm = 320; 
+#define ELEMENT 1
+#define LIMIT_STRAIGHT_YAW 5.0 
+#define LIMIT_LOOPSTOP_YAW 1.0
+#define THRESHOLD_TURNINGRADIUS_YAW 60.0 
+#define ALLOWHEIGHT 15
 
-const int16_t trim[4] = {0,14,-100,-10};
-const float expMax[4] = {100,115,100,89};
-const float expMin[4] = {100,47,100,110};
+#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
+//PwmOut servo1(PC_6); // TIM3_CH1  //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
-PwmOut servo7(PB_8); // TIM4_CH3    //PB_8
-PwmOut servo8(PB_9); // TIM4_CH4
+//PwmOut servo6(PB_7); // TIM4_CH2  //trigger
+//PwmOut servo7(PB_8); // TIM4_CH3    //PB_8
+//PwmOut servo8(PB_9); // TIM4_CH4
 
-Serial pc(PA_2, PA_3);
+RawSerial pc(PA_2,PA_3, 115200);    //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
+SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
 
-DigitalOut led1(PA_0);  //緑
-DigitalOut led2(PA_1);  //赤
+DigitalOut led1(PA_0);  //黄色のコネクタ
+DigitalOut led2(PA_1);
 DigitalOut led3(PB_4); 
 DigitalOut led4(PB_5);
 
-InterruptIn switch2(PC_14);
-
-MPU9250 mpu9250(PC_9,PA_8,&pc);
+//InterruptIn switch2(PC_14);
+MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン,シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
+HCSR04 usensor(PB_7,PC_6); //trig,echo  9,8 
 
-PID pid_ELE(KP_ELE,KI_ELE,KD_ELE);
-PID pid_RUD(KP_RUD,KI_RUD,KD_RUD);
+PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
+PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
 
-enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8};
-enum Angle{ROLL, PITCH, YAW};
-enum OperationMode{StartUp, SemiAuto};
+enum Channel{AIL, ELE, THR, RUD, DROP, Ch6, 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;
-static int16_t autopwm[8] = {1500,1500,1000,1500};
-
-static int16_t trimpwm[4] = {1500,1500,1000,1500};
-int16_t maxpwm[4] = {1820,1820,1820,1820};
-int16_t minpwm[4] = {1180,1180,1180,1180};
+BombingMode bombing_mode = Takeoff;
+static int16_t autopwm[8] = {1500,1500,1180,1500,1446,1500};
+static int16_t trimpwm[6] = {1500,1500,1180,1500,1446,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};
+const float trimAngle[3] = {0.0, 0.0, 0.0};
 const float maxAngle[2] = {90, 90};
 const float minAngle[2] = {-90, -90};
 
-Timer t;
+float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
 
-//IWDG_HandleTypeDef hiwdg;
+unsigned int g_distance;
+Ticker USsensor;
+static char g_buf[16];
+char g_landingcommand;
+float g_SerialTargetYAW = 0.0;
+
+Timer t;
+Timeout RerurnChickenServo1;
+Timeout RerurnChickenServo2;
 
 /*-----関数のプロトタイプ宣言-----*/
 void setup();
@@ -89,9 +148,15 @@
 void Init_servo();              //サーボ初期化
 void Init_sbus();               //SBUS初期化
 void Init_sensors();
-void DisplayClock();              //クロック状態確認
+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]);
@@ -99,14 +164,56 @@
 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[8]);    //pwmをサーボへ出力
+void Output_PWM(int16_t pwm[6]);    //pwmをサーボへ出力
+
+//シリアル割り込み
+void SendSerial();   //1文字きたら送り返す
+void SendArray();
+void getSF_Serial();
+float ConvertByteintoFloat(char high, char low);
 
+//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]);
+
+char Rotate(float targetAngle[3], float TargetYAW, char mode);
+
+//投下
+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();
 
@@ -114,59 +221,102 @@
 
 int main()
 {
-    setup();   
+    setup();
+    
     while(1){
+        
         loop();
+        
+        if(!CheckSW_Up(Ch7)){
+            led3=0;
+        }else{
+            led3=1;
+        }
     }
 }
 
 void setup(){
+    //buzzer = 0;
     led1 = 1;
     led2 = 1;
-    led3 = 0;
-    led4 = 0;
+    led3 = 1;
+    led4 = 1;
     
-    pc.baud(115200);    
+    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);
+    //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();
-    DisplayClock();
-
+    
+    
+    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");
-    
-    mpu9250.sensingAcGyMg();
-    mpu9250.calculatePostureAngle(nowAngle);
 }
 
 void loop(){
-    static float targetAngle[3], controlValue[2];
+    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);
-
-#if DEBUG_PRINT_INLOOP
+    wait_ms(23);
+//#if DEBUG_PRINT_INLOOP
     DebugPrint();
-#endif
+//#endif
 }
 
-
-
 //サーボ初期化関数
-void Init_servo()
-{
+void Init_servo(){
     
-    servo1.period_ms(14);
-    servo1.pulsewidth_us(trimpwm[AIL]);
+    //servo1.period_ms(14);
+    //servo1.pulsewidth_us(trimpwm[AIL]);
     
     servo2.period_ms(14);
     servo2.pulsewidth_us(trimpwm[ELE]);
@@ -178,21 +328,12 @@
     servo4.pulsewidth_us(trimpwm[RUD]);
     
     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);
+    servo5.pulsewidth_us(1392);
 
     pc.printf("servo initialized\r\n");
 }
 
-//Sbusを初期化する関数
+//Sbus初期化
 void Init_sbus(){
     sbus.initialize();
     sbus.setLastfuncPoint(Update_PWM);
@@ -200,16 +341,20 @@
 }
 
 void Init_sensors(){
-    if(!mpu9250.Initialize()){
+    if(mpu6050.setup() == -1){
         pc.printf("failed initialize\r\n");
-        while(1);
+        while(1){
+            led1 = 1; led2 = 0; led3 = 1; led4 = 0;
+            wait(1);
+            led1 = 0; led2 = 1; led3 = 0; led4 = 1;
+            wait(1);
+        }
     }
-    mpu9250.setMagBias(MAGBIAS_X,MAGBIAS_Y,MAGBIAS_Z);
 }
 
 void Init_PWM(){
     for (uint8_t i = 0; i < 4; ++i){
-        trimpwm[i] = 1500 + trim[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));
     }
@@ -225,12 +370,11 @@
 }
 
 void UpdateTargetAngle(float targetAngle[3]){
-    static uint16_t count_op = 0, count_out = 0;
-
+    static int16_t count_op = 0, count_out = 0;
 #if DEBUG_SEMIAUTO    
     switch(operation_mode){
         case StartUp:
-            if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){
+            if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
                 count_op++;
                 if(count_op > changeModeCount){
                     operation_mode = SemiAuto;
@@ -261,16 +405,308 @@
             break;
     }
 
-    if(CheckSW_Up(Ch5)){
-        output_status = Auto;
-        led1 = 1;
-    }else{
-        output_status = Manual;
-        led1 = 0;
+#else
+
+    switch(operation_mode){
+        case StartUp:
+            if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){        //ch7;自動・手動切り替え ch8;自動操縦モード切替
+                count_op++;
+                if(count_op > changeModeCount){
+                    operation_mode = LeftLoop;
+                    pc.printf("Goto LeftLoop mode\r\n");
+                    count_op = 0;
+                }
+            }else count_op = 0;
+            break;
+
+        case LeftLoop:
+            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;
+            UpdateTargetAngle_Leftloop(targetAngle);
+            break;
+        
+        case RightLoop:
+            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_Rightloop(targetAngle);
+            
+            //Rotate確認用
+            /*
+            static char mode = 'G';
+            mode = Rotate(targetAngle,g_SerialTargetYAW,mode); 
+            */
+            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 = GoStraight;
+                    pc.printf("Goto GoStraight mode\r\n");
+                    count_op = 0;
+                }
+            }else count_op = 0;
+            Take_off_and_landing(targetAngle);
+            break;
+        
+        case GoStraight:
+            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_GoStraight(targetAngle);
+            break;
+
+        
+        default:
+        
+            operation_mode = StartUp;
+            break;
     }
 #endif
+
+    if(CheckSW_Up(Ch7) && output_status == Manual){
+        count_out++;
+        if(count_out > changeModeCount){
+            output_status = Auto;
+            //led3 = 1;
+            count_out = 0;
+        }
+    }else if(!CheckSW_Up(Ch7) && output_status == Auto){
+        count_out++;
+        if(count_out > changeModeCount){
+            output_status = Manual;
+            count_out = 0;
+            //led3 = 0;
+        }
+    }else count_out = 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;
@@ -286,37 +722,17 @@
 
 void UpdateAutoPWM(float controlValue[3]){    
     int16_t addpwm[2]; //-500~500
-    addpwm[ROLL] = GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ:右回転正     レバー:右回転正
-    addpwm[PITCH] = -1 * GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH];    //センサ:機首下げ正    レバー:機首上げ正
-
-    autopwm[ELE] = trimpwm[ELE] + addpwm[PITCH];
-    autopwm[RUD] = trimpwm[RUD] + addpwm[ROLL];
-    autopwm[THR] = oldTHR;
+    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[RUD] = trimpwm[RUD] + reverce[RUD] * addpwm[ROLL];
+    //autopwm[THR] = oldTHR;
 
     autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
     autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]);
 }
 
-void ConvertPWMintoRAD(float targetAngle[3]){
-    float ratio[2];
-
-    if(sbus.manualpwm[ELE] > trimpwm[ELE]){    
-        ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)maxpwm[ELE]);
-        targetAngle[PITCH] = ratio[0]*(maxAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];
-    }else{
-        ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)minpwm[ELE]);
-        targetAngle[PITCH] = ratio[0]*(minAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];    
-    }
-
-    if(sbus.manualpwm[RUD] > trimpwm[RUD]){
-        ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)maxpwm[RUD]);        
-        targetAngle[ROLL] = -1*ratio[1]*(maxAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];
-    }else{
-        ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)minpwm[RUD]);        
-        targetAngle[ROLL] = -1*ratio[1]*(minAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];    
-    }
-}
-
 inline float CalcRatio(float value, float trim, float limit){
     return  (value - trim) / (limit - trim);
 }
@@ -335,7 +751,9 @@
     return value;
 }
 
-
+inline int16_t SetTHRinRatio(float ratio){
+    return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
+}
 
 /*---SBUS割り込み処理---*/
 
@@ -343,98 +761,567 @@
 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
 void Update_PWM()
 {    
-    static int16_t pwm[8];
+    static int16_t pwm[5], sbuspwm[5];
+    static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]};
+    static int16_t tempsbuspwm[5] = {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]};
+    static int counter_abnormalpwm=0;
     if(sbus.flg_ch_update == true){
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
-            case Manual: 
-                for(uint8_t i=0;i<8;i++){
-                    pwm[i] = sbus.manualpwm[i];
+            case Manual:
+                for(uint8_t i=1;i<4;i++){
+                    if(abs(sbus.manualpwm[i]-tempsbuspwm[i])>300){
+                        counter_abnormalpwm++;
+                        if(counter_abnormalpwm<=1) sbuspwm[i]=tempsbuspwm[i];
+                        else {
+                            counter_abnormalpwm = 0;
+                            sbuspwm[i] = sbus.manualpwm[i];
+                        }
+                    }
+                    else{
+                        counter_abnormalpwm = 0;
+                        sbuspwm[i] = sbus.manualpwm[i];
+                    }
+                    tempsbuspwm[i]=sbus.manualpwm[i];
                 }
-                oldTHR = sbus.manualpwm[THR];
+                sbuspwm[4] = sbus.manualpwm[4];
+                
+                for(uint8_t i=1;i<5;i++){
+                    pwm[i] = sbuspwm[i];
+                }
+                oldTHR = sbuspwm[THR];
+                                
                 //pc.printf("update_manual\r\n");
                 break;
         
             case Auto:
-                pwm[AIL] = sbus.manualpwm[AIL];
                 pwm[ELE] = autopwm[ELE];
                 pwm[THR] = autopwm[THR];
                 pwm[RUD] = autopwm[RUD];
-                pwm[Ch5] = sbus.manualpwm[Ch5];
-                pwm[Ch6] = sbus.manualpwm[Ch6];
-                pwm[Ch7] = sbus.manualpwm[Ch7];
-                pwm[Ch8] = sbus.manualpwm[Ch8];
- 
+                pwm[DROP] = autopwm[DROP];
                 //pc.printf("update_auto\r\n");
                 break;
                 
             default:
-                for(uint8_t i=0;i<8;i++){
+                for(uint8_t i=1;i<5;i++){
                     pwm[i] = sbus.manualpwm[i];
                 } //pc.printf("update_manual\r\n");
                 break;
         }
+        for(uint8_t i=1;i<5;i++) if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
+    
+        if(pwm[4]>1600 || pwm[4]<1420) pwm[4]=temppwm[4];
+        temppwm[4] = pwm[4];
+        
     }else{
         pc.printf("0\r\n");
     }
-    sbus.flg_ch_update = false;   
-    //for(uint8_t i=0; i<8; i++){pc.printf("%d, ",pwm[i]);}
-    //pc.printf("\r\n");
-    Output_PWM(pwm);  
+    sbus.flg_ch_update = false;
+    
+    Output_PWM(pwm);    
 }
 
 
-//pwmをサーボに出力する関数。
-void Output_PWM(int16_t pwm[8])
+//pwmをサーボに出力。
+void Output_PWM(int16_t pwm[5])
 {
-    servo1.pulsewidth_us(pwm[0]);
+    //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]);    
 }
 
 void ResetTrim(){
-    for(uint8_t i=0; i<4; i++){
+    for(uint8_t i=1; i<4; i++){
         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);
-    if(!mpu9250.sensingAcGyMg()){
-         //pc.printf("failed\r\n");
-         return;
-    }
+    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);
-    mpu9250.calculatePostureAngle(nowAngle);
-    /*
-    deltaT = t.read_us() - t_start;
-    pc.printf("t:%d us, ",deltaT);
-    mpu9250.displayAngle();
-    */
+    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_landingcommand=='C')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 && SFbuf[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 uint8_t precounter=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.0f && newYaw_Moebius < 180.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;}
+    }
+    if(RotateCounter == 1 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;}
+    
+    }
+    if(RotateCounter == 2 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;}
+    }
+    if(RotateCounter == 3 && newYaw_Moebius >10.0f && newYaw_Moebius < 90.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;}
+    }
+    if(RotateCounter == 4 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;}
+    }
+    if(RotateCounter == 5 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;}
+    }
+    if(RotateCounter == 6 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;}
+    }
+    if(RotateCounter == 7 && newYaw_Moebius >10.0f && newYaw_Moebius < 180.0f){
+        precounter++;
+        if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;}
+    }
+    
+    
+    if(RotateCounter < 6) 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;
+    
+    //時間計測開始設定
+    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;
     
-    //mpu9250.sensingPostureAngle(nowAngle);
+    switch(bombing_mode){
+        case Takeoff:
+            static bool flg_setFirstYaw = false;
+            static int TakeoffCount = 0;
+
+            if(!flg_setFirstYaw && CheckSW_Up(Ch7)){
+                FirstYAW = nowAngle[YAW] + FirstYAW;
+                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){
+                led2=1;
+                autopwm[THR] = SetTHRinRatio(g_loopTHR);
+                pc.printf("Now go to Approach mode!!");
+                bombing_mode = Approach;
+                led2=0;
+            }
+            
+            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]){
+    static int tELE_start = 0;
+    static bool flg_ELEup = false;
+    int t_def = 0;
+    if(!flg_ELEup && CheckSW_Up(Ch7)){
+        tELE_start = t.read_ms();
+        flg_ELEup = true;
+        pc.printf("timer start\r\n");
+    }else if(!CheckSW_Up(Ch7)){
+        tELE_start = 0;
+        flg_ELEup = false;
+    }
+    if(flg_ELEup){
+        t_def = t.read_ms() - tELE_start;
+        
+        //1.5秒経過すればELE上げ舵へ
+        if(t_def>500) targetAngle[PITCH]=-25.0;
+        else{
+            t_def = 0;
+            targetAngle[PITCH]=g_gostraightPITCH;
+        }
+    }
+    autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
+    targetAngle[ROLL] = 3.5;
+/*
+    //pc.printf("%d \r\n",g_distance);
+    targetAngle[ROLL] = g_gostraightROLL;
+    if UptargetAngle[PITCH] = -10.0;
+    autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
+*/
+}
+
+//ヨーを目標値にして許容角度になるまで水平旋回
+char Rotate(float targetAngle[3], float TargetYAW, char mode){
+    float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
+
+    switch(mode){
+        case 'G': //直進
+            UpdateTargetAngle_GoStraight(targetAngle);
+            if(diffYaw > LIMIT_STRAIGHT_YAW)        mode = 'R';
+            else if(diffYaw < -LIMIT_STRAIGHT_YAW)  mode = 'L';
+            break;
+            
+        case 'R': //右旋回
+            UpdateTargetAngle_Rightloop_short(targetAngle);
+            if(diffYaw < LIMIT_LOOPSTOP_YAW && diffYaw > -LIMIT_STRAIGHT_YAW)   mode = 'G';
+            break;
+            
+        case 'L': //左旋回
+            UpdateTargetAngle_Leftloop_short(targetAngle);
+            if(diffYaw > -LIMIT_LOOPSTOP_YAW && diffYaw < LIMIT_STRAIGHT_YAW)   mode = 'G';
+            break;
+    }
+    
+    return mode;
+    
+/*
+    if(diffYaw > LIMIT_STRAIGHT_YAW){
+        UpdateTargetAngle_Rightloop_short(targetAngle);
+        return 1;
+    }else if(diffYaw < -LIMIT_STRAIGHT_YAW){
+        UpdateTargetAngle_Leftloop_short(targetAngle);
+        return 1;
+    }else{
+        UpdateTargetAngle_GoStraight(targetAngle);
+        return 0;
+    }
+*/
+}
+
+//チキラー投下
+void Chicken_Drop(){
+    if(CheckSW_Up(Ch7)){
+        autopwm[DROP] = 1572;
+        pc.printf("Bombed!\r\n");
+        RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
+        //operation_mode = Approach;
+        //buzzer = 0;
+    }
+}
+
+void ReturnChickenServo1(){
+    autopwm[DROP] = 1438;
+    pc.printf("first reverse\r\n");
+    RerurnChickenServo2.attach(&ReturnChickenServo2, 1);
+}
+
+void ReturnChickenServo2(){
+    autopwm[DROP] = 1446;
+    pc.printf("second reverse\r\n");
+}
+
+//着陸モード(PCからの指令に従う)
+void UpdateTargetAngle_Approach(float targetAngle[3]){
+    static char rotatemode = 'G';
+    if(output_status == Manual) rotatemode = 'G';    
+
+    //pc.putc(g_landingcommand);
+    switch(g_landingcommand){
+        case 'R':   //右旋回セヨ
+            UpdateTargetAngle_Rightloop(targetAngle);
+            break;
+            
+        case 'L':   //左旋回セヨ
+            UpdateTargetAngle_Leftloop(targetAngle);
+            break;
+            
+        case 'G':   //直進セヨ
+            UpdateTargetAngle_GoStraight(targetAngle);
+            break;
+        
+        case 'Y':   //指定ノヨー方向ニ移動セヨ
+            rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode);
+            break;
+        
+        case 'B':   //ブザーヲ鳴ラセ
+            //buzzer = 1;
+            break;
+            
+        case 'D':   //物資ヲ落トセ
+            Chicken_Drop();
+            break;
+                    
+        case 'C':   //停止セヨ
+            rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode);
+            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.05f);
+    //pc.printf("nose UP");
+}
+
+void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){
+    
+    //targetAngle[PITCH] -= 2.0f;
+    autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05f);
+    //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 DebugPrint(){    
-
-    //static int16_t deltaT = 0, t_start = 0;
-    //t_start = t.read_us();
-    for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
+    /*
+    static int16_t deltaT = 0, t_start = 0;
+    deltaT = t.read_u2s() - t_start;
+    pc.printf("t:%d us, ",deltaT);
     pc.printf("\r\n");
-    //deltaT = t.read_us() - t_start;
-    //pc.printf("t:%d us, ",deltaT);
+    t_start = t.read_us();
+    */
+    
+    //pc.printf("%d", sbus.manualpwm[4]);
+    
+    //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
+    //for(uint8_t i=1; i<5; 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);
 
-    //mpu9250.displayAngle();
-    //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]);
+    //pc.printf("Mode: %c: ",g_buf[0]);
+    //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
+    //pc.printf("%x ",sbus.failsafe_status);
+    
     //pc.printf("\r\n");
-}
-
+}
\ No newline at end of file
diff -r 92024886c0be -r f31e17341659 mbed.bld
--- a/mbed.bld	Tue Aug 01 12:27:13 2017 +0000
+++ b/mbed.bld	Tue Aug 28 07:12:16 2018 +0000
@@ -1,1 +1,1 @@
-https://mbed.org/users/mbed_official/code/mbed/builds/0f02307a0877
\ No newline at end of file
+https://mbed.org/users/mbed_official/code/mbed/builds/e2bfab296f20
\ No newline at end of file