Autoflight2018_57 - 右旋回修正

航空研究会 » Code » Autoflight2018_57

航空研究会 / Mbed 2 deprecated Autoflight2018_57

右旋回修正

Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_56 by 航空研究会

main.cpp@46:390da0008921, 2018-09-27 (annotated)

Committer:: taknokolat
Date:: Thu Sep 27 01:46:34 2018 +0000
Revision:: 46:390da0008921
Parent:: 45:eebdf6fa7b15

z;

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

main.cpp@46:390da0008921, 2018-09-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning