Autoflight2018_18 - 着陸wait版9/16

航空研究会 » Code » Autoflight2018_18

航空研究会 / Mbed 2 deprecated Autoflight2018_18

着陸wait版9/16

Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_17 by 航空研究会

main.cpp@15:43427b0241d9, 2018-09-17 (annotated)

Committer:: TUATBM
Date:: Mon Sep 17 06:40:41 2018 +0000
Revision:: 15:43427b0241d9
Parent:: 14:bbd5ff31fb67

trim,max,min pwm?????????

Who changed what in which revision?

User	Revision	Line number	New contents of line
HARUKIDELTA	0:17f575135219	1	//mbed
HARUKIDELTA	0:17f575135219	2	#include "mbed.h"
HARUKIDELTA	0:17f575135219	3	#include "FATFileSystem.h"
HARUKIDELTA	0:17f575135219	4	#include "SDFileSystem.h"
HARUKIDELTA	0:17f575135219	5	//C
HARUKIDELTA	0:17f575135219	6	#include "math.h"
HARUKIDELTA	0:17f575135219	7	//sensor
HARUKIDELTA	0:17f575135219	8	#include "MPU6050_DMP6.h"
HARUKIDELTA	0:17f575135219	9	//#include "MPU9250.h"
HARUKIDELTA	0:17f575135219	10	//#include "BMP280.h"
HARUKIDELTA	0:17f575135219	11	#include "hcsr04.h"
HARUKIDELTA	0:17f575135219	12	//device
HARUKIDELTA	0:17f575135219	13	#include "sbus.h"
HARUKIDELTA	0:17f575135219	14	//config
HARUKIDELTA	0:17f575135219	15	#include "SkipperSv2.h"
HARUKIDELTA	0:17f575135219	16	#include "falfalla.h"
HARUKIDELTA	0:17f575135219	17	//other
HARUKIDELTA	0:17f575135219	18	#include "pid.h"
HARUKIDELTA	0:17f575135219	19
HARUKIDELTA	0:17f575135219	20	#define DEBUG_SEMIAUTO 0
HARUKIDELTA	0:17f575135219	21	#define DEBUG_PRINT_INLOOP 1
HARUKIDELTA	0:17f575135219	22
TUATBM	9:f6367b7fd7be	23	#define KP_ELE 15.0 //2.0
HARUKIDELTA	0:17f575135219	24	#define KI_ELE 0.0
TUATBM	9:f6367b7fd7be	25	#define KD_ELE 0.0 //0/0
HARUKIDELTA	0:17f575135219	26	#define KP_RUD 3.0
HARUKIDELTA	0:17f575135219	27	#define KI_RUD 0.0
HARUKIDELTA	0:17f575135219	28	#define KD_RUD 0.0
HARUKIDELTA	2:23daa5fa28b4	29	#define KP_AIL 0.1
HARUKIDELTA	2:23daa5fa28b4	30	#define KI_AIL 0.2
HARUKIDELTA	2:23daa5fa28b4	31	#define KD_AIL 0.2
taknokolat	1:f383708a5a52	32
HARUKIDELTA	2:23daa5fa28b4	33	//#define g_AIL_L_Ratio_rightloop 0.5
HARUKIDELTA	0:17f575135219	34
HARUKIDELTA	0:17f575135219	35	#define GAIN_CONTROLVALUE_TO_PWM 3.0
HARUKIDELTA	0:17f575135219	36
HARUKIDELTA	0:17f575135219	37	#define RIGHT_ROLL -12.0
TUATBM	9:f6367b7fd7be	38	#define RIGHT_PITCH -10.0 //5.0
HARUKIDELTA	0:17f575135219	39	#define LEFT_ROLL 12.0
HARUKIDELTA	0:17f575135219	40	#define LEFT_PITCH -5.0
HARUKIDELTA	0:17f575135219	41	#define STRAIGHT_ROLL 4.0
HARUKIDELTA	0:17f575135219	42	#define STRAIGHT_PITCH 3.0
HARUKIDELTA	0:17f575135219	43	#define TAKEOFF_THR 0.8
HARUKIDELTA	0:17f575135219	44	#define LOOP_THR 0.6
HARUKIDELTA	0:17f575135219	45
HARUKIDELTA	13:77b13d5f4d78	46	//#define g_rightloopRUD 1500
HARUKIDELTA	2:23daa5fa28b4	47
HARUKIDELTA	0:17f575135219	48	#define RIGHT_ROLL_SHORT -12.0
HARUKIDELTA	0:17f575135219	49	#define RIGHT_PITCH_SHORT -5.0
HARUKIDELTA	0:17f575135219	50	#define LEFT_ROLL_SHORT 12.0
HARUKIDELTA	0:17f575135219	51	#define LEFT_PITCH_SHORT -5.0
HARUKIDELTA	0:17f575135219	52
TUATBM	9:f6367b7fd7be	53	#define rightloopRUD 1300 //1250
TUATBM	9:f6367b7fd7be	54	#define AIL_R_correctionrightloop 0
TUATBM	9:f6367b7fd7be	55	#define AIL_L_correctionrightloop -400
HARUKIDELTA	3:206b17251f5b	56	#define rightloopshortRUD 1250
HARUKIDELTA	3:206b17251f5b	57	#define AIL_L_correctionrightloopshort 0
HARUKIDELTA	3:206b17251f5b	58	#define leftloopRUD 1500
HARUKIDELTA	3:206b17251f5b	59	#define AIL_L_correctionleftloop -0
HARUKIDELTA	3:206b17251f5b	60	#define leftloopshortRUD 1500
HARUKIDELTA	3:206b17251f5b	61	#define AIL_L_correctionleftloopshort 0
HARUKIDELTA	3:206b17251f5b	62	#define glideloopRUD 1300
HARUKIDELTA	3:206b17251f5b	63
HARUKIDELTA	8:66bba39b95a9	64	#define RIGHTLOOP_RUD 1250
HARUKIDELTA	8:66bba39b95a9	65	#define RIGHTLOOPSHORT_RUD 1250
HARUKIDELTA	8:66bba39b95a9	66	#define LEFTLOOP_RUD 1500
HARUKIDELTA	8:66bba39b95a9	67	#define LEFTLOOPSHORT_RUD 1500
HARUKIDELTA	8:66bba39b95a9	68	#define GLIDELOOP_RUD 1300
HARUKIDELTA	8:66bba39b95a9	69	#define AIL_L_CORRECTION_RIGHTLOOP 0
HARUKIDELTA	8:66bba39b95a9	70	#define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
HARUKIDELTA	8:66bba39b95a9	71	#define AIL_L_CORRECTION_LEFTLOOP 0
HARUKIDELTA	8:66bba39b95a9	72	#define AIL_L_CORRECTION_LEFTLOOPSHORT 0
HARUKIDELTA	3:206b17251f5b	73
HARUKIDELTA	0:17f575135219	74	#define GLIDE_ROLL -12.0
HARUKIDELTA	0:17f575135219	75	#define GLIDE_PITCH -3.0
HARUKIDELTA	0:17f575135219	76
HARUKIDELTA	3:206b17251f5b	77
HARUKIDELTA	3:206b17251f5b	78	#define AIL_L_RatioRising 0.5
HARUKIDELTA	3:206b17251f5b	79	#define AIL_L_RatioDescent 2
HARUKIDELTA	3:206b17251f5b	80
HARUKIDELTA	0:17f575135219	81	//コンパスキャリブレーション
HARUKIDELTA	0:17f575135219	82	//SkipperS2基板
HARUKIDELTA	0:17f575135219	83	/*
HARUKIDELTA	0:17f575135219	84	#define MAGBIAS_X -35.0
HARUKIDELTA	0:17f575135219	85	#define MAGBIAS_Y 535.0
HARUKIDELTA	0:17f575135219	86	#define MAGBIAS_Z -50.0
HARUKIDELTA	0:17f575135219	87	*/
HARUKIDELTA	0:17f575135219	88	//S2v2 1番基板
HARUKIDELTA	0:17f575135219	89	#define MAGBIAS_X 395.0
HARUKIDELTA	0:17f575135219	90	#define MAGBIAS_Y 505.0
HARUKIDELTA	0:17f575135219	91	#define MAGBIAS_Z -725.0
HARUKIDELTA	0:17f575135219	92	//S2v2 2番基板
HARUKIDELTA	0:17f575135219	93	/*
HARUKIDELTA	0:17f575135219	94	#define MAGBIAS_X 185.0
HARUKIDELTA	0:17f575135219	95	#define MAGBIAS_Y 220.0
HARUKIDELTA	0:17f575135219	96	#define MAGBIAS_Z -350.0
HARUKIDELTA	0:17f575135219	97	*/
HARUKIDELTA	0:17f575135219	98
HARUKIDELTA	0:17f575135219	99	#define ELEMENT 1
HARUKIDELTA	0:17f575135219	100	#define LIMIT_STRAIGHT_YAW 5.0
HARUKIDELTA	0:17f575135219	101	#define THRESHOLD_TURNINGRADIUS_YAW 60.0
HARUKIDELTA	0:17f575135219	102	#define ALLOWHEIGHT 15
HARUKIDELTA	0:17f575135219	103
HARUKIDELTA	0:17f575135219	104	#ifndef PI
HARUKIDELTA	0:17f575135219	105	#define PI 3.14159265358979
HARUKIDELTA	0:17f575135219	106	#endif
HARUKIDELTA	0:17f575135219	107
HARUKIDELTA	0:17f575135219	108	const int16_t lengthdivpwm = 320;
HARUKIDELTA	0:17f575135219	109	const int16_t changeModeCount = 6;
HARUKIDELTA	0:17f575135219	110
HARUKIDELTA	0:17f575135219	111
HARUKIDELTA	0:17f575135219	112	SBUS sbus(PA_9, PA_10); //SBUS
HARUKIDELTA	0:17f575135219	113
HARUKIDELTA	0:17f575135219	114	PwmOut servo1(PC_6); // TIM3_CH1 //old echo
HARUKIDELTA	0:17f575135219	115	PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
HARUKIDELTA	0:17f575135219	116	PwmOut servo3(PB_0); // TIM3_CH3
HARUKIDELTA	0:17f575135219	117	PwmOut servo4(PB_1); // TIM3_CH4
HARUKIDELTA	0:17f575135219	118	PwmOut servo5(PB_6); // TIM4_CH1
HARUKIDELTA	0:17f575135219	119	PwmOut servo6(PB_7); // TIM4_CH2 //old trigger
HARUKIDELTA	0:17f575135219	120	//PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo
HARUKIDELTA	0:17f575135219	121	//PwmOut servo8(PB_9); // TIM4_CH4 //new trigger
HARUKIDELTA	0:17f575135219	122
HARUKIDELTA	0:17f575135219	123	RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
taknokolat	1:f383708a5a52	124	//RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用
HARUKIDELTA	0:17f575135219	125	SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
HARUKIDELTA	0:17f575135219	126
HARUKIDELTA	0:17f575135219	127	DigitalOut led1(PA_0); //黄色のコネクタ
HARUKIDELTA	0:17f575135219	128	DigitalOut led2(PA_1);
HARUKIDELTA	0:17f575135219	129	DigitalOut led3(PB_4);
HARUKIDELTA	0:17f575135219	130	DigitalOut led4(PB_5);
HARUKIDELTA	0:17f575135219	131
HARUKIDELTA	0:17f575135219	132	//InterruptIn switch2(PC_14);
HARUKIDELTA	0:17f575135219	133	MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
taknokolat	10:652071c20bf6	134	//HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8
HARUKIDELTA	0:17f575135219	135
taknokolat	1:f383708a5a52	136	PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
HARUKIDELTA	0:17f575135219	137	PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
HARUKIDELTA	0:17f575135219	138	PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
HARUKIDELTA	0:17f575135219	139
HARUKIDELTA	0:17f575135219	140	enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8};
HARUKIDELTA	0:17f575135219	141	enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
HARUKIDELTA	0:17f575135219	142	enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
HARUKIDELTA	0:17f575135219	143	enum BombingMode{Takeoff, Chicken, Transition, Approach};
HARUKIDELTA	0:17f575135219	144	enum OutputStatus{Manual, Auto};
HARUKIDELTA	0:17f575135219	145
HARUKIDELTA	0:17f575135219	146	static OutputStatus output_status = Manual;
HARUKIDELTA	0:17f575135219	147	OperationMode operation_mode = StartUp;
HARUKIDELTA	0:17f575135219	148	BombingMode bombing_mode = Takeoff;
TUATBM	15:43427b0241d9	149
HARUKIDELTA	0:17f575135219	150	static int16_t autopwm[8] = {1500,1500,1180,1500,1392,1500};
TUATBM	15:43427b0241d9	151
TUATBM	15:43427b0241d9	152	//1号機
HARUKIDELTA	7:53b0eb6f6bd3	153	static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1600};
HARUKIDELTA	0:17f575135219	154	int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820};
HARUKIDELTA	0:17f575135219	155	int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
TUATBM	15:43427b0241d9	156	const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
TUATBM	15:43427b0241d9	157
TUATBM	15:43427b0241d9	158	//2号機
TUATBM	15:43427b0241d9	159	/*
TUATBM	15:43427b0241d9	160	static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1600};
TUATBM	15:43427b0241d9	161	int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820};
TUATBM	15:43427b0241d9	162	int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
TUATBM	15:43427b0241d9	163	const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
TUATBM	15:43427b0241d9	164	*/
TUATBM	15:43427b0241d9	165
HARUKIDELTA	0:17f575135219	166	int16_t oldTHR = 1000;
HARUKIDELTA	0:17f575135219	167
HARUKIDELTA	2:23daa5fa28b4	168	int16_t g_AIL_L_Ratio_rightloop = 0.5;
HARUKIDELTA	2:23daa5fa28b4	169
HARUKIDELTA	2:23daa5fa28b4	170
HARUKIDELTA	0:17f575135219	171	static float nowAngle[3] = {0,0,0};
HARUKIDELTA	0:17f575135219	172	const float trimAngle[3] = {0.0, 0.0, 0.0};
HARUKIDELTA	0:17f575135219	173	const float maxAngle[2] = {90, 90};
HARUKIDELTA	0:17f575135219	174	const float minAngle[2] = {-90, -90};
HARUKIDELTA	0:17f575135219	175
HARUKIDELTA	0:17f575135219	176	float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
HARUKIDELTA	0:17f575135219	177
HARUKIDELTA	0:17f575135219	178	unsigned int g_distance;
taknokolat	10:652071c20bf6	179	//Ticker USsensor;
HARUKIDELTA	0:17f575135219	180	static char g_buf[16];
taknokolat	10:652071c20bf6	181	char g_landingcommand='Z';
HARUKIDELTA	0:17f575135219	182	float g_SerialTargetYAW;
HARUKIDELTA	0:17f575135219	183
HARUKIDELTA	0:17f575135219	184	Timer t;
HARUKIDELTA	0:17f575135219	185	Timeout RerurnChickenServo1;
HARUKIDELTA	0:17f575135219	186	Timeout RerurnChickenServo2;
HARUKIDELTA	0:17f575135219	187
HARUKIDELTA	0:17f575135219	188	/-----関数のプロトタイプ宣言-----/
HARUKIDELTA	0:17f575135219	189	void setup();
HARUKIDELTA	0:17f575135219	190	void loop();
HARUKIDELTA	0:17f575135219	191
HARUKIDELTA	0:17f575135219	192	void Init_PWM();
HARUKIDELTA	0:17f575135219	193	void Init_servo(); //サーボ初期化
HARUKIDELTA	0:17f575135219	194	void Init_sbus(); //SBUS初期化
HARUKIDELTA	0:17f575135219	195	void Init_sensors();
HARUKIDELTA	0:17f575135219	196	void DisplayClock(); //クロック状態確認
HARUKIDELTA	0:17f575135219	197
HARUKIDELTA	0:17f575135219	198	//センサの値取得
HARUKIDELTA	0:17f575135219	199	void SensingMPU();
HARUKIDELTA	0:17f575135219	200	void UpdateDist();
HARUKIDELTA	0:17f575135219	201
HARUKIDELTA	0:17f575135219	202	//void offsetRollPitch(float FirstROLL, float FirstPITCH);
HARUKIDELTA	0:17f575135219	203	//void TransYaw(float FirstYAW);
HARUKIDELTA	0:17f575135219	204	float TranslateNewYaw(float beforeYaw, float newzeroYaw);
HARUKIDELTA	0:17f575135219	205	void UpdateTargetAngle(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	206	void CalculateControlValue(float targetAngle[3], float controlValue[3]);
HARUKIDELTA	0:17f575135219	207	void UpdateAutoPWM(float controlValue[3]);
HARUKIDELTA	0:17f575135219	208	void ConvertPWMintoRAD(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	209	inline float CalcRatio(float value, float trim, float limit);
HARUKIDELTA	0:17f575135219	210	bool CheckSW_Up(Channel ch);
HARUKIDELTA	0:17f575135219	211	int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
HARUKIDELTA	0:17f575135219	212	inline int16_t SetTHRinRatio(float ratio);
HARUKIDELTA	0:17f575135219	213
HARUKIDELTA	0:17f575135219	214	//sbus割り込み
HARUKIDELTA	0:17f575135219	215	void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
HARUKIDELTA	0:17f575135219	216	void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力
HARUKIDELTA	0:17f575135219	217
HARUKIDELTA	0:17f575135219	218	//シリアル割り込み
HARUKIDELTA	0:17f575135219	219	void getSF_Serial();
HARUKIDELTA	0:17f575135219	220	float ConvertByteintoFloat(char high, char low);
TUATBM	15:43427b0241d9	221
HARUKIDELTA	0:17f575135219	222
HARUKIDELTA	0:17f575135219	223	//SD設定
HARUKIDELTA	0:17f575135219	224	int GetParameter(FILE fp, const char paramName,char parameter[]);
HARUKIDELTA	0:17f575135219	225	int SetOptions(float g_kpELE, float g_kiELE, float *g_kdELE,
HARUKIDELTA	0:17f575135219	226	float g_kpRUD, float g_kiRUD, float *g_kdRUD,
HARUKIDELTA	0:17f575135219	227	float g_rightloopROLL, float g_rightloopPITCH,
HARUKIDELTA	0:17f575135219	228	float g_leftloopROLL, float g_leftloopPITCH,
HARUKIDELTA	0:17f575135219	229	float g_gostraightROLL, float g_gostraightPITCH,
HARUKIDELTA	0:17f575135219	230	float g_takeoffTHR, float g_loopTHR,
HARUKIDELTA	0:17f575135219	231	float g_rightloopROLLshort, float g_rightloopPITCHshort,
HARUKIDELTA	0:17f575135219	232	float g_leftloopROLLshort, float g_leftloopPITCHshort,
HARUKIDELTA	12:763bf416ba4b	233	float g_glideloopROLL, float g_glideloopPITCH,
HARUKIDELTA	13:77b13d5f4d78	234	float g_kpAIL, float g_kiAIL, float *g_kdAIL,
HARUKIDELTA	13:77b13d5f4d78	235	int g_rightloopRUD, int g_rightloopshortRUD,
HARUKIDELTA	13:77b13d5f4d78	236	int g_leftloopRUD, int g_leftloopshortRUD,
HARUKIDELTA	13:77b13d5f4d78	237	int *g_glideRUD,
HARUKIDELTA	13:77b13d5f4d78	238	int g_AIL_L_correctionrightloop,int g_AIL_L_correctionrightloopshort,
HARUKIDELTA	13:77b13d5f4d78	239	int g_AIL_L_correctionlefttloop,int g_AIL_L_correctionleftloopshort
HARUKIDELTA	13:77b13d5f4d78	240	);
HARUKIDELTA	0:17f575135219	241	//switch2割り込み
HARUKIDELTA	0:17f575135219	242	void ResetTrim();
HARUKIDELTA	0:17f575135219	243
HARUKIDELTA	0:17f575135219	244	//自動操縦
HARUKIDELTA	0:17f575135219	245	void UpdateTargetAngle_GoStraight(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	246	void UpdateTargetAngle_Rightloop(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	247	void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	248	void UpdateTargetAngle_Leftloop(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	249	void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	250	void UpdateTargetAngle_Moebius(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	251	void UpdateTargetAngle_Glide(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	252	void UpdateTargetAngle_Takeoff(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	253	void UpdateTargetAngle_Approach(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	254	void Take_off_and_landing(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	255
HARUKIDELTA	0:17f575135219	256	int Rotate(float targetAngle[3], float TargetYAW);
HARUKIDELTA	0:17f575135219	257
HARUKIDELTA	0:17f575135219	258	//投下
HARUKIDELTA	0:17f575135219	259	void Chicken_Drop();
HARUKIDELTA	0:17f575135219	260	void ReturnChickenServo1();
HARUKIDELTA	0:17f575135219	261	void ReturnChickenServo2();
HARUKIDELTA	0:17f575135219	262
HARUKIDELTA	0:17f575135219	263	//超音波による高度補正
HARUKIDELTA	0:17f575135219	264	void checkHeight(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	265	void UpdateTargetAngle_NoseUP(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	266	void UpdateTargetAngle_NoseDOWN(float targetAngle[3]);
HARUKIDELTA	0:17f575135219	267
HARUKIDELTA	0:17f575135219	268	//デバッグ用
HARUKIDELTA	0:17f575135219	269	void DebugPrint();
HARUKIDELTA	0:17f575135219	270
HARUKIDELTA	0:17f575135219	271	/---関数のプロトタイプ宣言終わり---/
HARUKIDELTA	0:17f575135219	272
HARUKIDELTA	0:17f575135219	273	int main()
HARUKIDELTA	0:17f575135219	274	{
HARUKIDELTA	0:17f575135219	275	setup();
HARUKIDELTA	0:17f575135219	276
HARUKIDELTA	0:17f575135219	277
HARUKIDELTA	0:17f575135219	278	while(1){
HARUKIDELTA	0:17f575135219	279
HARUKIDELTA	0:17f575135219	280	loop();
HARUKIDELTA	0:17f575135219	281
taknokolat	10:652071c20bf6	282
taknokolat	10:652071c20bf6	283
HARUKIDELTA	0:17f575135219	284	if(!CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	285	led3=0;
HARUKIDELTA	0:17f575135219	286	}else{
HARUKIDELTA	0:17f575135219	287	led3=1;
HARUKIDELTA	0:17f575135219	288	}
HARUKIDELTA	0:17f575135219	289	}
HARUKIDELTA	0:17f575135219	290
HARUKIDELTA	0:17f575135219	291	}
HARUKIDELTA	0:17f575135219	292
HARUKIDELTA	0:17f575135219	293	void setup(){
HARUKIDELTA	0:17f575135219	294	//buzzer = 0;
HARUKIDELTA	0:17f575135219	295	led1 = 1;
HARUKIDELTA	0:17f575135219	296	led2 = 1;
HARUKIDELTA	0:17f575135219	297	led3 = 1;
HARUKIDELTA	0:17f575135219	298	led4 = 1;
HARUKIDELTA	0:17f575135219	299
HARUKIDELTA	0:17f575135219	300	SetOptions(&g_kpELE, &g_kiELE, &g_kdELE,
HARUKIDELTA	0:17f575135219	301	&g_kpRUD, &g_kiRUD, &g_kdRUD,
HARUKIDELTA	0:17f575135219	302	&g_rightloopROLL, &g_rightloopPITCH,
HARUKIDELTA	0:17f575135219	303	&g_leftloopROLL, &g_leftloopPITCH,
HARUKIDELTA	0:17f575135219	304	&g_gostraightROLL, &g_gostraightPITCH,
HARUKIDELTA	0:17f575135219	305	&g_takeoffTHR, &g_loopTHR,
HARUKIDELTA	0:17f575135219	306	&g_rightloopROLLshort, &g_rightloopPITCHshort,
HARUKIDELTA	0:17f575135219	307	&g_leftloopROLLshort, &g_leftloopPITCHshort,
HARUKIDELTA	12:763bf416ba4b	308	&g_glideloopROLL, &g_glideloopPITCH,
HARUKIDELTA	13:77b13d5f4d78	309	&g_kpAIL, &g_kiAIL,&g_kdAIL,
HARUKIDELTA	13:77b13d5f4d78	310	&g_rightloopRUD, &g_rightloopshortRUD,
HARUKIDELTA	13:77b13d5f4d78	311	&g_leftloopRUD, &g_leftloopshortRUD,
HARUKIDELTA	13:77b13d5f4d78	312	&g_glideloopRUD,
HARUKIDELTA	13:77b13d5f4d78	313	&g_AIL_L_correctionrightloop,&g_AIL_L_correctionrightloopshort,
HARUKIDELTA	13:77b13d5f4d78	314	&g_AIL_L_correctionleftloop,&g_AIL_L_correctionleftloopshort
HARUKIDELTA	13:77b13d5f4d78	315	);
HARUKIDELTA	0:17f575135219	316
HARUKIDELTA	0:17f575135219	317
HARUKIDELTA	0:17f575135219	318	Init_PWM();
HARUKIDELTA	0:17f575135219	319	Init_servo();
HARUKIDELTA	0:17f575135219	320	Init_sbus();
HARUKIDELTA	0:17f575135219	321	Init_sensors();
HARUKIDELTA	0:17f575135219	322	//switch2.rise(ResetTrim);
HARUKIDELTA	0:17f575135219	323	pc.attach(getSF_Serial, Serial::RxIrq);
taknokolat	10:652071c20bf6	324	//USsensor.attach(&UpdateDist, 0.05);
HARUKIDELTA	0:17f575135219	325
HARUKIDELTA	0:17f575135219	326	NVIC_SetPriority(USART1_IRQn,0);
HARUKIDELTA	0:17f575135219	327	NVIC_SetPriority(EXTI0_IRQn,1);
HARUKIDELTA	0:17f575135219	328	NVIC_SetPriority(TIM5_IRQn,2);
HARUKIDELTA	0:17f575135219	329	NVIC_SetPriority(EXTI9_5_IRQn,3);
HARUKIDELTA	0:17f575135219	330	NVIC_SetPriority(USART2_IRQn,4);
HARUKIDELTA	0:17f575135219	331	DisplayClock();
HARUKIDELTA	0:17f575135219	332	t.start();
HARUKIDELTA	0:17f575135219	333
HARUKIDELTA	0:17f575135219	334
HARUKIDELTA	0:17f575135219	335	pc.printf("MPU calibration start\r\n");
HARUKIDELTA	0:17f575135219	336
HARUKIDELTA	0:17f575135219	337	float offsetstart = t.read();
HARUKIDELTA	0:17f575135219	338	while(t.read() - offsetstart < 26){
HARUKIDELTA	0:17f575135219	339	SensingMPU();
HARUKIDELTA	0:17f575135219	340	for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
HARUKIDELTA	0:17f575135219	341	pc.printf("\r\n");
HARUKIDELTA	0:17f575135219	342	led1 = !led1;
HARUKIDELTA	0:17f575135219	343	led2 = !led2;
HARUKIDELTA	0:17f575135219	344	led3 = !led3;
HARUKIDELTA	0:17f575135219	345	led4 = !led4;
HARUKIDELTA	0:17f575135219	346	}
HARUKIDELTA	0:17f575135219	347
HARUKIDELTA	0:17f575135219	348	FirstROLL = nowAngle[ROLL];
HARUKIDELTA	0:17f575135219	349	FirstPITCH = nowAngle[PITCH];
HARUKIDELTA	0:17f575135219	350	nowAngle[ROLL] -=FirstROLL;
HARUKIDELTA	0:17f575135219	351	nowAngle[PITCH] -=FirstPITCH;
HARUKIDELTA	0:17f575135219	352
HARUKIDELTA	0:17f575135219	353	led1 = 0;
HARUKIDELTA	0:17f575135219	354	led2 = 0;
HARUKIDELTA	0:17f575135219	355	led3 = 0;
HARUKIDELTA	0:17f575135219	356	led4 = 0;
HARUKIDELTA	0:17f575135219	357	wait(0.2);
HARUKIDELTA	0:17f575135219	358
HARUKIDELTA	0:17f575135219	359
HARUKIDELTA	0:17f575135219	360	pc.printf("All initialized\r\n");
HARUKIDELTA	0:17f575135219	361	}
HARUKIDELTA	0:17f575135219	362
HARUKIDELTA	0:17f575135219	363	void loop(){
HARUKIDELTA	0:17f575135219	364	static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
HARUKIDELTA	0:17f575135219	365
HARUKIDELTA	0:17f575135219	366	SensingMPU();
taknokolat	10:652071c20bf6	367	NVIC_DisableIRQ(USART2_IRQn);
HARUKIDELTA	0:17f575135219	368	UpdateTargetAngle(targetAngle);
HARUKIDELTA	0:17f575135219	369	//Rotate(targetAngle, 30.0);
HARUKIDELTA	0:17f575135219	370	CalculateControlValue(targetAngle, controlValue);
taknokolat	10:652071c20bf6	371	NVIC_DisableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	372	UpdateAutoPWM(controlValue);
taknokolat	10:652071c20bf6	373	NVIC_EnableIRQ(USART1_IRQn);
taknokolat	10:652071c20bf6	374	NVIC_EnableIRQ(USART2_IRQn);
taknokolat	11:c0b9ad25d3db	375	wait_ms(23);
taknokolat	10:652071c20bf6	376	pc.printf("%c",g_landingcommand);
HARUKIDELTA	0:17f575135219	377	#if DEBUG_PRINT_INLOOP
HARUKIDELTA	0:17f575135219	378	DebugPrint();
HARUKIDELTA	0:17f575135219	379	#endif
HARUKIDELTA	0:17f575135219	380	}
HARUKIDELTA	0:17f575135219	381
HARUKIDELTA	0:17f575135219	382	//サーボ初期化関数
HARUKIDELTA	0:17f575135219	383	void Init_servo(){
HARUKIDELTA	0:17f575135219	384
HARUKIDELTA	0:17f575135219	385	servo1.period_ms(14);
HARUKIDELTA	0:17f575135219	386	servo1.pulsewidth_us(trimpwm[AIL_R]);
HARUKIDELTA	0:17f575135219	387
HARUKIDELTA	0:17f575135219	388	servo2.period_ms(14);
HARUKIDELTA	0:17f575135219	389	servo2.pulsewidth_us(trimpwm[ELE]);
HARUKIDELTA	0:17f575135219	390
HARUKIDELTA	0:17f575135219	391	servo3.period_ms(14);
HARUKIDELTA	0:17f575135219	392	servo3.pulsewidth_us(trimpwm[THR]);
HARUKIDELTA	0:17f575135219	393
HARUKIDELTA	0:17f575135219	394	servo4.period_ms(14);
HARUKIDELTA	0:17f575135219	395	servo4.pulsewidth_us(trimpwm[RUD]);
HARUKIDELTA	0:17f575135219	396
HARUKIDELTA	0:17f575135219	397	servo5.period_ms(14);
HARUKIDELTA	0:17f575135219	398	servo5.pulsewidth_us(1392);
HARUKIDELTA	0:17f575135219	399
HARUKIDELTA	0:17f575135219	400	servo6.period_ms(14);
HARUKIDELTA	0:17f575135219	401	servo6.pulsewidth_us(trimpwm[AIL_L]);
HARUKIDELTA	0:17f575135219	402
HARUKIDELTA	0:17f575135219	403	pc.printf("servo initialized\r\n");
HARUKIDELTA	0:17f575135219	404	}
HARUKIDELTA	0:17f575135219	405
HARUKIDELTA	0:17f575135219	406	//Sbus初期化
HARUKIDELTA	0:17f575135219	407	void Init_sbus(){
HARUKIDELTA	0:17f575135219	408	sbus.initialize();
HARUKIDELTA	0:17f575135219	409	sbus.setLastfuncPoint(Update_PWM);
HARUKIDELTA	0:17f575135219	410	sbus.startInterrupt();
HARUKIDELTA	0:17f575135219	411	}
HARUKIDELTA	0:17f575135219	412
HARUKIDELTA	0:17f575135219	413	void Init_sensors(){
HARUKIDELTA	0:17f575135219	414	if(mpu6050.setup() == -1){
HARUKIDELTA	0:17f575135219	415	pc.printf("failed initialize\r\n");
HARUKIDELTA	0:17f575135219	416	while(1){
HARUKIDELTA	0:17f575135219	417	led1 = 1; led2 = 0; led3 = 1; led4 = 0;
HARUKIDELTA	0:17f575135219	418	wait(1);
HARUKIDELTA	0:17f575135219	419	led1 = 0; led2 = 1; led3 = 0; led4 = 1;
HARUKIDELTA	0:17f575135219	420	wait(1);
HARUKIDELTA	0:17f575135219	421	}
HARUKIDELTA	0:17f575135219	422	}
HARUKIDELTA	0:17f575135219	423	}
HARUKIDELTA	0:17f575135219	424
HARUKIDELTA	0:17f575135219	425	void Init_PWM(){
HARUKIDELTA	0:17f575135219	426	pc.printf("PWM initialized\r\n");
HARUKIDELTA	0:17f575135219	427	}
HARUKIDELTA	0:17f575135219	428
HARUKIDELTA	0:17f575135219	429	void DisplayClock(){
HARUKIDELTA	0:17f575135219	430	pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
HARUKIDELTA	0:17f575135219	431	pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
HARUKIDELTA	0:17f575135219	432	pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
HARUKIDELTA	0:17f575135219	433	pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
HARUKIDELTA	0:17f575135219	434	pc.printf("\r\n");
HARUKIDELTA	0:17f575135219	435	}
HARUKIDELTA	0:17f575135219	436
HARUKIDELTA	0:17f575135219	437	void UpdateTargetAngle(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	438	static int16_t count_op = 0;
HARUKIDELTA	0:17f575135219	439	#if DEBUG_SEMIAUTO
HARUKIDELTA	0:17f575135219	440	switch(operation_mode){
HARUKIDELTA	0:17f575135219	441	case StartUp:
HARUKIDELTA	0:17f575135219	442	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	443	count_op++;
HARUKIDELTA	0:17f575135219	444	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	445	operation_mode = SemiAuto;
HARUKIDELTA	0:17f575135219	446	pc.printf("Goto SemiAuto mode\r\n");
HARUKIDELTA	0:17f575135219	447	count_op = 0;
HARUKIDELTA	0:17f575135219	448	}
HARUKIDELTA	0:17f575135219	449	}else count_op = 0;
HARUKIDELTA	0:17f575135219	450	break;
HARUKIDELTA	0:17f575135219	451
HARUKIDELTA	0:17f575135219	452	case SemiAuto:
HARUKIDELTA	0:17f575135219	453	/* 大会用では以下のif文を入れてoperation_modeを変える
HARUKIDELTA	0:17f575135219	454	if(CheckSW_Up(Ch6)){
HARUKIDELTA	0:17f575135219	455	count_op++;
HARUKIDELTA	0:17f575135219	456	if(count_op>changeModeCount){
HARUKIDELTA	0:17f575135219	457	output_status = XXX;
HARUKIDELTA	0:17f575135219	458	led2 = 0;
HARUKIDELTA	0:17f575135219	459	pc.printf("Goto XXX mode\r\n");
HARUKIDELTA	0:17f575135219	460	count_op = 0;
HARUKIDELTA	0:17f575135219	461	}else count_op = 0;
HARUKIDELTA	0:17f575135219	462	ConvertPWMintoRAD(targetAngle);
HARUKIDELTA	0:17f575135219	463	}
HARUKIDELTA	0:17f575135219	464	*/
HARUKIDELTA	0:17f575135219	465	ConvertPWMintoRAD(targetAngle);
HARUKIDELTA	0:17f575135219	466	break;
HARUKIDELTA	0:17f575135219	467
HARUKIDELTA	0:17f575135219	468	default:
HARUKIDELTA	0:17f575135219	469	operation_mode = SemiAuto;
HARUKIDELTA	0:17f575135219	470	break;
HARUKIDELTA	0:17f575135219	471	}
HARUKIDELTA	0:17f575135219	472
HARUKIDELTA	0:17f575135219	473	#else
HARUKIDELTA	0:17f575135219	474
HARUKIDELTA	0:17f575135219	475	switch(operation_mode){
HARUKIDELTA	0:17f575135219	476	case StartUp:
HARUKIDELTA	0:17f575135219	477	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え　ch8;自動操縦モード切替
HARUKIDELTA	0:17f575135219	478	count_op++;
HARUKIDELTA	0:17f575135219	479	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	480	operation_mode = RightLoop;
HARUKIDELTA	0:17f575135219	481	pc.printf("Goto RightLoop mode\r\n");
HARUKIDELTA	0:17f575135219	482	count_op = 0;
HARUKIDELTA	0:17f575135219	483	}
HARUKIDELTA	0:17f575135219	484	}else count_op = 0;
HARUKIDELTA	0:17f575135219	485	break;
HARUKIDELTA	0:17f575135219	486
HARUKIDELTA	0:17f575135219	487	case RightLoop:
HARUKIDELTA	0:17f575135219	488	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	489	count_op++;
HARUKIDELTA	0:17f575135219	490	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	491	operation_mode = LeftLoop;
HARUKIDELTA	0:17f575135219	492	pc.printf("Goto LeftLoop mode\r\n");
HARUKIDELTA	0:17f575135219	493	count_op = 0;
HARUKIDELTA	0:17f575135219	494	}
HARUKIDELTA	0:17f575135219	495	}else count_op = 0;
HARUKIDELTA	0:17f575135219	496	UpdateTargetAngle_Rightloop(targetAngle);
HARUKIDELTA	0:17f575135219	497
HARUKIDELTA	0:17f575135219	498	break;
HARUKIDELTA	0:17f575135219	499
HARUKIDELTA	0:17f575135219	500	case LeftLoop:
HARUKIDELTA	0:17f575135219	501	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	502	count_op++;
HARUKIDELTA	0:17f575135219	503	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	504	operation_mode = GoStraight;
HARUKIDELTA	0:17f575135219	505	pc.printf("Goto GoStraight mode\r\n");
HARUKIDELTA	0:17f575135219	506	count_op = 0;
HARUKIDELTA	0:17f575135219	507	}
HARUKIDELTA	0:17f575135219	508	}else count_op = 0;
HARUKIDELTA	0:17f575135219	509	UpdateTargetAngle_Leftloop(targetAngle);
HARUKIDELTA	0:17f575135219	510	break;
HARUKIDELTA	0:17f575135219	511
HARUKIDELTA	0:17f575135219	512	case GoStraight:
HARUKIDELTA	0:17f575135219	513	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	514	count_op++;
HARUKIDELTA	0:17f575135219	515	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	516	operation_mode = Moebius;
HARUKIDELTA	0:17f575135219	517	pc.printf("Goto Moebius mode\r\n");
HARUKIDELTA	0:17f575135219	518	count_op = 0;
HARUKIDELTA	0:17f575135219	519	}
HARUKIDELTA	0:17f575135219	520	}else count_op = 0;
HARUKIDELTA	0:17f575135219	521	UpdateTargetAngle_GoStraight(targetAngle);
HARUKIDELTA	0:17f575135219	522	break;
HARUKIDELTA	0:17f575135219	523
HARUKIDELTA	0:17f575135219	524	case Moebius:
HARUKIDELTA	0:17f575135219	525	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	526	count_op++;
HARUKIDELTA	0:17f575135219	527	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	528	operation_mode = Glide;
HARUKIDELTA	0:17f575135219	529	pc.printf("Goto Glide mode\r\n");
HARUKIDELTA	0:17f575135219	530	count_op = 0;
HARUKIDELTA	0:17f575135219	531	}
HARUKIDELTA	0:17f575135219	532	}else count_op = 0;
HARUKIDELTA	0:17f575135219	533	UpdateTargetAngle_Moebius(targetAngle);
HARUKIDELTA	0:17f575135219	534	break;
HARUKIDELTA	0:17f575135219	535
HARUKIDELTA	0:17f575135219	536	case Glide:
HARUKIDELTA	0:17f575135219	537	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	538	count_op++;
HARUKIDELTA	0:17f575135219	539	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	540	operation_mode = BombwithPC;
HARUKIDELTA	0:17f575135219	541	pc.printf("Goto Bombing mode\r\n");
HARUKIDELTA	0:17f575135219	542	count_op = 0;
HARUKIDELTA	0:17f575135219	543	}
HARUKIDELTA	0:17f575135219	544	}else count_op = 0;
HARUKIDELTA	0:17f575135219	545	UpdateTargetAngle_Glide(targetAngle);
HARUKIDELTA	0:17f575135219	546	break;
HARUKIDELTA	0:17f575135219	547
HARUKIDELTA	0:17f575135219	548	case BombwithPC:
HARUKIDELTA	0:17f575135219	549	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){
HARUKIDELTA	0:17f575135219	550	count_op++;
HARUKIDELTA	0:17f575135219	551	if(count_op > changeModeCount){
HARUKIDELTA	0:17f575135219	552	operation_mode = RightLoop;
HARUKIDELTA	0:17f575135219	553	pc.printf("Goto RightLoop mode\r\n");
HARUKIDELTA	0:17f575135219	554	count_op = 0;
HARUKIDELTA	0:17f575135219	555	}
HARUKIDELTA	0:17f575135219	556	}else count_op = 0;
HARUKIDELTA	0:17f575135219	557	Take_off_and_landing(targetAngle);
HARUKIDELTA	0:17f575135219	558	break;
HARUKIDELTA	0:17f575135219	559
HARUKIDELTA	0:17f575135219	560	default:
HARUKIDELTA	0:17f575135219	561	operation_mode = StartUp;
HARUKIDELTA	0:17f575135219	562	break;
HARUKIDELTA	0:17f575135219	563	}
HARUKIDELTA	0:17f575135219	564	#endif
HARUKIDELTA	0:17f575135219	565
HARUKIDELTA	0:17f575135219	566	if(CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	567	output_status = Auto;
HARUKIDELTA	0:17f575135219	568	led1 = 1;
HARUKIDELTA	0:17f575135219	569	}else{
HARUKIDELTA	0:17f575135219	570	output_status = Manual;
HARUKIDELTA	0:17f575135219	571	led1 = 0;
HARUKIDELTA	0:17f575135219	572	}
HARUKIDELTA	0:17f575135219	573	}
HARUKIDELTA	0:17f575135219	574
HARUKIDELTA	0:17f575135219	575	int GetParameter(FILE fp, const char paramName,char parameter[]){
HARUKIDELTA	0:17f575135219	576	int i=0, j=0;
HARUKIDELTA	0:17f575135219	577	int strmax = 200;
HARUKIDELTA	0:17f575135219	578	char str[strmax];
HARUKIDELTA	0:17f575135219	579
HARUKIDELTA	0:17f575135219	580	rewind(fp); //ファイル位置を先頭に
HARUKIDELTA	0:17f575135219	581	while(1){
HARUKIDELTA	0:17f575135219	582	if (fgets(str, strmax, fp) == NULL) {
HARUKIDELTA	0:17f575135219	583	return 0;
HARUKIDELTA	0:17f575135219	584	}
HARUKIDELTA	0:17f575135219	585	if (!strncmp(str, paramName, strlen(paramName))) {
HARUKIDELTA	0:17f575135219	586	while (str[i++] != '=') {}
HARUKIDELTA	0:17f575135219	587	while (str[i] != '\n') {
HARUKIDELTA	0:17f575135219	588	parameter[j++] = str[i++];
HARUKIDELTA	0:17f575135219	589	}
HARUKIDELTA	0:17f575135219	590	parameter[j] = '\0';
HARUKIDELTA	0:17f575135219	591	return 1;
HARUKIDELTA	0:17f575135219	592	}
HARUKIDELTA	0:17f575135219	593	}
HARUKIDELTA	0:17f575135219	594	return 0;
HARUKIDELTA	0:17f575135219	595	}
HARUKIDELTA	0:17f575135219	596
HARUKIDELTA	0:17f575135219	597
HARUKIDELTA	0:17f575135219	598	//sdによる設定
HARUKIDELTA	0:17f575135219	599	int SetOptions(float g_kpELE, float g_kiELE, float *g_kdELE,
HARUKIDELTA	0:17f575135219	600	float g_kpRUD, float g_kiRUD, float *g_kdRUD,
HARUKIDELTA	0:17f575135219	601	float g_rightloopROLL, float g_rightloopPITCH,
HARUKIDELTA	0:17f575135219	602	float g_leftloopROLL, float g_leftloopPITCH,
HARUKIDELTA	0:17f575135219	603	float g_gostraightROLL, float g_gostraightPITCH,
HARUKIDELTA	0:17f575135219	604	float g_takeoffTHR, float g_loopTHR,
HARUKIDELTA	0:17f575135219	605	float g_rightloopROLLshort, float g_rightloopPITCHshort,
HARUKIDELTA	0:17f575135219	606	float g_leftloopROLLshort, float g_leftloopPITCHshort,
HARUKIDELTA	12:763bf416ba4b	607	float g_glideloopROLL, float g_glideloopPITCH,
HARUKIDELTA	13:77b13d5f4d78	608	float g_kpAIL, float g_kiAIL, float *g_kdAIL,
HARUKIDELTA	13:77b13d5f4d78	609	int g_rightloopRUD, int g_rightloopshortRUD,
HARUKIDELTA	13:77b13d5f4d78	610	int g_leftloopRUD, int g_leftloopshortRUD,
HARUKIDELTA	13:77b13d5f4d78	611	int *g_glideloopRUD,
HARUKIDELTA	13:77b13d5f4d78	612	int g_AIL_L_correctionrightloop,int g_AIL_L_correctionrightloopshort,
HARUKIDELTA	13:77b13d5f4d78	613	int g_AIL_L_correctionleftloop,int g_AIL_L_correctionleftloopshort
HARUKIDELTA	0:17f575135219	614	){
HARUKIDELTA	0:17f575135219	615
HARUKIDELTA	0:17f575135219	616	pc.printf("SDsetup start.\r\n");
HARUKIDELTA	0:17f575135219	617
HARUKIDELTA	0:17f575135219	618	FILE *fp;
HARUKIDELTA	13:77b13d5f4d78	619	char parameter[40]; //文字列渡す用の配列
HARUKIDELTA	0:17f575135219	620	int SDerrorcount = 0; //取得できなかった数を返す
HARUKIDELTA	0:17f575135219	621	const char *paramNames[] = {
HARUKIDELTA	0:17f575135219	622	"KP_ELEVATOR",
HARUKIDELTA	0:17f575135219	623	"KI_ELEVATOR",
HARUKIDELTA	0:17f575135219	624	"KD_ELEVATOR",
HARUKIDELTA	0:17f575135219	625	"KP_RUDDER",
HARUKIDELTA	0:17f575135219	626	"KI_RUDDER",
HARUKIDELTA	0:17f575135219	627	"KD_RUDDER",
HARUKIDELTA	0:17f575135219	628	"RIGHTLOOP_ROLL",
HARUKIDELTA	0:17f575135219	629	"RIGHTLOOP_PITCH",
HARUKIDELTA	0:17f575135219	630	"LEFTLOOP_ROLL",
HARUKIDELTA	0:17f575135219	631	"LEFTLOOP_PITCH",
HARUKIDELTA	0:17f575135219	632	"GOSTRAIGHT_ROLL",
HARUKIDELTA	0:17f575135219	633	"GOSTRAIGHT_PITCH",
HARUKIDELTA	0:17f575135219	634	"TAKEOFF_THR_RATE",
HARUKIDELTA	0:17f575135219	635	"LOOP_THR_RATE",
HARUKIDELTA	0:17f575135219	636	"RIGHTLOOP_ROLL_SHORT",
HARUKIDELTA	0:17f575135219	637	"RIGHTLOOP_PITCH_SHORT",
HARUKIDELTA	0:17f575135219	638	"LEFTLOOP_ROLL_SHORT",
HARUKIDELTA	0:17f575135219	639	"LEFTLOOP_PITCH_SHORT",
HARUKIDELTA	0:17f575135219	640	"AUTOGLIDE_ROLL",
HARUKIDELTA	12:763bf416ba4b	641	"AUTOGLIDE PITCH",
HARUKIDELTA	12:763bf416ba4b	642	"KP_AILERON",
HARUKIDELTA	12:763bf416ba4b	643	"KI_AILERON",
HARUKIDELTA	13:77b13d5f4d78	644	"KD_AILERON",
HARUKIDELTA	13:77b13d5f4d78	645	"RIGHTLOOP_RUDDER",
HARUKIDELTA	13:77b13d5f4d78	646	"RIGHTLOOPSHORT_RUDDER",
HARUKIDELTA	13:77b13d5f4d78	647	"LEFTLOOP_RUDDER",
HARUKIDELTA	13:77b13d5f4d78	648	"LEFTLOOPSHORT_RUDDER",
HARUKIDELTA	13:77b13d5f4d78	649	"GLIDELOOP_RUDDER",
HARUKIDELTA	13:77b13d5f4d78	650	"AILERON_LEFT_CORRECTION_RIGHTLOOP",
HARUKIDELTA	13:77b13d5f4d78	651	"AILERON_LEFT_CORRECTION_RIGHTLOOPSHORT",
HARUKIDELTA	13:77b13d5f4d78	652	"AILERON_LEFT_CORRECTION_LEFTLOOP",
HARUKIDELTA	13:77b13d5f4d78	653	"AILERON_LEFT_CORRECTION_LEFTLOOPSHORT"
HARUKIDELTA	0:17f575135219	654	};
HARUKIDELTA	0:17f575135219	655
HARUKIDELTA	0:17f575135219	656	fp = fopen("/sd/option.txt","r");
HARUKIDELTA	0:17f575135219	657
HARUKIDELTA	0:17f575135219	658	if(fp != NULL){ //開けたら
HARUKIDELTA	0:17f575135219	659	pc.printf("File was openned.\r\n");
HARUKIDELTA	0:17f575135219	660	if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter);
HARUKIDELTA	0:17f575135219	661	else{ *g_kpELE = KP_ELE;
HARUKIDELTA	0:17f575135219	662	SDerrorcount++;
HARUKIDELTA	0:17f575135219	663	}
HARUKIDELTA	0:17f575135219	664	if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter);
HARUKIDELTA	0:17f575135219	665	else{ *g_kiELE = KI_ELE;
HARUKIDELTA	0:17f575135219	666	SDerrorcount++;
HARUKIDELTA	0:17f575135219	667	}
HARUKIDELTA	0:17f575135219	668	if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter);
HARUKIDELTA	0:17f575135219	669	else{ *g_kdELE = KD_ELE;
HARUKIDELTA	0:17f575135219	670	SDerrorcount++;
HARUKIDELTA	0:17f575135219	671	}
HARUKIDELTA	0:17f575135219	672	if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter);
HARUKIDELTA	0:17f575135219	673	else{ *g_kpRUD = KP_RUD;
HARUKIDELTA	0:17f575135219	674	SDerrorcount++;
HARUKIDELTA	0:17f575135219	675	}
HARUKIDELTA	0:17f575135219	676	if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter);
HARUKIDELTA	0:17f575135219	677	else{ *g_kiRUD = KI_RUD;
HARUKIDELTA	0:17f575135219	678	SDerrorcount++;
HARUKIDELTA	0:17f575135219	679	}
HARUKIDELTA	0:17f575135219	680	if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter);
HARUKIDELTA	0:17f575135219	681	else{ *g_kdRUD = KD_RUD;
HARUKIDELTA	0:17f575135219	682	SDerrorcount++;
HARUKIDELTA	0:17f575135219	683	}
HARUKIDELTA	0:17f575135219	684	if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter);
HARUKIDELTA	0:17f575135219	685	else{ *g_rightloopROLL = RIGHT_ROLL;
HARUKIDELTA	0:17f575135219	686	SDerrorcount++;
HARUKIDELTA	0:17f575135219	687	}
HARUKIDELTA	0:17f575135219	688	if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter);
HARUKIDELTA	0:17f575135219	689	else{ *g_rightloopPITCH = RIGHT_PITCH;
HARUKIDELTA	0:17f575135219	690	SDerrorcount++;
HARUKIDELTA	0:17f575135219	691	}
HARUKIDELTA	0:17f575135219	692	if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter);
HARUKIDELTA	0:17f575135219	693	else{ *g_leftloopROLL = LEFT_ROLL;
HARUKIDELTA	0:17f575135219	694	SDerrorcount++;
HARUKIDELTA	0:17f575135219	695	}
HARUKIDELTA	0:17f575135219	696	if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter);
HARUKIDELTA	0:17f575135219	697	else{ *g_leftloopPITCH = LEFT_PITCH;
HARUKIDELTA	0:17f575135219	698	SDerrorcount++;
HARUKIDELTA	0:17f575135219	699	}
HARUKIDELTA	0:17f575135219	700	if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter);
HARUKIDELTA	0:17f575135219	701	else{ *g_gostraightROLL = STRAIGHT_ROLL;
HARUKIDELTA	0:17f575135219	702	SDerrorcount++;
HARUKIDELTA	0:17f575135219	703	}
HARUKIDELTA	0:17f575135219	704	if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter);
HARUKIDELTA	0:17f575135219	705	else{ *g_gostraightPITCH = STRAIGHT_PITCH;
HARUKIDELTA	0:17f575135219	706	SDerrorcount++;
HARUKIDELTA	0:17f575135219	707	}
HARUKIDELTA	0:17f575135219	708	if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter);
HARUKIDELTA	0:17f575135219	709	else{ *g_takeoffTHR = TAKEOFF_THR;
HARUKIDELTA	0:17f575135219	710	SDerrorcount++;
HARUKIDELTA	0:17f575135219	711	}
HARUKIDELTA	0:17f575135219	712	if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter);
HARUKIDELTA	0:17f575135219	713	else{ *g_loopTHR = LOOP_THR;
HARUKIDELTA	0:17f575135219	714	SDerrorcount++;
HARUKIDELTA	0:17f575135219	715	}
HARUKIDELTA	0:17f575135219	716	if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter);
HARUKIDELTA	0:17f575135219	717	else{ *g_rightloopROLLshort = RIGHT_ROLL_SHORT;
HARUKIDELTA	0:17f575135219	718	SDerrorcount++;
HARUKIDELTA	0:17f575135219	719	}
HARUKIDELTA	0:17f575135219	720	if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter);
HARUKIDELTA	0:17f575135219	721	else{ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT;
HARUKIDELTA	0:17f575135219	722	SDerrorcount++;
HARUKIDELTA	0:17f575135219	723	}
HARUKIDELTA	0:17f575135219	724	if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter);
HARUKIDELTA	0:17f575135219	725	else{ *g_leftloopROLLshort = LEFT_ROLL_SHORT;
HARUKIDELTA	0:17f575135219	726	SDerrorcount++;
HARUKIDELTA	0:17f575135219	727	}
HARUKIDELTA	0:17f575135219	728	if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter);
HARUKIDELTA	0:17f575135219	729	else{ *g_leftloopPITCHshort = LEFT_PITCH_SHORT;
HARUKIDELTA	0:17f575135219	730	SDerrorcount++;
HARUKIDELTA	0:17f575135219	731	}
HARUKIDELTA	0:17f575135219	732	if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter);
HARUKIDELTA	0:17f575135219	733	else{ *g_glideloopROLL = GLIDE_ROLL;
HARUKIDELTA	0:17f575135219	734	SDerrorcount++;
HARUKIDELTA	0:17f575135219	735	}
HARUKIDELTA	0:17f575135219	736	if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter);
HARUKIDELTA	0:17f575135219	737	else{ *g_glideloopPITCH = GLIDE_PITCH;
HARUKIDELTA	0:17f575135219	738	SDerrorcount++;
HARUKIDELTA	0:17f575135219	739	}
HARUKIDELTA	12:763bf416ba4b	740	if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter);
HARUKIDELTA	12:763bf416ba4b	741	else{ *g_kpAIL = KP_AIL;
HARUKIDELTA	12:763bf416ba4b	742	SDerrorcount++;
HARUKIDELTA	12:763bf416ba4b	743	}
HARUKIDELTA	12:763bf416ba4b	744	if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter);
HARUKIDELTA	12:763bf416ba4b	745	else{ *g_kiAIL = KI_AIL;
HARUKIDELTA	12:763bf416ba4b	746	SDerrorcount++;
HARUKIDELTA	12:763bf416ba4b	747	}
HARUKIDELTA	12:763bf416ba4b	748	if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter);
HARUKIDELTA	12:763bf416ba4b	749	else{ *g_kdAIL = KP_AIL;
HARUKIDELTA	12:763bf416ba4b	750	SDerrorcount++;
HARUKIDELTA	12:763bf416ba4b	751	}
HARUKIDELTA	13:77b13d5f4d78	752	if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	753	else{ *g_rightloopRUD = RIGHTLOOP_RUD;
HARUKIDELTA	13:77b13d5f4d78	754	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	755	}
HARUKIDELTA	13:77b13d5f4d78	756	if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	757	else{ *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
HARUKIDELTA	13:77b13d5f4d78	758	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	759	}
HARUKIDELTA	13:77b13d5f4d78	760	if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	761	else{ *g_leftloopshortRUD = LEFTLOOP_RUD;
HARUKIDELTA	13:77b13d5f4d78	762	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	763	}
HARUKIDELTA	13:77b13d5f4d78	764	if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	765	else{ *g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
HARUKIDELTA	13:77b13d5f4d78	766	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	767	}
HARUKIDELTA	13:77b13d5f4d78	768	if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	769	else{ *g_glideloopRUD = GLIDELOOP_RUD;
HARUKIDELTA	13:77b13d5f4d78	770	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	771	}
HARUKIDELTA	13:77b13d5f4d78	772	if(GetParameter(fp,paramNames[28],parameter)) *g_AIL_L_correctionrightloop = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	773	else{ *g_AIL_L_correctionrightloop = AIL_L_CORRECTION_RIGHTLOOP;
HARUKIDELTA	13:77b13d5f4d78	774	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	775	}
HARUKIDELTA	13:77b13d5f4d78	776	if(GetParameter(fp,paramNames[29],parameter)) *g_AIL_L_correctionrightloopshort = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	777	else{ *g_AIL_L_correctionrightloopshort = AIL_L_CORRECTION_RIGHTLOOPSHORT;
HARUKIDELTA	13:77b13d5f4d78	778	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	779	}
HARUKIDELTA	13:77b13d5f4d78	780	if(GetParameter(fp,paramNames[30],parameter)) *g_AIL_L_correctionleftloop = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	781	else{ *g_AIL_L_correctionleftloop = AIL_L_CORRECTION_LEFTLOOP;
HARUKIDELTA	13:77b13d5f4d78	782	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	783	}
HARUKIDELTA	13:77b13d5f4d78	784	if(GetParameter(fp,paramNames[31],parameter)) *g_AIL_L_correctionleftloopshort = atof(parameter);
HARUKIDELTA	13:77b13d5f4d78	785	else{ *g_AIL_L_correctionleftloopshort = AIL_L_CORRECTION_LEFTLOOPSHORT;
HARUKIDELTA	13:77b13d5f4d78	786	SDerrorcount++;
HARUKIDELTA	13:77b13d5f4d78	787	}
HARUKIDELTA	13:77b13d5f4d78	788
HARUKIDELTA	0:17f575135219	789	fclose(fp);
HARUKIDELTA	0:17f575135219	790
HARUKIDELTA	0:17f575135219	791	}else{ //ファイルがなかったら
HARUKIDELTA	0:17f575135219	792	pc.printf("fp was null.\r\n");
HARUKIDELTA	0:17f575135219	793	*g_kpELE = KP_ELE;
HARUKIDELTA	0:17f575135219	794	*g_kiELE = KI_ELE;
HARUKIDELTA	0:17f575135219	795	*g_kdELE = KD_ELE;
HARUKIDELTA	0:17f575135219	796	*g_kpRUD = KP_RUD;
HARUKIDELTA	0:17f575135219	797	*g_kiRUD = KI_RUD;
HARUKIDELTA	0:17f575135219	798	*g_kdRUD = KD_RUD;
HARUKIDELTA	0:17f575135219	799	*g_rightloopROLL = RIGHT_ROLL;
HARUKIDELTA	0:17f575135219	800	*g_rightloopPITCH = RIGHT_PITCH;
HARUKIDELTA	0:17f575135219	801	*g_leftloopROLL = LEFT_ROLL;
HARUKIDELTA	0:17f575135219	802	*g_leftloopPITCH = LEFT_PITCH;
HARUKIDELTA	0:17f575135219	803	*g_gostraightROLL = STRAIGHT_ROLL;
HARUKIDELTA	0:17f575135219	804	*g_gostraightPITCH = STRAIGHT_PITCH;
HARUKIDELTA	0:17f575135219	805	*g_takeoffTHR = TAKEOFF_THR;
HARUKIDELTA	0:17f575135219	806	*g_loopTHR = LOOP_THR;
HARUKIDELTA	12:763bf416ba4b	807	*g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく！！
HARUKIDELTA	12:763bf416ba4b	808	*g_kiAIL = KI_AIL;
HARUKIDELTA	12:763bf416ba4b	809	*g_kdAIL = KD_AIL;
HARUKIDELTA	13:77b13d5f4d78	810	*g_rightloopRUD = RIGHTLOOP_RUD;
HARUKIDELTA	13:77b13d5f4d78	811	*g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
HARUKIDELTA	13:77b13d5f4d78	812	*g_leftloopRUD = LEFTLOOP_RUD;
HARUKIDELTA	13:77b13d5f4d78	813	*g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
HARUKIDELTA	13:77b13d5f4d78	814	*g_glideloopRUD = GLIDELOOP_RUD;
HARUKIDELTA	13:77b13d5f4d78	815	*g_AIL_L_correctionrightloop = AIL_L_CORRECTION_RIGHTLOOP;
HARUKIDELTA	13:77b13d5f4d78	816	*g_AIL_L_correctionrightloopshort = AIL_L_CORRECTION_RIGHTLOOPSHORT;
HARUKIDELTA	13:77b13d5f4d78	817	*g_AIL_L_correctionleftloop = AIL_L_CORRECTION_LEFTLOOP;
HARUKIDELTA	13:77b13d5f4d78	818	*g_AIL_L_correctionleftloopshort = AIL_L_CORRECTION_LEFTLOOPSHORT;
HARUKIDELTA	13:77b13d5f4d78	819
HARUKIDELTA	13:77b13d5f4d78	820
HARUKIDELTA	0:17f575135219	821	SDerrorcount = -1;
HARUKIDELTA	0:17f575135219	822	}
HARUKIDELTA	0:17f575135219	823	pc.printf("SDsetup finished.\r\n");
HARUKIDELTA	0:17f575135219	824	if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
HARUKIDELTA	0:17f575135219	825	else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
HARUKIDELTA	0:17f575135219	826	else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
HARUKIDELTA	0:17f575135219	827
HARUKIDELTA	0:17f575135219	828	pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", g_kpRUD, g_kiRUD, *g_kdRUD);
HARUKIDELTA	0:17f575135219	829	pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", g_kpELE, g_kiELE, *g_kdELE);
HARUKIDELTA	0:17f575135219	830	pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", g_rightloopROLL, g_rightloopPITCH);
HARUKIDELTA	0:17f575135219	831	pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", g_leftloopROLL, g_leftloopPITCH);
HARUKIDELTA	0:17f575135219	832	pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", g_gostraightROLL, g_gostraightPITCH);
HARUKIDELTA	0:17f575135219	833	pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", g_takeoffTHR, g_loopTHR);
HARUKIDELTA	0:17f575135219	834	pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", g_rightloopROLLshort, g_rightloopPITCHshort);
HARUKIDELTA	0:17f575135219	835	pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", g_leftloopROLLshort, g_leftloopPITCHshort);
HARUKIDELTA	0:17f575135219	836	pc.printf("glideROLL = %f, glidePITCH = %f\r\n", g_glideloopROLL, g_glideloopPITCH);
HARUKIDELTA	0:17f575135219	837
HARUKIDELTA	0:17f575135219	838	return SDerrorcount;
HARUKIDELTA	0:17f575135219	839	}
HARUKIDELTA	0:17f575135219	840
HARUKIDELTA	0:17f575135219	841	void CalculateControlValue(float targetAngle[3], float controlValue[3]){
HARUKIDELTA	0:17f575135219	842	static int t_last;
HARUKIDELTA	0:17f575135219	843	int t_now;
HARUKIDELTA	0:17f575135219	844	float dt;
HARUKIDELTA	0:17f575135219	845
HARUKIDELTA	0:17f575135219	846	t_now = t.read_us();
HARUKIDELTA	0:17f575135219	847	dt = (float)((t_now - t_last)/1000000.0f) ;
HARUKIDELTA	0:17f575135219	848	t_last = t_now;
HARUKIDELTA	0:17f575135219	849
taknokolat	1:f383708a5a52	850
taknokolat	1:f383708a5a52	851	//controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
taknokolat	1:f383708a5a52	852	controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御
HARUKIDELTA	0:17f575135219	853	controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
HARUKIDELTA	0:17f575135219	854	}
HARUKIDELTA	0:17f575135219	855
HARUKIDELTA	0:17f575135219	856	void UpdateAutoPWM(float controlValue[3]){
HARUKIDELTA	0:17f575135219	857	int16_t addpwm[2]; //-500~500
HARUKIDELTA	0:17f575135219	858	addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正レバー:機首上げ正
HARUKIDELTA	0:17f575135219	859	addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！）レバー:右回転正
HARUKIDELTA	0:17f575135219	860
HARUKIDELTA	2:23daa5fa28b4	861	autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite
HARUKIDELTA	8:66bba39b95a9	862	autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL];
HARUKIDELTA	0:17f575135219	863	//autopwm[THR] = oldTHR;
HARUKIDELTA	0:17f575135219	864
HARUKIDELTA	0:17f575135219	865	autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
taknokolat	1:f383708a5a52	866	autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
HARUKIDELTA	2:23daa5fa28b4	867
HARUKIDELTA	0:17f575135219	868	}
HARUKIDELTA	0:17f575135219	869
HARUKIDELTA	0:17f575135219	870	inline float CalcRatio(float value, float trim, float limit){
HARUKIDELTA	0:17f575135219	871	return (value - trim) / (limit - trim);
HARUKIDELTA	0:17f575135219	872	}
HARUKIDELTA	0:17f575135219	873
HARUKIDELTA	0:17f575135219	874	bool CheckSW_Up(Channel ch){
HARUKIDELTA	0:17f575135219	875	if(SWITCH_CHECK < sbus.manualpwm[ch]){
HARUKIDELTA	0:17f575135219	876	return true;
HARUKIDELTA	0:17f575135219	877	}else{
HARUKIDELTA	0:17f575135219	878	return false;
HARUKIDELTA	0:17f575135219	879	}
HARUKIDELTA	0:17f575135219	880	}
HARUKIDELTA	0:17f575135219	881
HARUKIDELTA	0:17f575135219	882	int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){
HARUKIDELTA	0:17f575135219	883	if(value > max) return max;
HARUKIDELTA	0:17f575135219	884	if(value < min) return min;
HARUKIDELTA	0:17f575135219	885	return value;
HARUKIDELTA	0:17f575135219	886	}
HARUKIDELTA	0:17f575135219	887
HARUKIDELTA	0:17f575135219	888	inline int16_t SetTHRinRatio(float ratio){
HARUKIDELTA	0:17f575135219	889	return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
HARUKIDELTA	0:17f575135219	890	}
HARUKIDELTA	0:17f575135219	891
HARUKIDELTA	2:23daa5fa28b4	892
HARUKIDELTA	2:23daa5fa28b4	893
HARUKIDELTA	0:17f575135219	894	/---SBUS割り込み処理---/
HARUKIDELTA	0:17f575135219	895
HARUKIDELTA	0:17f575135219	896	//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
HARUKIDELTA	0:17f575135219	897	//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
HARUKIDELTA	0:17f575135219	898	void Update_PWM()
HARUKIDELTA	0:17f575135219	899	{
HARUKIDELTA	0:17f575135219	900	static int16_t pwm[6];
HARUKIDELTA	0:17f575135219	901	static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
HARUKIDELTA	0:17f575135219	902	if(sbus.flg_ch_update == true){
taknokolat	10:652071c20bf6	903	NVIC_DisableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	904	switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
HARUKIDELTA	0:17f575135219	905	case Manual:
HARUKIDELTA	0:17f575135219	906	for(uint8_t i=0;i<6;i++){
taknokolat	1:f383708a5a52	907	pwm[i] = sbus.manualpwm[i];
HARUKIDELTA	0:17f575135219	908	}
HARUKIDELTA	2:23daa5fa28b4	909	/*pc.printf("%d ,",pwm[0]);//R
HARUKIDELTA	2:23daa5fa28b4	910	pc.printf("%d ,\r\n",pwm[5]);//L*/
HARUKIDELTA	0:17f575135219	911	oldTHR = sbus.manualpwm[THR];
HARUKIDELTA	0:17f575135219	912	//pc.printf("update_manual\r\n");
HARUKIDELTA	0:17f575135219	913	break;
HARUKIDELTA	0:17f575135219	914
HARUKIDELTA	0:17f575135219	915	case Auto:
HARUKIDELTA	0:17f575135219	916	pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL];
HARUKIDELTA	0:17f575135219	917	pwm[ELE] = autopwm[ELE];
HARUKIDELTA	0:17f575135219	918	pwm[THR] = autopwm[THR];
HARUKIDELTA	0:17f575135219	919	pwm[RUD] = autopwm[RUD];
HARUKIDELTA	0:17f575135219	920	pwm[DROP] = autopwm[DROP];
HARUKIDELTA	0:17f575135219	921	pwm[AIL_L] = autopwm[AIL_L];
taknokolat	10:652071c20bf6	922	//pc.printf("%d ,",pwm[AIL_R]);//R
taknokolat	10:652071c20bf6	923	//pc.printf("%d ,\r\n",pwm[AIL_L]);//L
HARUKIDELTA	0:17f575135219	924	//pc.printf("update_auto\r\n");
HARUKIDELTA	2:23daa5fa28b4	925
HARUKIDELTA	0:17f575135219	926	break;
HARUKIDELTA	0:17f575135219	927
HARUKIDELTA	0:17f575135219	928	default:
HARUKIDELTA	0:17f575135219	929	for(uint8_t i=0;i<6;i++){
HARUKIDELTA	0:17f575135219	930	pwm[i] = sbus.manualpwm[i];
HARUKIDELTA	0:17f575135219	931	} //pc.printf("update_manual\r\n");
HARUKIDELTA	0:17f575135219	932	break;
taknokolat	10:652071c20bf6	933	NVIC_EnableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	934	}
HARUKIDELTA	0:17f575135219	935	for(uint8_t i=0;i<6;i++){
HARUKIDELTA	0:17f575135219	936	if(pwm[i]<1000 \|\| pwm[i]>2000) pwm[i]=temppwm[i];
HARUKIDELTA	0:17f575135219	937	temppwm[i]=pwm[i];
HARUKIDELTA	0:17f575135219	938	}
HARUKIDELTA	0:17f575135219	939	}else{
HARUKIDELTA	0:17f575135219	940	pc.printf("0\r\n");
HARUKIDELTA	0:17f575135219	941	}
HARUKIDELTA	0:17f575135219	942	sbus.flg_ch_update = false;
HARUKIDELTA	0:17f575135219	943	Output_PWM(pwm);
HARUKIDELTA	0:17f575135219	944	}
HARUKIDELTA	0:17f575135219	945
HARUKIDELTA	0:17f575135219	946
HARUKIDELTA	0:17f575135219	947	//pwmをサーボに出力。
HARUKIDELTA	0:17f575135219	948	void Output_PWM(int16_t pwm[5])
HARUKIDELTA	0:17f575135219	949	{
taknokolat	10:652071c20bf6	950	NVIC_DisableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	951	servo1.pulsewidth_us(pwm[0]);
HARUKIDELTA	0:17f575135219	952	servo2.pulsewidth_us(pwm[1]);
HARUKIDELTA	0:17f575135219	953	servo3.pulsewidth_us(pwm[2]);
HARUKIDELTA	0:17f575135219	954	servo4.pulsewidth_us(pwm[3]);
HARUKIDELTA	0:17f575135219	955	servo5.pulsewidth_us(pwm[4]);
HARUKIDELTA	0:17f575135219	956	servo6.pulsewidth_us(pwm[5]);
taknokolat	10:652071c20bf6	957	NVIC_EnableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	958
HARUKIDELTA	0:17f575135219	959	}
HARUKIDELTA	0:17f575135219	960
HARUKIDELTA	0:17f575135219	961	void ResetTrim(){
HARUKIDELTA	0:17f575135219	962	for(uint8_t i=0; i<6; i++){ //i=4から書き換え＿投下サーボは入ってない模様
HARUKIDELTA	0:17f575135219	963	trimpwm[i] = sbus.manualpwm[i];
HARUKIDELTA	0:17f575135219	964	}
HARUKIDELTA	0:17f575135219	965	pc.printf("reset PWM trim\r\n");
HARUKIDELTA	0:17f575135219	966	}
HARUKIDELTA	0:17f575135219	967
HARUKIDELTA	0:17f575135219	968
HARUKIDELTA	0:17f575135219	969	void SensingMPU(){
HARUKIDELTA	0:17f575135219	970	//static int16_t deltaT = 0, t_start = 0;
HARUKIDELTA	0:17f575135219	971	//t_start = t.read_us();
HARUKIDELTA	0:17f575135219	972
HARUKIDELTA	0:17f575135219	973	float rpy[3] = {0}, oldrpy[3] = {0};
HARUKIDELTA	0:17f575135219	974	static uint16_t count_changeRPY = 0;
HARUKIDELTA	0:17f575135219	975	static bool flg_checkoutlier = false;
HARUKIDELTA	0:17f575135219	976	NVIC_DisableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	977	NVIC_DisableIRQ(USART2_IRQn);
HARUKIDELTA	0:17f575135219	978	NVIC_DisableIRQ(TIM5_IRQn);
taknokolat	10:652071c20bf6	979	NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
taknokolat	10:652071c20bf6	980	NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
HARUKIDELTA	0:17f575135219	981
HARUKIDELTA	0:17f575135219	982	mpu6050.getRollPitchYaw_Skipper(rpy);
HARUKIDELTA	0:17f575135219	983
HARUKIDELTA	0:17f575135219	984	NVIC_EnableIRQ(USART1_IRQn);
HARUKIDELTA	0:17f575135219	985	NVIC_EnableIRQ(USART2_IRQn);
HARUKIDELTA	0:17f575135219	986	NVIC_EnableIRQ(TIM5_IRQn);
HARUKIDELTA	0:17f575135219	987	NVIC_EnableIRQ(EXTI0_IRQn);
HARUKIDELTA	0:17f575135219	988	NVIC_EnableIRQ(EXTI9_5_IRQn);
HARUKIDELTA	0:17f575135219	989
HARUKIDELTA	0:17f575135219	990
HARUKIDELTA	0:17f575135219	991	//外れ値対策
HARUKIDELTA	0:17f575135219	992	for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
HARUKIDELTA	0:17f575135219	993	rpy[ROLL] -= FirstROLL;
HARUKIDELTA	0:17f575135219	994	rpy[PITCH] -= FirstPITCH;
HARUKIDELTA	0:17f575135219	995	rpy[YAW] -= FirstYAW;
HARUKIDELTA	0:17f575135219	996
HARUKIDELTA	0:17f575135219	997	for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 \|\| rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
HARUKIDELTA	0:17f575135219	998	if(!flg_checkoutlier \|\| count_changeRPY >= 2){
HARUKIDELTA	0:17f575135219	999	for(uint8_t i=0; i<3; i++){
HARUKIDELTA	0:17f575135219	1000	nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
HARUKIDELTA	0:17f575135219	1001	}
HARUKIDELTA	0:17f575135219	1002	count_changeRPY = 0;
HARUKIDELTA	0:17f575135219	1003	}else count_changeRPY++;
HARUKIDELTA	0:17f575135219	1004	flg_checkoutlier = false;
HARUKIDELTA	0:17f575135219	1005
HARUKIDELTA	0:17f575135219	1006	}
HARUKIDELTA	0:17f575135219	1007
HARUKIDELTA	0:17f575135219	1008	float TranslateNewYaw(float beforeYaw, float newzeroYaw){
HARUKIDELTA	0:17f575135219	1009	float newYaw = beforeYaw - newzeroYaw;
HARUKIDELTA	0:17f575135219	1010
HARUKIDELTA	0:17f575135219	1011	if(newYaw<-180.0f) newYaw += 360.0f;
HARUKIDELTA	0:17f575135219	1012	else if(newYaw>180.0f) newYaw -= 360.0f;
HARUKIDELTA	0:17f575135219	1013	return newYaw;
HARUKIDELTA	0:17f575135219	1014	}
HARUKIDELTA	0:17f575135219	1015
HARUKIDELTA	0:17f575135219	1016
HARUKIDELTA	0:17f575135219	1017	void getSF_Serial(){
HARUKIDELTA	0:17f575135219	1018
HARUKIDELTA	0:17f575135219	1019	static char SFbuf[16];
HARUKIDELTA	0:17f575135219	1020	static int bufcounter=0;
HARUKIDELTA	0:17f575135219	1021
taknokolat	14:bbd5ff31fb67	1022	if(pc.readable()) { // 受信確認
taknokolat	14:bbd5ff31fb67	1023	SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
taknokolat	14:bbd5ff31fb67	1024	}
taknokolat	11:c0b9ad25d3db	1025
taknokolat	11:c0b9ad25d3db	1026
taknokolat	11:c0b9ad25d3db	1027	pc.printf("%c",SFbuf[bufcounter]);
taknokolat	11:c0b9ad25d3db	1028
HARUKIDELTA	0:17f575135219	1029	if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
HARUKIDELTA	0:17f575135219	1030
HARUKIDELTA	0:17f575135219	1031	if(bufcounter==5 && SFbuf[4]=='F'){
HARUKIDELTA	0:17f575135219	1032	g_landingcommand = SFbuf[1];
taknokolat	14:bbd5ff31fb67	1033	wait_ms(8);
HARUKIDELTA	0:17f575135219	1034	if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
HARUKIDELTA	0:17f575135219	1035	bufcounter = 0;
HARUKIDELTA	0:17f575135219	1036	memset(SFbuf, 0, strlen(SFbuf));
taknokolat	10:652071c20bf6	1037	//pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
HARUKIDELTA	0:17f575135219	1038	}
taknokolat	10:652071c20bf6	1039	else if(bufcounter>=5 ){
HARUKIDELTA	0:17f575135219	1040	pc.printf("Communication Falsed.\r\n");
HARUKIDELTA	0:17f575135219	1041	bufcounter = 0;
HARUKIDELTA	0:17f575135219	1042	}
taknokolat	11:c0b9ad25d3db	1043
HARUKIDELTA	0:17f575135219	1044	}
HARUKIDELTA	0:17f575135219	1045
HARUKIDELTA	0:17f575135219	1046	float ConvertByteintoFloat(char high, char low){
HARUKIDELTA	0:17f575135219	1047
HARUKIDELTA	0:17f575135219	1048	//int16_t intvalue = (int16_t)high*256 + (int16_t)low;
HARUKIDELTA	0:17f575135219	1049	int16_t intvalue = (int16_t)(((int16_t)high << 8) \| low); // Turn the MSB and LSB into a signed 16-bit value
HARUKIDELTA	0:17f575135219	1050	float floatvalue = (float)intvalue;
HARUKIDELTA	0:17f575135219	1051	return floatvalue;
HARUKIDELTA	0:17f575135219	1052	}
HARUKIDELTA	0:17f575135219	1053
HARUKIDELTA	0:17f575135219	1054
HARUKIDELTA	0:17f575135219	1055	//超音波割り込み
taknokolat	10:652071c20bf6	1056	/*void UpdateDist(){
HARUKIDELTA	0:17f575135219	1057	g_distance = usensor.get_dist_cm();
HARUKIDELTA	0:17f575135219	1058	usensor.start();
taknokolat	10:652071c20bf6	1059	}*/
HARUKIDELTA	0:17f575135219	1060
HARUKIDELTA	0:17f575135219	1061	//8の字旋回
HARUKIDELTA	0:17f575135219	1062	void UpdateTargetAngle_Moebius(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1063	static uint8_t RotateCounter=0;
HARUKIDELTA	0:17f575135219	1064	static bool flg_setInStartAuto = false;
HARUKIDELTA	0:17f575135219	1065	static float FirstYAW_Moebius = 0.0;
HARUKIDELTA	0:17f575135219	1066	float newYaw_Moebius;
HARUKIDELTA	0:17f575135219	1067
HARUKIDELTA	0:17f575135219	1068	if(!flg_setInStartAuto && CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1069	FirstYAW_Moebius = nowAngle[YAW];
HARUKIDELTA	0:17f575135219	1070	RotateCounter = 0;
HARUKIDELTA	0:17f575135219	1071	flg_setInStartAuto = true;
HARUKIDELTA	0:17f575135219	1072	}else if(!CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1073	flg_setInStartAuto = false;
HARUKIDELTA	0:17f575135219	1074	led2 = 0;
HARUKIDELTA	0:17f575135219	1075	}
HARUKIDELTA	6:ed61ed8b8fab	1076	autopwm[THR]=oldTHR;
HARUKIDELTA	6:ed61ed8b8fab	1077
HARUKIDELTA	0:17f575135219	1078	newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);
HARUKIDELTA	0:17f575135219	1079
HARUKIDELTA	0:17f575135219	1080	if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n");}
HARUKIDELTA	0:17f575135219	1081	if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n");}
HARUKIDELTA	6:ed61ed8b8fab	1082	if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-10.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");}
HARUKIDELTA	6:ed61ed8b8fab	1083	if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");}
HARUKIDELTA	0:17f575135219	1084
HARUKIDELTA	0:17f575135219	1085
HARUKIDELTA	0:17f575135219	1086	if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle);
HARUKIDELTA	0:17f575135219	1087	else UpdateTargetAngle_Leftloop_short(targetAngle); //左旋回
HARUKIDELTA	0:17f575135219	1088
HARUKIDELTA	0:17f575135219	1089	}
HARUKIDELTA	0:17f575135219	1090
HARUKIDELTA	0:17f575135219	1091	//自動滑空
HARUKIDELTA	0:17f575135219	1092	void UpdateTargetAngle_Glide(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1093	static int THRcount = 0;
HARUKIDELTA	0:17f575135219	1094	static int t_start = 0;
HARUKIDELTA	0:17f575135219	1095	static bool flg_tstart = false;
HARUKIDELTA	0:17f575135219	1096	int t_diff = 0;
taknokolat	1:f383708a5a52	1097	static int groundcount = 0;
HARUKIDELTA	0:17f575135219	1098
HARUKIDELTA	0:17f575135219	1099	targetAngle[ROLL] = g_glideloopROLL;
HARUKIDELTA	0:17f575135219	1100	targetAngle[PITCH] = g_glideloopPITCH;
HARUKIDELTA	0:17f575135219	1101
HARUKIDELTA	7:53b0eb6f6bd3	1102	// autopwm[THR]=oldTHR;
taknokolat	10:652071c20bf6	1103
taknokolat	1:f383708a5a52	1104
HARUKIDELTA	7:53b0eb6f6bd3	1105
HARUKIDELTA	0:17f575135219	1106	//時間計測開始設定
HARUKIDELTA	0:17f575135219	1107	if(!flg_tstart && CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1108	t_start = t.read();
HARUKIDELTA	0:17f575135219	1109	flg_tstart = true;
HARUKIDELTA	0:17f575135219	1110	pc.printf("timer start\r\n");
taknokolat	1:f383708a5a52	1111	}else if(!CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1112	t_start = 0;
HARUKIDELTA	0:17f575135219	1113	flg_tstart = false;
HARUKIDELTA	0:17f575135219	1114	}
HARUKIDELTA	0:17f575135219	1115
taknokolat	1:f383708a5a52	1116
HARUKIDELTA	0:17f575135219	1117	//フラグが偽であれば計測は行わない
HARUKIDELTA	0:17f575135219	1118	if(flg_tstart){
HARUKIDELTA	0:17f575135219	1119	t_diff = t.read() - t_start;
HARUKIDELTA	0:17f575135219	1120	//一定高度or15秒でled点灯
taknokolat	10:652071c20bf6	1121	NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat	1:f383708a5a52	1122	if((groundcount>5 && g_distance>0) \|\| t_diff > 15){
HARUKIDELTA	0:17f575135219	1123	led2 = 1;
HARUKIDELTA	0:17f575135219	1124	//pc.printf("Call [Stop!] calling!\r\n");
HARUKIDELTA	0:17f575135219	1125	}
taknokolat	10:652071c20bf6	1126
taknokolat	1:f383708a5a52	1127	if(g_distance<180 && g_distance > 0) groundcount++;
taknokolat	10:652071c20bf6	1128	NVIC_EnableIRQ(EXTI9_5_IRQn);
HARUKIDELTA	0:17f575135219	1129	}else{
HARUKIDELTA	0:17f575135219	1130	t_diff = 0;
taknokolat	1:f383708a5a52	1131	groundcount = 0;
HARUKIDELTA	0:17f575135219	1132	led2 = 0;
HARUKIDELTA	0:17f575135219	1133	}
HARUKIDELTA	0:17f575135219	1134
taknokolat	1:f383708a5a52	1135	if(t_diff > 17){
HARUKIDELTA	0:17f575135219	1136	autopwm[THR] = SetTHRinRatio(0.5);
HARUKIDELTA	0:17f575135219	1137	}else{
taknokolat	10:652071c20bf6	1138	NVIC_DisableIRQ(EXTI9_5_IRQn);
HARUKIDELTA	0:17f575135219	1139	if(g_distance<150 && g_distance>0 ){
taknokolat	10:652071c20bf6	1140	NVIC_EnableIRQ(EXTI9_5_IRQn);
HARUKIDELTA	0:17f575135219	1141	THRcount++;
HARUKIDELTA	0:17f575135219	1142	if(THRcount>5){
taknokolat	1:f383708a5a52	1143	autopwm[THR] = SetTHRinRatio(0.6);
HARUKIDELTA	0:17f575135219	1144	//pc.printf("throttle ON\r\n");
HARUKIDELTA	0:17f575135219	1145	}
HARUKIDELTA	0:17f575135219	1146	}else{
HARUKIDELTA	0:17f575135219	1147	autopwm[THR] = 1180;
HARUKIDELTA	0:17f575135219	1148	THRcount = 0;
HARUKIDELTA	0:17f575135219	1149	}
HARUKIDELTA	7:53b0eb6f6bd3	1150	}
HARUKIDELTA	0:17f575135219	1151	}
HARUKIDELTA	0:17f575135219	1152	//離陸-投下-着陸一連
HARUKIDELTA	0:17f575135219	1153	void Take_off_and_landing(float targetAngle[3]){
taknokolat	10:652071c20bf6	1154	/*
HARUKIDELTA	0:17f575135219	1155	if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
HARUKIDELTA	0:17f575135219	1156
HARUKIDELTA	0:17f575135219	1157	switch(bombing_mode){
HARUKIDELTA	0:17f575135219	1158	case Takeoff:
HARUKIDELTA	0:17f575135219	1159	static bool flg_setFirstYaw = false;
HARUKIDELTA	0:17f575135219	1160	static int TakeoffCount = 0;
HARUKIDELTA	0:17f575135219	1161
HARUKIDELTA	0:17f575135219	1162	if(!flg_setFirstYaw && CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1163	FirstYAW = nowAngle[YAW];
HARUKIDELTA	0:17f575135219	1164	flg_setFirstYaw = true;
HARUKIDELTA	0:17f575135219	1165	}else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1166	flg_setFirstYaw = false;
HARUKIDELTA	0:17f575135219	1167	}
HARUKIDELTA	0:17f575135219	1168
HARUKIDELTA	0:17f575135219	1169	UpdateTargetAngle_Takeoff(targetAngle);
HARUKIDELTA	0:17f575135219	1170
HARUKIDELTA	0:17f575135219	1171	if(g_distance>150) TakeoffCount++;
HARUKIDELTA	0:17f575135219	1172	else TakeoffCount = 0;
HARUKIDELTA	0:17f575135219	1173	if(TakeoffCount>5){
HARUKIDELTA	0:17f575135219	1174	autopwm[THR] = 1180+32020.5;
HARUKIDELTA	0:17f575135219	1175	pc.printf("Now go to Approach mode!!");
HARUKIDELTA	0:17f575135219	1176	bombing_mode = Approach;
HARUKIDELTA	0:17f575135219	1177	}
HARUKIDELTA	0:17f575135219	1178	break;
taknokolat	10:652071c20bf6	1179
taknokolat	10:652071c20bf6	1180	//case Chicken:
taknokolat	10:652071c20bf6	1181	//break;
taknokolat	10:652071c20bf6	1182
HARUKIDELTA	0:17f575135219	1183	case Transition:
HARUKIDELTA	0:17f575135219	1184	static int ApproachCount = 0;
HARUKIDELTA	0:17f575135219	1185	targetAngle[YAW]=180.0;
HARUKIDELTA	0:17f575135219	1186	int Judge = Rotate(targetAngle, g_SerialTargetYAW);
HARUKIDELTA	0:17f575135219	1187
HARUKIDELTA	0:17f575135219	1188	if(Judge==0) ApproachCount++;
HARUKIDELTA	0:17f575135219	1189	if(ApproachCount>5) bombing_mode = Approach;
HARUKIDELTA	0:17f575135219	1190	break;
HARUKIDELTA	0:17f575135219	1191
HARUKIDELTA	0:17f575135219	1192	case Approach:
taknokolat	10:652071c20bf6	1193	*/
taknokolat	10:652071c20bf6	1194	autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
HARUKIDELTA	0:17f575135219	1195	UpdateTargetAngle_Approach(targetAngle);
taknokolat	10:652071c20bf6	1196	/*
HARUKIDELTA	0:17f575135219	1197	break;
HARUKIDELTA	0:17f575135219	1198
HARUKIDELTA	0:17f575135219	1199	default:
HARUKIDELTA	0:17f575135219	1200	bombing_mode = Takeoff;
HARUKIDELTA	0:17f575135219	1201	break;
HARUKIDELTA	0:17f575135219	1202	}
taknokolat	10:652071c20bf6	1203	*/
HARUKIDELTA	0:17f575135219	1204	}
HARUKIDELTA	0:17f575135219	1205
HARUKIDELTA	0:17f575135219	1206	//離陸モード
HARUKIDELTA	0:17f575135219	1207	void UpdateTargetAngle_Takeoff(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1208	//pc.printf("%d \r\n",g_distance);
HARUKIDELTA	0:17f575135219	1209	targetAngle[ROLL] = g_gostraightROLL;
HARUKIDELTA	0:17f575135219	1210	targetAngle[PITCH] = g_loopTHR;
HARUKIDELTA	0:17f575135219	1211	autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
HARUKIDELTA	0:17f575135219	1212	}
HARUKIDELTA	0:17f575135219	1213
HARUKIDELTA	0:17f575135219	1214	//ヨーを目標値にして許容角度になるまで水平旋回
HARUKIDELTA	0:17f575135219	1215	int Rotate(float targetAngle[3], float TargetYAW){
HARUKIDELTA	0:17f575135219	1216	float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
HARUKIDELTA	0:17f575135219	1217
HARUKIDELTA	0:17f575135219	1218	if(diffYaw > LIMIT_STRAIGHT_YAW){
HARUKIDELTA	0:17f575135219	1219	/*
HARUKIDELTA	0:17f575135219	1220	if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
HARUKIDELTA	0:17f575135219	1221	else UpdateTargetAngle_Rightloop(targetAngle);
HARUKIDELTA	0:17f575135219	1222	*/
HARUKIDELTA	0:17f575135219	1223	UpdateTargetAngle_Rightloop_short(targetAngle);
HARUKIDELTA	0:17f575135219	1224	return 1;
HARUKIDELTA	0:17f575135219	1225	}else if(diffYaw < -LIMIT_STRAIGHT_YAW){
HARUKIDELTA	0:17f575135219	1226	UpdateTargetAngle_Leftloop_short(targetAngle);
HARUKIDELTA	0:17f575135219	1227	/*
HARUKIDELTA	0:17f575135219	1228	if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
HARUKIDELTA	0:17f575135219	1229	else UpdateTargetAngle_Leftloop(targetAngle);
HARUKIDELTA	0:17f575135219	1230	*/
HARUKIDELTA	0:17f575135219	1231	return 1;
HARUKIDELTA	0:17f575135219	1232	}else{
HARUKIDELTA	0:17f575135219	1233	UpdateTargetAngle_GoStraight(targetAngle);
HARUKIDELTA	0:17f575135219	1234	return 0;
HARUKIDELTA	0:17f575135219	1235	}
HARUKIDELTA	0:17f575135219	1236	}
HARUKIDELTA	0:17f575135219	1237
HARUKIDELTA	0:17f575135219	1238	//チキラー投下
HARUKIDELTA	0:17f575135219	1239	void Chicken_Drop(){
HARUKIDELTA	0:17f575135219	1240	if(CheckSW_Up(Ch7)){
HARUKIDELTA	0:17f575135219	1241	autopwm[DROP] = 1512;
HARUKIDELTA	0:17f575135219	1242	pc.printf("Bombed!\r\n");
HARUKIDELTA	0:17f575135219	1243	RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
HARUKIDELTA	0:17f575135219	1244	//operation_mode = Approach;
HARUKIDELTA	0:17f575135219	1245	//buzzer = 0;
HARUKIDELTA	0:17f575135219	1246	pc.printf("Goto LeftLoop mode\r\n");
HARUKIDELTA	0:17f575135219	1247	}
HARUKIDELTA	0:17f575135219	1248	}
HARUKIDELTA	0:17f575135219	1249
HARUKIDELTA	0:17f575135219	1250	void ReturnChickenServo1(){
HARUKIDELTA	0:17f575135219	1251	autopwm[DROP] = 1344;
HARUKIDELTA	0:17f575135219	1252	pc.printf("first reverse\r\n");
HARUKIDELTA	0:17f575135219	1253	RerurnChickenServo2.attach(&ReturnChickenServo2, 1);
HARUKIDELTA	0:17f575135219	1254	}
HARUKIDELTA	0:17f575135219	1255
HARUKIDELTA	0:17f575135219	1256	void ReturnChickenServo2(){
HARUKIDELTA	0:17f575135219	1257	autopwm[DROP] = 1392;
HARUKIDELTA	0:17f575135219	1258	pc.printf("second reverse\r\n");
taknokolat	10:652071c20bf6	1259	}
taknokolat	10:652071c20bf6	1260
taknokolat	10:652071c20bf6	1261	//着陸モード（PCからの指令に従う）
taknokolat	10:652071c20bf6	1262	void UpdateTargetAngle_Approach(float targetAngle[3]){
taknokolat	10:652071c20bf6	1263
taknokolat	10:652071c20bf6	1264	NVIC_DisableIRQ(USART2_IRQn);
taknokolat	10:652071c20bf6	1265
taknokolat	10:652071c20bf6	1266	switch(g_landingcommand){
taknokolat	10:652071c20bf6	1267	case 'R': //右旋回セヨ
taknokolat	10:652071c20bf6	1268	targetAngle[ROLL] = g_rightloopROLL;
taknokolat	10:652071c20bf6	1269	targetAngle[PITCH] = g_rightloopPITCH ;
taknokolat	10:652071c20bf6	1270	autopwm[RUD]=rightloopRUD; //RUD固定
taknokolat	10:652071c20bf6	1271	if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節
taknokolat	10:652071c20bf6	1272	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
taknokolat	10:652071c20bf6	1273	}
taknokolat	10:652071c20bf6	1274	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
taknokolat	10:652071c20bf6	1275	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
HARUKIDELTA	0:17f575135219	1276
taknokolat	10:652071c20bf6	1277	break;
taknokolat	10:652071c20bf6	1278
taknokolat	10:652071c20bf6	1279	case 'L': //左旋回セヨ
taknokolat	10:652071c20bf6	1280	targetAngle[ROLL] = g_leftloopROLL;
taknokolat	10:652071c20bf6	1281	targetAngle[PITCH] = g_leftloopPITCH;
taknokolat	10:652071c20bf6	1282	autopwm[RUD]=leftloopRUD;
taknokolat	10:652071c20bf6	1283	if(autopwm[AIL_R]>trimpwm[AIL_R]){
taknokolat	10:652071c20bf6	1284	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
taknokolat	10:652071c20bf6	1285	}
taknokolat	10:652071c20bf6	1286	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
HARUKIDELTA	0:17f575135219	1287
taknokolat	10:652071c20bf6	1288	break;
taknokolat	10:652071c20bf6	1289
taknokolat	10:652071c20bf6	1290	case 'G': //直進セヨ
taknokolat	10:652071c20bf6	1291	targetAngle[ROLL] = g_gostraightROLL;
taknokolat	10:652071c20bf6	1292	targetAngle[PITCH] = g_gostraightPITCH;
taknokolat	10:652071c20bf6	1293
taknokolat	10:652071c20bf6	1294
taknokolat	10:652071c20bf6	1295	break;
taknokolat	10:652071c20bf6	1296
taknokolat	10:652071c20bf6	1297	case 'Y': //指定ノヨー方向ニ移動セヨ
taknokolat	10:652071c20bf6	1298	Rotate(targetAngle, g_SerialTargetYAW);
taknokolat	10:652071c20bf6	1299	break;
taknokolat	10:652071c20bf6	1300
taknokolat	10:652071c20bf6	1301	case 'B': //ブザーヲ鳴ラセ
taknokolat	10:652071c20bf6	1302	//buzzer = 1;
taknokolat	10:652071c20bf6	1303	break;
taknokolat	10:652071c20bf6	1304
taknokolat	10:652071c20bf6	1305	case 'D': //物資ヲ落トセ
taknokolat	10:652071c20bf6	1306	Chicken_Drop();
taknokolat	10:652071c20bf6	1307	break;
taknokolat	10:652071c20bf6	1308
taknokolat	10:652071c20bf6	1309	case 'C': //停止セヨ
taknokolat	10:652071c20bf6	1310	targetAngle[ROLL] = 0.0;
taknokolat	10:652071c20bf6	1311	targetAngle[PITCH] = -3.0;
taknokolat	10:652071c20bf6	1312	autopwm[THR] = minpwm[THR];
taknokolat	10:652071c20bf6	1313	break;
HARUKIDELTA	0:17f575135219	1314
taknokolat	10:652071c20bf6	1315	}
taknokolat	10:652071c20bf6	1316	NVIC_EnableIRQ(USART2_IRQn);
taknokolat	10:652071c20bf6	1317	}
taknokolat	10:652071c20bf6	1318
taknokolat	10:652071c20bf6	1319	void checkHeight(float targetAngle[3]){
taknokolat	10:652071c20bf6	1320
taknokolat	10:652071c20bf6	1321	static int targetHeight = 200;
taknokolat	10:652071c20bf6	1322	NVIC_DisableIRQ(EXTI9_5_IRQn);
taknokolat	10:652071c20bf6	1323	if(g_distance < targetHeight + ALLOWHEIGHT){
taknokolat	10:652071c20bf6	1324	UpdateTargetAngle_NoseUP(targetAngle);
taknokolat	10:652071c20bf6	1325	if(CheckSW_Up(Ch7)) led2 = 1;
taknokolat	10:652071c20bf6	1326	}
taknokolat	10:652071c20bf6	1327	else if(g_distance > targetHeight - ALLOWHEIGHT){
taknokolat	10:652071c20bf6	1328	UpdateTargetAngle_NoseDOWN(targetAngle);
taknokolat	10:652071c20bf6	1329	if(CheckSW_Up(Ch7)) led2 = 1;
taknokolat	10:652071c20bf6	1330	}
taknokolat	10:652071c20bf6	1331	else led2=0;
taknokolat	10:652071c20bf6	1332	NVIC_EnableIRQ(EXTI9_5_IRQn);
HARUKIDELTA	0:17f575135219	1333	}
HARUKIDELTA	0:17f575135219	1334
taknokolat	10:652071c20bf6	1335	void UpdateTargetAngle_NoseUP(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1336
HARUKIDELTA	0:17f575135219	1337	//targetAngle[PITCH] += 2.0f;
HARUKIDELTA	0:17f575135219	1338	//if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
HARUKIDELTA	0:17f575135219	1339	autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05);
HARUKIDELTA	0:17f575135219	1340	//pc.printf("nose UP");
HARUKIDELTA	0:17f575135219	1341	}
HARUKIDELTA	0:17f575135219	1342
HARUKIDELTA	0:17f575135219	1343	void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1344
HARUKIDELTA	0:17f575135219	1345	//targetAngle[PITCH] -= 2.0f;
HARUKIDELTA	0:17f575135219	1346	autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05);
HARUKIDELTA	0:17f575135219	1347	//pc.printf("nose DOWN");
HARUKIDELTA	0:17f575135219	1348	}
HARUKIDELTA	0:17f575135219	1349
HARUKIDELTA	0:17f575135219	1350	//直進
HARUKIDELTA	0:17f575135219	1351	void UpdateTargetAngle_GoStraight(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1352
HARUKIDELTA	0:17f575135219	1353	targetAngle[ROLL] = g_gostraightROLL;
HARUKIDELTA	0:17f575135219	1354	targetAngle[PITCH] = g_gostraightPITCH;
HARUKIDELTA	0:17f575135219	1355	autopwm[THR] = SetTHRinRatio(g_loopTHR);
HARUKIDELTA	0:17f575135219	1356
HARUKIDELTA	0:17f575135219	1357	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	0:17f575135219	1358	}
HARUKIDELTA	0:17f575135219	1359
HARUKIDELTA	3:206b17251f5b	1360
HARUKIDELTA	0:17f575135219	1361	//右旋回
HARUKIDELTA	0:17f575135219	1362	void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
HARUKIDELTA	0:17f575135219	1363
HARUKIDELTA	0:17f575135219	1364	targetAngle[ROLL] = g_rightloopROLL;
HARUKIDELTA	0:17f575135219	1365	targetAngle[PITCH] = g_rightloopPITCH ;
HARUKIDELTA	3:206b17251f5b	1366	autopwm[RUD]=rightloopRUD; //RUD固定
TUATBM	9:f6367b7fd7be	1367	autopwm[THR] = SetTHRinRatio(0.5); //手動スロットル記憶
HARUKIDELTA	3:206b17251f5b	1368	if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節
HARUKIDELTA	3:206b17251f5b	1369	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
HARUKIDELTA	3:206b17251f5b	1370	}
HARUKIDELTA	3:206b17251f5b	1371	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
TUATBM	9:f6367b7fd7be	1372	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
TUATBM	9:f6367b7fd7be	1373
HARUKIDELTA	0:17f575135219	1374	//checkHeight(targetAngle);
HARUKIDELTA	0:17f575135219	1375	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	0:17f575135219	1376	}
HARUKIDELTA	0:17f575135219	1377
HARUKIDELTA	4:fff1165ca50c	1378	//右旋回（着陸時スロットル０の時）
HARUKIDELTA	4:fff1165ca50c	1379	void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回
HARUKIDELTA	4:fff1165ca50c	1380
HARUKIDELTA	4:fff1165ca50c	1381	targetAngle[ROLL] = g_rightloopROLL;
HARUKIDELTA	4:fff1165ca50c	1382	targetAngle[PITCH] = g_rightloopPITCH ;
HARUKIDELTA	4:fff1165ca50c	1383	autopwm[RUD]=rightloopRUD;
HARUKIDELTA	4:fff1165ca50c	1384	autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR);
HARUKIDELTA	4:fff1165ca50c	1385	if(autopwm[AIL_R]>trimpwm[AIL_R]){
HARUKIDELTA	4:fff1165ca50c	1386	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
HARUKIDELTA	4:fff1165ca50c	1387	}
HARUKIDELTA	4:fff1165ca50c	1388	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
HARUKIDELTA	4:fff1165ca50c	1389
HARUKIDELTA	4:fff1165ca50c	1390	//checkHeight(targetAngle);
HARUKIDELTA	4:fff1165ca50c	1391	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	4:fff1165ca50c	1392	}
HARUKIDELTA	4:fff1165ca50c	1393
HARUKIDELTA	0:17f575135219	1394	void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回
HARUKIDELTA	0:17f575135219	1395
HARUKIDELTA	0:17f575135219	1396	targetAngle[ROLL] = g_rightloopROLLshort;
HARUKIDELTA	0:17f575135219	1397	targetAngle[PITCH] = g_rightloopPITCHshort;
HARUKIDELTA	3:206b17251f5b	1398	autopwm[RUD]=rightloopshortRUD;
HARUKIDELTA	3:206b17251f5b	1399	autopwm[THR] = oldTHR;
HARUKIDELTA	3:206b17251f5b	1400	if(autopwm[AIL_R]>trimpwm[AIL_R]){
HARUKIDELTA	3:206b17251f5b	1401	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
HARUKIDELTA	3:206b17251f5b	1402	}
HARUKIDELTA	3:206b17251f5b	1403	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
HARUKIDELTA	0:17f575135219	1404
HARUKIDELTA	0:17f575135219	1405	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	0:17f575135219	1406	}
HARUKIDELTA	0:17f575135219	1407
HARUKIDELTA	0:17f575135219	1408	//左旋回
HARUKIDELTA	0:17f575135219	1409	void UpdateTargetAngle_Leftloop(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1410
HARUKIDELTA	0:17f575135219	1411	targetAngle[ROLL] = g_leftloopROLL;
HARUKIDELTA	0:17f575135219	1412	targetAngle[PITCH] = g_leftloopPITCH;
HARUKIDELTA	4:fff1165ca50c	1413	autopwm[RUD]=leftloopRUD;
HARUKIDELTA	4:fff1165ca50c	1414	autopwm[THR] = oldTHR;
HARUKIDELTA	4:fff1165ca50c	1415	if(autopwm[AIL_R]>trimpwm[AIL_R]){
HARUKIDELTA	4:fff1165ca50c	1416	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
HARUKIDELTA	4:fff1165ca50c	1417	}
HARUKIDELTA	4:fff1165ca50c	1418	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
HARUKIDELTA	4:fff1165ca50c	1419	//checkHeight(targetAngle);
HARUKIDELTA	4:fff1165ca50c	1420
HARUKIDELTA	4:fff1165ca50c	1421	//printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	4:fff1165ca50c	1422	}
HARUKIDELTA	4:fff1165ca50c	1423
HARUKIDELTA	4:fff1165ca50c	1424	//左旋回（着陸時スロットル０のとき）
HARUKIDELTA	4:fff1165ca50c	1425	void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){
HARUKIDELTA	4:fff1165ca50c	1426
HARUKIDELTA	4:fff1165ca50c	1427	targetAngle[ROLL] = g_leftloopROLL;
HARUKIDELTA	4:fff1165ca50c	1428	targetAngle[PITCH] = g_leftloopPITCH;
HARUKIDELTA	4:fff1165ca50c	1429	autopwm[RUD]=leftloopRUD;
HARUKIDELTA	4:fff1165ca50c	1430	autopwm[THR] = minpwm[THR];
HARUKIDELTA	4:fff1165ca50c	1431	if(autopwm[AIL_R]>trimpwm[AIL_R]){
HARUKIDELTA	4:fff1165ca50c	1432	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
HARUKIDELTA	4:fff1165ca50c	1433	}
HARUKIDELTA	4:fff1165ca50c	1434	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
HARUKIDELTA	4:fff1165ca50c	1435	//autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop;
HARUKIDELTA	0:17f575135219	1436	//checkHeight(targetAngle);
HARUKIDELTA	0:17f575135219	1437
HARUKIDELTA	2:23daa5fa28b4	1438	//printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	0:17f575135219	1439	}
HARUKIDELTA	0:17f575135219	1440
HARUKIDELTA	0:17f575135219	1441	void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){
HARUKIDELTA	0:17f575135219	1442
HARUKIDELTA	0:17f575135219	1443	targetAngle[ROLL] = g_leftloopROLLshort;
HARUKIDELTA	0:17f575135219	1444	targetAngle[PITCH] = g_leftloopPITCHshort;
HARUKIDELTA	4:fff1165ca50c	1445	autopwm[RUD]=leftloopRUD;
HARUKIDELTA	4:fff1165ca50c	1446	autopwm[THR] = oldTHR;
HARUKIDELTA	4:fff1165ca50c	1447	if(autopwm[AIL_R]>trimpwm[AIL_R]){
HARUKIDELTA	4:fff1165ca50c	1448	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
HARUKIDELTA	4:fff1165ca50c	1449	}
HARUKIDELTA	4:fff1165ca50c	1450	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
HARUKIDELTA	4:fff1165ca50c	1451
HARUKIDELTA	0:17f575135219	1452	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
HARUKIDELTA	0:17f575135219	1453	}
taknokolat	10:652071c20bf6	1454
HARUKIDELTA	0:17f575135219	1455
HARUKIDELTA	0:17f575135219	1456	//デバッグ用
HARUKIDELTA	0:17f575135219	1457	void DebugPrint(){
HARUKIDELTA	0:17f575135219	1458	/*
HARUKIDELTA	0:17f575135219	1459	static int16_t deltaT = 0, t_start = 0;
HARUKIDELTA	0:17f575135219	1460	deltaT = t.read_u2s() - t_start;
HARUKIDELTA	0:17f575135219	1461	pc.printf("t:%d us, ",deltaT);
HARUKIDELTA	0:17f575135219	1462	pc.printf("\r\n");
HARUKIDELTA	0:17f575135219	1463	t_start = t.read_us();
HARUKIDELTA	0:17f575135219	1464	*/
HARUKIDELTA	0:17f575135219	1465	//for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
HARUKIDELTA	0:17f575135219	1466	//for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]);
HARUKIDELTA	0:17f575135219	1467	//pc.printf("\r\n");
HARUKIDELTA	2:23daa5fa28b4	1468	//for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
HARUKIDELTA	0:17f575135219	1469	//for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
taknokolat	10:652071c20bf6	1470	//pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]);
HARUKIDELTA	0:17f575135219	1471	//pc.printf("%d",g_distance);
HARUKIDELTA	0:17f575135219	1472
HARUKIDELTA	0:17f575135219	1473	//pc.printf("Mode: %c: ",g_buf[0]);
HARUKIDELTA	0:17f575135219	1474	//if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
taknokolat	10:652071c20bf6	1475	//pc.printf("\r\n");
HARUKIDELTA	0:17f575135219	1476	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	16 Sep 2018
Imports:	3
Forks:	0
Commits:	16
Dependents:	0
Dependencies:	4
Followers:	23

main.cpp@15:43427b0241d9, 2018-09-17 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning