File content as of revision 47:b3d78207b08f:

//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 15.0  //2.0
#define KI_ELE 0.0
#define KD_ELE 0.0  //0/0
#define KP_RUD 3.0
#define KI_RUD 0.0
#define KD_RUD 0.0
#define KP_AIL 0.1
#define KI_AIL 0.2
#define KD_AIL 0.2

//#define g_AIL_L_Ratio_rightloop 0.5

#define GAIN_CONTROLVALUE_TO_PWM 3.0

#define RIGHT_ROLL -12.0
#define RIGHT_PITCH -10.0  //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 g_rightloopRUD 1500 

#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 RIGHTLOOPROLL_APPROACH -17.0
#define LEFTLOOPROLL_APPROACH 16.0
#define RIGHTLOOPPITCH_APPROACH -15.0
#define LEFTLOOPPITCH_APPROACH -13.0

#define rightloopROLL2 -10.0

/*#define rightloopRUD 1300  //1250
#define rightloopshortRUD 1250 
#define leftloopRUD  1500
#define leftloopshortRUD 1500 
#define glideloopRUD 1300
*/
#define AIL_R_correctionrightloop 0
#define AIL_L_correctionrightloop 0
#define AIL_L_correctionrightloopshort 0
#define AIL_L_correctionleftloop -0
#define AIL_L_correctionleftloopshort 0


#define RIGHTLOOP_RUD 1250 
#define RIGHTLOOPSHORT_RUD 1250 
#define LEFTLOOP_RUD 1500 
#define LEFTLOOPSHORT_RUD 1500
#define GLIDELOOP_RUD 1300
#define RIGHTLOOPRUD_APPROACH 1500
#define LEFTLOOPRUD_APPROACH  1500
  
#define AIL_L_CORRECTION_RIGHTLOOP 0
#define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
#define AIL_L_CORRECTION_LEFTLOOP 0
#define AIL_L_CORRECTION_LEFTLOOPSHORT 0

#define GLIDE_ROLL -12.0
#define GLIDE_PITCH -3.0


#define AIL_L_RatioRising 0.5
#define AIL_L_RatioDescent 2

//コンパスキャリブレーション
//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;


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_5); 
DigitalOut led4(PB_4);

//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] = {1455,1450,1176,1628,1417,1452};

int main(void){
   for(int i=1400;i<1600;i++){
        servo1.pulsewidth_us(i);
        pc.printf("%d\r\n",i);
        wait(3);
        
        }
    }