Autoflight2018_42 - a

航空研究会 » Code » Autoflight2018_42

航空研究会 / Mbed 2 deprecated Autoflight2018_42

Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_38 by 航空研究会

main.cpp@33:1a377cb25565, 2018-09-23 (annotated)

Committer:: HARUKIDELTA
Date:: Sun Sep 23 08:12:44 2018 +0000
Revision:: 33:1a377cb25565
Parent:: 32:9d0646372abe

FAILSAFE????

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

main.cpp@33:1a377cb25565, 2018-09-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning