Autoflight2022_913 - test

航空研究会 » Code » Autoflight2022_913

航空研究会 / Mbed 2 deprecated Autoflight2022_913

test

Dependencies: BMP280 HCSR04_from_mtmt MPU6050_2 mbed SDFileSystem3

main.cpp@0:2b57931c6ed4, 2022-09-13 (annotated)

Committer:: hitonari
Date:: Tue Sep 13 03:12:26 2022 +0000
Revision:: 0:2b57931c6ed4
Child:: 1:e7079f9771d3

test

Who changed what in which revision?

User	Revision	Line number	New contents of line
hitonari	0:2b57931c6ed4	1	//mbed
hitonari	0:2b57931c6ed4	2	#include "mbed.h"
hitonari	0:2b57931c6ed4	3	#include "FATFileSystem.h"
hitonari	0:2b57931c6ed4	4	#include "SDFileSystem.h"
hitonari	0:2b57931c6ed4	5	//C
hitonari	0:2b57931c6ed4	6	#include "math.h"
hitonari	0:2b57931c6ed4	7	//sensor
hitonari	0:2b57931c6ed4	8	#include "MPU6050_DMP6.h"
hitonari	0:2b57931c6ed4	9	//#include "MPU9250.h"
hitonari	0:2b57931c6ed4	10	#include "MPU9250_BMP.h"
hitonari	0:2b57931c6ed4	11	#include "BMP280.h"
hitonari	0:2b57931c6ed4	12	#include "hcsr04.h"
hitonari	0:2b57931c6ed4	13	//device
hitonari	0:2b57931c6ed4	14	#include "sbus.h"
hitonari	0:2b57931c6ed4	15	//config
hitonari	0:2b57931c6ed4	16	#include "SkipperSv2.h"
hitonari	0:2b57931c6ed4	17	#include "falfalla.h"
hitonari	0:2b57931c6ed4	18	//other
hitonari	0:2b57931c6ed4	19	#include "pid.h"
hitonari	0:2b57931c6ed4	20
hitonari	0:2b57931c6ed4	21	#define DEBUG_SEMIAUTO 0
hitonari	0:2b57931c6ed4	22	#define DEBUG_PRINT_INLOOP 1
hitonari	0:2b57931c6ed4	23
hitonari	0:2b57931c6ed4	24	#define KP_ELE 16.5//15.0 //2.0
hitonari	0:2b57931c6ed4	25	#define KI_ELE 0.0
hitonari	0:2b57931c6ed4	26	#define KD_ELE 0.1 //0/0
hitonari	0:2b57931c6ed4	27	#define KP_RUD 3.0
hitonari	0:2b57931c6ed4	28	#define KI_RUD 0.0
hitonari	0:2b57931c6ed4	29	#define KD_RUD 0.0
hitonari	0:2b57931c6ed4	30	#define KP_AIL 0.11//0.1
hitonari	0:2b57931c6ed4	31	#define KI_AIL 0.2
hitonari	0:2b57931c6ed4	32	#define KD_AIL 0.2
hitonari	0:2b57931c6ed4	33
hitonari	0:2b57931c6ed4	34	//#define g_AIL_L_Ratio_rightloop 0.5
hitonari	0:2b57931c6ed4	35
hitonari	0:2b57931c6ed4	36	#define GAIN_CONTROLVALUE_TO_PWM 3.0
hitonari	0:2b57931c6ed4	37
hitonari	0:2b57931c6ed4	38	#define RIGHT_ROLL 5.0//10
hitonari	0:2b57931c6ed4	39	#define RIGHT_PITCH 45//-20.0//-22//-24.0 //5.0
hitonari	0:2b57931c6ed4	40	#define LEFT_ROLL 15//-38.0//30
hitonari	0:2b57931c6ed4	41
hitonari	0:2b57931c6ed4	42	#define LEFT_PITCH 10//-15.0
hitonari	0:2b57931c6ed4	43	#define STRAIGHT_ROLL 0.0//12.0//8.0
hitonari	0:2b57931c6ed4	44	#define STRAIGHT_PITCH 3.0
hitonari	0:2b57931c6ed4	45	#define TAKEOFF_THR 0.8
hitonari	0:2b57931c6ed4	46	#define LOOP_THR 0.6
hitonari	0:2b57931c6ed4	47
hitonari	0:2b57931c6ed4	48	//#define g_rightloopRUD 1500
hitonari	0:2b57931c6ed4	49
hitonari	0:2b57931c6ed4	50	#define RIGHT_ROLL_SHORT 10.0
hitonari	0:2b57931c6ed4	51	#define RIGHT_PITCH_SHORT -25.0
hitonari	0:2b57931c6ed4	52	#define LEFT_ROLL_SHORT -30.0
hitonari	0:2b57931c6ed4	53	#define LEFT_PITCH_SHORT -20.0
hitonari	0:2b57931c6ed4	54	#define RIGHTLOOPROLL_APPROACH 10.0
hitonari	0:2b57931c6ed4	55	#define LEFTLOOPROLL_APPROACH -30.0
hitonari	0:2b57931c6ed4	56	#define RIGHTLOOPPITCH_APPROACH -25.0
hitonari	0:2b57931c6ed4	57	#define LEFTLOOPPITCH_APPROACH -30.0
hitonari	0:2b57931c6ed4	58
hitonari	0:2b57931c6ed4	59	#define ASCENDING_PITCH -35.0
hitonari	0:2b57931c6ed4	60	#define TAKE_OFF_THROTTLE 1650
hitonari	0:2b57931c6ed4	61	#define TAKE_OFF_PITCH 50//-40.0
hitonari	0:2b57931c6ed4	62
hitonari	0:2b57931c6ed4	63	#define rightloopROLL2 -10.0
hitonari	0:2b57931c6ed4	64
hitonari	0:2b57931c6ed4	65	/*#define rightloopRUD 1300 //1250
hitonari	0:2b57931c6ed4	66	#define rightloopshortRUD 1250
hitonari	0:2b57931c6ed4	67	#define leftloopRUD 1500
hitonari	0:2b57931c6ed4	68	#define leftloopshortRUD 1500
hitonari	0:2b57931c6ed4	69	#define glideloopRUD 1300
hitonari	0:2b57931c6ed4	70	*/
hitonari	0:2b57931c6ed4	71	#define AIL_R_correctionrightloop 0
hitonari	0:2b57931c6ed4	72	#define AIL_L_correctionrightloop 0
hitonari	0:2b57931c6ed4	73	#define AIL_L_correctionrightloopshort 0
hitonari	0:2b57931c6ed4	74	#define AIL_L_correctionleftloop -0
hitonari	0:2b57931c6ed4	75	#define AIL_L_correctionleftloopshort 0
hitonari	0:2b57931c6ed4	76
hitonari	0:2b57931c6ed4	77
hitonari	0:2b57931c6ed4	78	#define RIGHTLOOP_RUD 1190//1150 //1200
hitonari	0:2b57931c6ed4	79	#define RIGHTLOOPSHORT_RUD 1250
hitonari	0:2b57931c6ed4	80	#define LEFTLOOP_RUD 1560 //1680//1750//1700
hitonari	0:2b57931c6ed4	81	#define LEFTLOOPSHORT_RUD 1500
hitonari	0:2b57931c6ed4	82	#define GLIDELOOP_RUD 1300
hitonari	0:2b57931c6ed4	83	#define RIGHTLOOPRUD_APPROACH 1500
hitonari	0:2b57931c6ed4	84	#define LEFTLOOPRUD_APPROACH 1500
hitonari	0:2b57931c6ed4	85	#define RIGHTLOOP_THR_RATE 0.64//0.66
hitonari	0:2b57931c6ed4	86	#define LEFTLOOP_THR_RATE 0.49//0.62//0.66
hitonari	0:2b57931c6ed4	87	#define ASCENDING_THR_RATE 0.75
hitonari	0:2b57931c6ed4	88	#define ASCENDING_ROLL 16.0
hitonari	0:2b57931c6ed4	89	#define ASCENDING_RUD 1200
hitonari	0:2b57931c6ed4	90
hitonari	0:2b57931c6ed4	91
hitonari	0:2b57931c6ed4	92	#define AIL_L_CORRECTION_RIGHTLOOP 0
hitonari	0:2b57931c6ed4	93	#define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
hitonari	0:2b57931c6ed4	94	#define AIL_L_CORRECTION_LEFTLOOP 0
hitonari	0:2b57931c6ed4	95	#define AIL_L_CORRECTION_LEFTLOOPSHORT 0
hitonari	0:2b57931c6ed4	96
hitonari	0:2b57931c6ed4	97
hitonari	0:2b57931c6ed4	98
hitonari	0:2b57931c6ed4	99	#define TRIM1 1480
hitonari	0:2b57931c6ed4	100	#define TRIM2 1500
hitonari	0:2b57931c6ed4	101	#define TRIM3 1150
hitonari	0:2b57931c6ed4	102	#define TRIM4 1500
hitonari	0:2b57931c6ed4	103	#define TRIM5 1300
hitonari	0:2b57931c6ed4	104	#define TRIM6 1500
hitonari	0:2b57931c6ed4	105	#define TRIM_CHECK 0
hitonari	0:2b57931c6ed4	106
hitonari	0:2b57931c6ed4	107	#define GLIDE_ROLL -12.0
hitonari	0:2b57931c6ed4	108	#define GLIDE_PITCH -3.0
hitonari	0:2b57931c6ed4	109
hitonari	0:2b57931c6ed4	110
hitonari	0:2b57931c6ed4	111	#define AIL_L_RatioRising 0.5
hitonari	0:2b57931c6ed4	112	#define AIL_L_RatioDescent 2
hitonari	0:2b57931c6ed4	113
hitonari	0:2b57931c6ed4	114	//コンパスキャリブレーション
hitonari	0:2b57931c6ed4	115	//SkipperS2基板
hitonari	0:2b57931c6ed4	116	/*
hitonari	0:2b57931c6ed4	117	#define MAGBIAS_X -35.0
hitonari	0:2b57931c6ed4	118	#define MAGBIAS_Y 535.0
hitonari	0:2b57931c6ed4	119	#define MAGBIAS_Z -50.0
hitonari	0:2b57931c6ed4	120	*/
hitonari	0:2b57931c6ed4	121	//S2v2 1番基板
hitonari	0:2b57931c6ed4	122	#define MAGBIAS_X 395.0
hitonari	0:2b57931c6ed4	123	#define MAGBIAS_Y 505.0
hitonari	0:2b57931c6ed4	124	#define MAGBIAS_Z -725.0
hitonari	0:2b57931c6ed4	125	//S2v2 2番基板
hitonari	0:2b57931c6ed4	126	/*
hitonari	0:2b57931c6ed4	127	#define MAGBIAS_X 185.0
hitonari	0:2b57931c6ed4	128	#define MAGBIAS_Y 220.0
hitonari	0:2b57931c6ed4	129	#define MAGBIAS_Z -350.0
hitonari	0:2b57931c6ed4	130	*/
hitonari	0:2b57931c6ed4	131
hitonari	0:2b57931c6ed4	132	#define ELEMENT 1
hitonari	0:2b57931c6ed4	133	#define LIMIT_STRAIGHT_YAW 5.0
hitonari	0:2b57931c6ed4	134	#define THRESHOLD_TURNINGRADIUS_YAW 60.0
hitonari	0:2b57931c6ed4	135	#define ALLOWHEIGHT 15
hitonari	0:2b57931c6ed4	136
hitonari	0:2b57931c6ed4	137	//気圧用
hitonari	0:2b57931c6ed4	138	#define SBUS_SIGNAL_OK 0x00
hitonari	0:2b57931c6ed4	139	#define SBUS_SIGNAL_LOST 0x01
hitonari	0:2b57931c6ed4	140	#define SBUS_SIGNAL_FAILSAFE 0x03
hitonari	0:2b57931c6ed4	141	MPU9250 mpu9250;
hitonari	0:2b57931c6ed4	142	float g_sum = 0; //グローバルなので要注意!!
hitonari	0:2b57931c6ed4	143	uint32_t g_sumCount = 0;
hitonari	0:2b57931c6ed4	144	float g_grav = 9.80665;
hitonari	0:2b57931c6ed4	145	float g_st_press = 1018.5;
hitonari	0:2b57931c6ed4	146	float g_press, g_BMPheight;
hitonari	0:2b57931c6ed4	147	int g_BMPheight_int = 0;
hitonari	0:2b57931c6ed4	148	float g_InitPress[101];
hitonari	0:2b57931c6ed4	149	float g_InitPressAve = 0;
hitonari	0:2b57931c6ed4	150	float g_InitBMPHeight[101];
hitonari	0:2b57931c6ed4	151	float g_InitBMPHeightAve = 0;
hitonari	0:2b57931c6ed4	152	int g_temporary;
hitonari	0:2b57931c6ed4	153	float g_dt = 0.0;
hitonari	0:2b57931c6ed4	154	float g_v_prev = 0.0;
hitonari	0:2b57931c6ed4	155	float g_z_prev;
hitonari	0:2b57931c6ed4	156	float g_z_PressHeight, g_z_AccelHeight;
hitonari	0:2b57931c6ed4	157	float g_z_comp;
hitonari	0:2b57931c6ed4	158	float g_tempaz[2];
hitonari	0:2b57931c6ed4	159
hitonari	0:2b57931c6ed4	160	BMP280 bmp(PC_9, PA_8);
hitonari	0:2b57931c6ed4	161	Timer tBMP;
hitonari	0:2b57931c6ed4	162
hitonari	0:2b57931c6ed4	163
hitonari	0:2b57931c6ed4	164	#ifndef PI
hitonari	0:2b57931c6ed4	165	#define PI 3.14159265358979
hitonari	0:2b57931c6ed4	166	#endif
hitonari	0:2b57931c6ed4	167
hitonari	0:2b57931c6ed4	168	const int16_t lengthdivpwm = 320;
hitonari	0:2b57931c6ed4	169	const int16_t changeModeCount = 6;
hitonari	0:2b57931c6ed4	170
hitonari	0:2b57931c6ed4	171
hitonari	0:2b57931c6ed4	172	SBUS sbus(PA_9, PA_10); //SBUS
hitonari	0:2b57931c6ed4	173
hitonari	0:2b57931c6ed4	174	PwmOut servo1(PC_6); // TIM3_CH1 //old echo
hitonari	0:2b57931c6ed4	175	PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
hitonari	0:2b57931c6ed4	176	PwmOut servo3(PB_0); // TIM3_CH3
hitonari	0:2b57931c6ed4	177	PwmOut servo4(PB_1); // TIM3_CH4
hitonari	0:2b57931c6ed4	178	PwmOut servo5(PB_6); // TIM4_CH1
hitonari	0:2b57931c6ed4	179	PwmOut servo6(PB_7); // TIM4_CH2 //old trigger
hitonari	0:2b57931c6ed4	180	//PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo
hitonari	0:2b57931c6ed4	181	//PwmOut servo8(PB_9); // TIM4_CH4 //new trigger
hitonari	0:2b57931c6ed4	182
hitonari	0:2b57931c6ed4	183	RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
hitonari	0:2b57931c6ed4	184	//RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用
hitonari	0:2b57931c6ed4	185	SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
hitonari	0:2b57931c6ed4	186
hitonari	0:2b57931c6ed4	187	DigitalOut led1(PA_0); //黄色のコネクタ
hitonari	0:2b57931c6ed4	188	DigitalOut led2(PA_1);
hitonari	0:2b57931c6ed4	189	DigitalOut led3(PB_5);
hitonari	0:2b57931c6ed4	190	DigitalOut led4(PB_4);
hitonari	0:2b57931c6ed4	191
hitonari	0:2b57931c6ed4	192	//InterruptIn switch2(PC_14);
hitonari	0:2b57931c6ed4	193	MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
hitonari	0:2b57931c6ed4	194	HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8
hitonari	0:2b57931c6ed4	195
hitonari	0:2b57931c6ed4	196	PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
hitonari	0:2b57931c6ed4	197	PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
hitonari	0:2b57931c6ed4	198	PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
hitonari	0:2b57931c6ed4	199
hitonari	0:2b57931c6ed4	200	enum Channel {AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8};
hitonari	0:2b57931c6ed4	201	enum Angle {ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
hitonari	0:2b57931c6ed4	202	enum OperationMode {StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
hitonari	0:2b57931c6ed4	203	enum BombingMode {Takeoff, Chicken, Transition, Approach};
hitonari	0:2b57931c6ed4	204	enum OutputStatus {Manual, Auto};
hitonari	0:2b57931c6ed4	205
hitonari	0:2b57931c6ed4	206	static OutputStatus output_status = Manual;
hitonari	0:2b57931c6ed4	207	OperationMode operation_mode = StartUp;
hitonari	0:2b57931c6ed4	208	BombingMode bombing_mode = Takeoff;
hitonari	0:2b57931c6ed4	209
hitonari	0:2b57931c6ed4	210	static int16_t autopwm[8] = {1514,1435,1180,1512,1176,1800};//6:1848
hitonari	0:2b57931c6ed4	211
hitonari	0:2b57931c6ed4	212	//1号機14SG
hitonari	0:2b57931c6ed4	213
hitonari	0:2b57931c6ed4	214	static int16_t trimpwm[6] = {1428,1646,1176,1476,1344,1478};//1500
hitonari	0:2b57931c6ed4	215	int16_t maxpwm[6] = {1786,1848,1848,1889,1520,1848};
hitonari	0:2b57931c6ed4	216	int16_t minpwm[6] = {1114,1384,500,1293,1280,1176};///3-1376
hitonari	0:2b57931c6ed4	217	const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
hitonari	0:2b57931c6ed4	218
hitonari	0:2b57931c6ed4	219
hitonari	0:2b57931c6ed4	220	//2号機
hitonari	0:2b57931c6ed4	221	/*
hitonari	0:2b57931c6ed4	222	static int16_t trimpwm[6] = {1508,1559,1184,1664,1072,1952};
hitonari	0:2b57931c6ed4	223	int16_t maxpwm[6] = {1840,1884,1840,1992,1728,1952};
hitonari	0:2b57931c6ed4	224	int16_t minpwm[6] = {1183,1228,1176,1420,1420,1072};
hitonari	0:2b57931c6ed4	225	const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
hitonari	0:2b57931c6ed4	226	*/
hitonari	0:2b57931c6ed4	227
hitonari	0:2b57931c6ed4	228	int16_t oldTHR = 1000;
hitonari	0:2b57931c6ed4	229
hitonari	0:2b57931c6ed4	230	int16_t g_AIL_L_Ratio_rightloop = 0.5;
hitonari	0:2b57931c6ed4	231
hitonari	0:2b57931c6ed4	232
hitonari	0:2b57931c6ed4	233	static float nowAngle[3] = {0,0,0};
hitonari	0:2b57931c6ed4	234	const float trimAngle[3] = {0.0, 0.0, 0.0};
hitonari	0:2b57931c6ed4	235	const float maxAngle[2] = {90, 90};
hitonari	0:2b57931c6ed4	236	const float minAngle[2] = {-90, -90};
hitonari	0:2b57931c6ed4	237
hitonari	0:2b57931c6ed4	238	float FirstROLL = 0.0, FirstPITCH = 0.0,FirstYAW = 0.0;
hitonari	0:2b57931c6ed4	239
hitonari	0:2b57931c6ed4	240	unsigned int g_distance;
hitonari	0:2b57931c6ed4	241	Ticker USsensor;
hitonari	0:2b57931c6ed4	242	static char g_buf[16];
hitonari	0:2b57931c6ed4	243	char g_landingcommand='T';
hitonari	0:2b57931c6ed4	244	float g_SerialTargetYAW;
hitonari	0:2b57931c6ed4	245
hitonari	0:2b57931c6ed4	246
hitonari	0:2b57931c6ed4	247	Timer t;
hitonari	0:2b57931c6ed4	248	Timer t2;
hitonari	0:2b57931c6ed4	249	Timeout RerurnChickenServo1;
hitonari	0:2b57931c6ed4	250	Timeout RerurnChickenServo2;
hitonari	0:2b57931c6ed4	251
hitonari	0:2b57931c6ed4	252	/-----関数のプロトタイプ宣言-----/
hitonari	0:2b57931c6ed4	253	void setup();
hitonari	0:2b57931c6ed4	254	void loop();
hitonari	0:2b57931c6ed4	255
hitonari	0:2b57931c6ed4	256	void Init_PWM();
hitonari	0:2b57931c6ed4	257	void Init_servo(); //サーボ初期化
hitonari	0:2b57931c6ed4	258	void Init_sbus(); //SBUS初期化
hitonari	0:2b57931c6ed4	259	void Init_sensors();
hitonari	0:2b57931c6ed4	260	void DisplayClock(); //クロック状態確認
hitonari	0:2b57931c6ed4	261	void SetupBMP280();
hitonari	0:2b57931c6ed4	262	void Set_trim();
hitonari	0:2b57931c6ed4	263
hitonari	0:2b57931c6ed4	264	//センサの値取得
hitonari	0:2b57931c6ed4	265	void SensingMPU();
hitonari	0:2b57931c6ed4	266	void UpdateDist();
hitonari	0:2b57931c6ed4	267	void sensing();
hitonari	0:2b57931c6ed4	268	void getBMP280();
hitonari	0:2b57931c6ed4	269
hitonari	0:2b57931c6ed4	270	//void offsetRollPitch(float FirstROLL, float FirstPITCH);
hitonari	0:2b57931c6ed4	271	//void TransYaw(float FirstYAW);
hitonari	0:2b57931c6ed4	272	float TranslateNewYaw(float beforeYaw, float newzeroYaw);
hitonari	0:2b57931c6ed4	273	void UpdateTargetAngle(float targetAngle[3]);
hitonari	0:2b57931c6ed4	274	void CalculateControlValue(float targetAngle[3], float controlValue[3]);
hitonari	0:2b57931c6ed4	275	void UpdateAutoPWM(float controlValue[3]);
hitonari	0:2b57931c6ed4	276	void ConvertPWMintoRAD(float targetAngle[3]);
hitonari	0:2b57931c6ed4	277	inline float CalcRatio(float value, float trim, float limit);
hitonari	0:2b57931c6ed4	278	bool CheckSW_Up(Channel ch);
hitonari	0:2b57931c6ed4	279	int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
hitonari	0:2b57931c6ed4	280	inline int16_t SetTHRinRatio(float ratio);
hitonari	0:2b57931c6ed4	281	void Ascending(float targetAngle[3]);
hitonari	0:2b57931c6ed4	282
hitonari	0:2b57931c6ed4	283	//sbus割り込み
hitonari	0:2b57931c6ed4	284	void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
hitonari	0:2b57931c6ed4	285	void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力
hitonari	0:2b57931c6ed4	286
hitonari	0:2b57931c6ed4	287	//シリアル割り込み
hitonari	0:2b57931c6ed4	288	void getSF_Serial();
hitonari	0:2b57931c6ed4	289	float ConvertByteintoFloat(char high, char low);
hitonari	0:2b57931c6ed4	290
hitonari	0:2b57931c6ed4	291	bool g_Finflag = false;
hitonari	0:2b57931c6ed4	292
hitonari	0:2b57931c6ed4	293
hitonari	0:2b57931c6ed4	294	//SD設定
hitonari	0:2b57931c6ed4	295	int GetParameter(FILE fp, const char paramName,char parameter[]);
hitonari	0:2b57931c6ed4	296	int SetOptions(float g_kpELE, float g_kiELE, float *g_kdELE,
hitonari	0:2b57931c6ed4	297	float g_kpRUD, float g_kiRUD, float *g_kdRUD,
hitonari	0:2b57931c6ed4	298	float g_rightloopROLL, float g_rightloopPITCH,
hitonari	0:2b57931c6ed4	299	float g_leftloopROLL, float g_leftloopPITCH,
hitonari	0:2b57931c6ed4	300	float g_gostraightROLL, float g_gostraightPITCH,
hitonari	0:2b57931c6ed4	301	float g_takeoffTHR, float g_loopTHR,
hitonari	0:2b57931c6ed4	302	float g_rightloopROLLshort, float g_rightloopPITCHshort,
hitonari	0:2b57931c6ed4	303	float g_leftloopROLLshort, float g_leftloopPITCHshort,
hitonari	0:2b57931c6ed4	304	float g_glideloopROLL, float g_glideloopPITCH,
hitonari	0:2b57931c6ed4	305	float g_kpAIL, float g_kiAIL, float *g_kdAIL,
hitonari	0:2b57931c6ed4	306	int g_rightloopRUD, int g_rightloopshortRUD,
hitonari	0:2b57931c6ed4	307	int g_leftloopRUD, int g_leftloopshortRUD,
hitonari	0:2b57931c6ed4	308	int *g_glideRUD,
hitonari	0:2b57931c6ed4	309	float g_rightloopROLL_approach, float g_leftloopROLL_approach,
hitonari	0:2b57931c6ed4	310	int g_rightloopRUD_approach, int g_leftloopRUD_approach,
hitonari	0:2b57931c6ed4	311	float g_rightloopPITCH_approach, float g_leftloopPITCH_approach,
hitonari	0:2b57931c6ed4	312	float *g_ascendingPITCH,
hitonari	0:2b57931c6ed4	313	int g_take_off_THROTTLE, float g_take_off_PITCH,
hitonari	0:2b57931c6ed4	314	float g_rightloop_THR_rate, float g_leftloop_THR_rate,
hitonari	0:2b57931c6ed4	315	float *g_ascendding_thr_rate,
hitonari	0:2b57931c6ed4	316	float g_ascending_ROLL, int g_ascending_RUD,
hitonari	0:2b57931c6ed4	317	int g_trim1, int g_trim2, int *g_trim3,
hitonari	0:2b57931c6ed4	318	int g_trim4, int g_trim5, int *g_trim6,
hitonari	0:2b57931c6ed4	319	bool *g_trim_check
hitonari	0:2b57931c6ed4	320	);
hitonari	0:2b57931c6ed4	321	//switch2割り込み
hitonari	0:2b57931c6ed4	322	void ResetTrim();
hitonari	0:2b57931c6ed4	323
hitonari	0:2b57931c6ed4	324	//自動操縦
hitonari	0:2b57931c6ed4	325	void UpdateTargetAngle_GoStraight(float targetAngle[3]);
hitonari	0:2b57931c6ed4	326	void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]); //着陸時にスロットルが０の時の直進
hitonari	0:2b57931c6ed4	327	void UpdateTargetAngle_Rightloop(float targetAngle[3]);
hitonari	0:2b57931c6ed4	328	void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
hitonari	0:2b57931c6ed4	329	void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の右旋回
hitonari	0:2b57931c6ed4	330	void UpdateTargetAngle_Rightloop_UP(float targetAngle[3]);
hitonari	0:2b57931c6ed4	331	void UpdateTargetAngle_Leftloop(float targetAngle[3]);
hitonari	0:2b57931c6ed4	332	void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
hitonari	0:2b57931c6ed4	333	void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の左旋回
hitonari	0:2b57931c6ed4	334	void UpdateTargetAngle_Moebius(float targetAngle[3]);
hitonari	0:2b57931c6ed4	335	void UpdateTargetAngle_Takeoff(float targetAngle[3]);
hitonari	0:2b57931c6ed4	336	void UpdateTargetAngle_Approach(float targetAngle[3]);
hitonari	0:2b57931c6ed4	337	void Take_off_and_landing(float targetAngle[3]);
hitonari	0:2b57931c6ed4	338
hitonari	0:2b57931c6ed4	339
hitonari	0:2b57931c6ed4	340
hitonari	0:2b57931c6ed4	341	int Rotate(float targetAngle[3], float TargetYAW);
hitonari	0:2b57931c6ed4	342	int Rotate_zero(float targetAngle[3], float TargetYAW);
hitonari	0:2b57931c6ed4	343
hitonari	0:2b57931c6ed4	344	//投下
hitonari	0:2b57931c6ed4	345	void Chicken_Drop();
hitonari	0:2b57931c6ed4	346	void ReturnChickenServo1();
hitonari	0:2b57931c6ed4	347	void ReturnChickenServo2();
hitonari	0:2b57931c6ed4	348
hitonari	0:2b57931c6ed4	349	//超音波による高度補正
hitonari	0:2b57931c6ed4	350	void checkHeight(float targetAngle[3]);
hitonari	0:2b57931c6ed4	351	void UpdateTargetAngle_NoseUP(float targetAngle[3]);
hitonari	0:2b57931c6ed4	352	void UpdateTargetAngle_NoseDOWN(float targetAngle[3]);
hitonari	0:2b57931c6ed4	353
hitonari	0:2b57931c6ed4	354	//デバッグ用
hitonari	0:2b57931c6ed4	355	void Sbusprintf();
hitonari	0:2b57931c6ed4	356	void DebugPrint();
hitonari	0:2b57931c6ed4	357
hitonari	0:2b57931c6ed4	358	/---関数のプロトタイプ宣言終わり---/
hitonari	0:2b57931c6ed4	359
hitonari	0:2b57931c6ed4	360	int main()
hitonari	0:2b57931c6ed4	361	{
hitonari	0:2b57931c6ed4	362	setup();
hitonari	0:2b57931c6ed4	363
hitonari	0:2b57931c6ed4	364
hitonari	0:2b57931c6ed4	365	while(1) {
hitonari	0:2b57931c6ed4	366
hitonari	0:2b57931c6ed4	367	loop();
hitonari	0:2b57931c6ed4	368
hitonari	0:2b57931c6ed4	369
hitonari	0:2b57931c6ed4	370	NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	371	if(!CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	372	led3=0;
hitonari	0:2b57931c6ed4	373
hitonari	0:2b57931c6ed4	374	} else {
hitonari	0:2b57931c6ed4	375	led3=1;
hitonari	0:2b57931c6ed4	376	}
hitonari	0:2b57931c6ed4	377	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	378	}
hitonari	0:2b57931c6ed4	379
hitonari	0:2b57931c6ed4	380	}
hitonari	0:2b57931c6ed4	381
hitonari	0:2b57931c6ed4	382	void setup()
hitonari	0:2b57931c6ed4	383	{
hitonari	0:2b57931c6ed4	384	//buzzer = 0;
hitonari	0:2b57931c6ed4	385	led1 = 1;
hitonari	0:2b57931c6ed4	386	led2 = 1;
hitonari	0:2b57931c6ed4	387	led3 = 1;
hitonari	0:2b57931c6ed4	388	led4 = 1;
hitonari	0:2b57931c6ed4	389
hitonari	0:2b57931c6ed4	390	SetOptions(&g_kpELE, &g_kiELE, &g_kdELE,
hitonari	0:2b57931c6ed4	391	&g_kpRUD, &g_kiRUD, &g_kdRUD,
hitonari	0:2b57931c6ed4	392	&g_rightloopROLL, &g_rightloopPITCH,
hitonari	0:2b57931c6ed4	393	&g_leftloopROLL, &g_leftloopPITCH,
hitonari	0:2b57931c6ed4	394	&g_gostraightROLL, &g_gostraightPITCH,
hitonari	0:2b57931c6ed4	395	&g_takeoffTHR, &g_loopTHR,
hitonari	0:2b57931c6ed4	396	&g_rightloopROLLshort, &g_rightloopPITCHshort,
hitonari	0:2b57931c6ed4	397	&g_leftloopROLLshort, &g_leftloopPITCHshort,
hitonari	0:2b57931c6ed4	398	&g_glideloopROLL, &g_glideloopPITCH,
hitonari	0:2b57931c6ed4	399	&g_kpAIL, &g_kiAIL,&g_kdAIL,
hitonari	0:2b57931c6ed4	400	&g_rightloopRUD, &g_rightloopshortRUD,
hitonari	0:2b57931c6ed4	401	&g_leftloopRUD, &g_leftloopshortRUD,
hitonari	0:2b57931c6ed4	402	&g_glideloopRUD,
hitonari	0:2b57931c6ed4	403	&g_rightloopROLL_approach,&g_leftloopROLL_approach,
hitonari	0:2b57931c6ed4	404	&g_rightloopRUD_approach,&g_leftloopRUD_approach,
hitonari	0:2b57931c6ed4	405	&g_rightloopPITCH_approach,&g_leftloopPITCH_approach,
hitonari	0:2b57931c6ed4	406	&g_ascendingPITCH,
hitonari	0:2b57931c6ed4	407	&g_take_off_THROTTLE, &g_take_off_PITCH,
hitonari	0:2b57931c6ed4	408	&g_rightloop_THR_rate, &g_leftloop_THR_rate,
hitonari	0:2b57931c6ed4	409	&g_ascending_THR_rate,
hitonari	0:2b57931c6ed4	410	&g_ascending_ROLL, &g_ascending_RUD,
hitonari	0:2b57931c6ed4	411	&g_trim1, &g_trim2, &g_trim3,
hitonari	0:2b57931c6ed4	412	&g_trim4, &g_trim5, &g_trim6,
hitonari	0:2b57931c6ed4	413	&g_trim_check
hitonari	0:2b57931c6ed4	414	);
hitonari	0:2b57931c6ed4	415
hitonari	0:2b57931c6ed4	416	Set_trim();
hitonari	0:2b57931c6ed4	417
hitonari	0:2b57931c6ed4	418
hitonari	0:2b57931c6ed4	419
hitonari	0:2b57931c6ed4	420	Init_PWM();
hitonari	0:2b57931c6ed4	421	Init_servo();
hitonari	0:2b57931c6ed4	422	Init_sbus();
hitonari	0:2b57931c6ed4	423	Init_sensors();
hitonari	0:2b57931c6ed4	424	//switch2.rise(ResetTrim);
hitonari	0:2b57931c6ed4	425
hitonari	0:2b57931c6ed4	426	USsensor.attach(&UpdateDist, 0.05);
hitonari	0:2b57931c6ed4	427
hitonari	0:2b57931c6ed4	428	NVIC_SetPriority(USART1_IRQn,0);//プロポ
hitonari	0:2b57931c6ed4	429	NVIC_SetPriority(EXTI0_IRQn,1);//MPU割り込み禁止
hitonari	0:2b57931c6ed4	430	NVIC_SetPriority(TIM5_IRQn,2);
hitonari	0:2b57931c6ed4	431	NVIC_SetPriority(EXTI9_5_IRQn,3);
hitonari	0:2b57931c6ed4	432	NVIC_SetPriority(EXTI4_IRQn,5);
hitonari	0:2b57931c6ed4	433	DisplayClock();
hitonari	0:2b57931c6ed4	434	t.start();
hitonari	0:2b57931c6ed4	435	t.start();
hitonari	0:2b57931c6ed4	436
hitonari	0:2b57931c6ed4	437
hitonari	0:2b57931c6ed4	438	pc.printf("MPU calibration start\r\n");
hitonari	0:2b57931c6ed4	439
hitonari	0:2b57931c6ed4	440	float offsetstart = t.read();
hitonari	0:2b57931c6ed4	441	while(t.read() - offsetstart < 26) {
hitonari	0:2b57931c6ed4	442	SensingMPU();
hitonari	0:2b57931c6ed4	443	for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
hitonari	0:2b57931c6ed4	444	pc.printf("\r\n");
hitonari	0:2b57931c6ed4	445	led1 = !led1;
hitonari	0:2b57931c6ed4	446	led2 = !led2;
hitonari	0:2b57931c6ed4	447	led3 = !led3;
hitonari	0:2b57931c6ed4	448	led4 = !led4;
hitonari	0:2b57931c6ed4	449	}
hitonari	0:2b57931c6ed4	450	//pc.attach(getSF_Serial, Serial::RxIrq);
hitonari	0:2b57931c6ed4	451	NVIC_SetPriority(USART2_IRQn,4);//シリアル
hitonari	0:2b57931c6ed4	452
hitonari	0:2b57931c6ed4	453	FirstROLL = nowAngle[ROLL];
hitonari	0:2b57931c6ed4	454	FirstPITCH = nowAngle[PITCH];
hitonari	0:2b57931c6ed4	455	nowAngle[ROLL] -=FirstROLL;
hitonari	0:2b57931c6ed4	456	nowAngle[PITCH] -=FirstPITCH;
hitonari	0:2b57931c6ed4	457
hitonari	0:2b57931c6ed4	458	SetupBMP280();
hitonari	0:2b57931c6ed4	459
hitonari	0:2b57931c6ed4	460	// NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	461	//
hitonari	0:2b57931c6ed4	462	// if(pc.readable()) { // 受信確認
hitonari	0:2b57931c6ed4	463	//
hitonari	0:2b57931c6ed4	464	// static char SFbuf[16]= {'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
hitonari	0:2b57931c6ed4	465	// SFbuf[0] = pc.getc(); // 1文字取り出し
hitonari	0:2b57931c6ed4	466	// if(SFbuf[0]=='S') {
hitonari	0:2b57931c6ed4	467	// for(int i=0;i<3;i++){
hitonari	0:2b57931c6ed4	468	// led1 = 1;
hitonari	0:2b57931c6ed4	469	// led2 = 1;
hitonari	0:2b57931c6ed4	470	// led3 = 1;
hitonari	0:2b57931c6ed4	471	// led4 = 1;
hitonari	0:2b57931c6ed4	472	// wait(0.2);
hitonari	0:2b57931c6ed4	473	// }
hitonari	0:2b57931c6ed4	474	// }
hitonari	0:2b57931c6ed4	475	// }
hitonari	0:2b57931c6ed4	476	// NVIC_DisableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	477
hitonari	0:2b57931c6ed4	478
hitonari	0:2b57931c6ed4	479	led1 = 0;
hitonari	0:2b57931c6ed4	480	led2 = 0;
hitonari	0:2b57931c6ed4	481	led3 = 0;
hitonari	0:2b57931c6ed4	482	led4 = 0;
hitonari	0:2b57931c6ed4	483	wait(0.2);
hitonari	0:2b57931c6ed4	484
hitonari	0:2b57931c6ed4	485
hitonari	0:2b57931c6ed4	486	pc.printf("All initialized\r\n");
hitonari	0:2b57931c6ed4	487	}
hitonari	0:2b57931c6ed4	488
hitonari	0:2b57931c6ed4	489	void Set_trim(){
hitonari	0:2b57931c6ed4	490	trimpwm[0] = g_trim1;
hitonari	0:2b57931c6ed4	491	trimpwm[1] = g_trim2;
hitonari	0:2b57931c6ed4	492	trimpwm[2] = g_trim3;
hitonari	0:2b57931c6ed4	493	trimpwm[3] = g_trim4;
hitonari	0:2b57931c6ed4	494	trimpwm[4] = g_trim5;
hitonari	0:2b57931c6ed4	495	trimpwm[5] = g_trim6;
hitonari	0:2b57931c6ed4	496
hitonari	0:2b57931c6ed4	497	maxpwm[0] = 1768;//trimpwm[0] + 330;
hitonari	0:2b57931c6ed4	498	maxpwm[1] = 1964;//trimpwm[1] + 330;
hitonari	0:2b57931c6ed4	499	maxpwm[2] = 1848;//trimpwm[2] + 660;
hitonari	0:2b57931c6ed4	500	maxpwm[3] = 1825;//trimpwm[3] + 330;
hitonari	0:2b57931c6ed4	501	maxpwm[4] = 1914;//trimpwm[4] + 660;
hitonari	0:2b57931c6ed4	502	maxpwm[5] = 1816;//trimpwm[5] + 330;
hitonari	0:2b57931c6ed4	503
hitonari	0:2b57931c6ed4	504	minpwm[0]= 1186;//trimpwm[0] - 330;
hitonari	0:2b57931c6ed4	505	minpwm[1]= 1314;//trimpwm[1] - 330;
hitonari	0:2b57931c6ed4	506	minpwm[2]= trimpwm[2] - 0;
hitonari	0:2b57931c6ed4	507	minpwm[3]= 1154;//trimpwm[3] - 330;
hitonari	0:2b57931c6ed4	508	minpwm[4]= 1344;//trimpwm[4] - 0;
hitonari	0:2b57931c6ed4	509	minpwm[5]= 1144;//trimpwm[5] - 330;
hitonari	0:2b57931c6ed4	510	}
hitonari	0:2b57931c6ed4	511
hitonari	0:2b57931c6ed4	512	void loop()
hitonari	0:2b57931c6ed4	513	{
hitonari	0:2b57931c6ed4	514	static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
hitonari	0:2b57931c6ed4	515	SensingMPU();
hitonari	0:2b57931c6ed4	516	UpdateTargetAngle(targetAngle);
hitonari	0:2b57931c6ed4	517	CalculateControlValue(targetAngle, controlValue);
hitonari	0:2b57931c6ed4	518	UpdateAutoPWM(controlValue);
hitonari	0:2b57931c6ed4	519
hitonari	0:2b57931c6ed4	520
hitonari	0:2b57931c6ed4	521	//NVIC_SetPriority(TIM5_IRQn,4);
hitonari	0:2b57931c6ed4	522	//NVIC_SetPriority(USART2_IRQn,2);
hitonari	0:2b57931c6ed4	523
hitonari	0:2b57931c6ed4	524	//wait_ms(23);
hitonari	0:2b57931c6ed4	525	wait_ms(30);
hitonari	0:2b57931c6ed4	526
hitonari	0:2b57931c6ed4	527	//NVIC_SetPriority(TIM5_IRQn,2);
hitonari	0:2b57931c6ed4	528	//NVIC_SetPriority(USART2_IRQn,4);
hitonari	0:2b57931c6ed4	529
hitonari	0:2b57931c6ed4	530
hitonari	0:2b57931c6ed4	531	// pc.printf("6\r\n");
hitonari	0:2b57931c6ed4	532	//NVIC_DisableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	533	//pc.printf("%c",g_landingcommand);
hitonari	0:2b57931c6ed4	534	//NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	535	#if DEBUG_PRINT_INLOOP
hitonari	0:2b57931c6ed4	536	//Sbusprintf();
hitonari	0:2b57931c6ed4	537	DebugPrint();
hitonari	0:2b57931c6ed4	538	#endif
hitonari	0:2b57931c6ed4	539	}
hitonari	0:2b57931c6ed4	540
hitonari	0:2b57931c6ed4	541	//サーボ初期化関数
hitonari	0:2b57931c6ed4	542	void Init_servo()
hitonari	0:2b57931c6ed4	543	{
hitonari	0:2b57931c6ed4	544	servo1.period_ms(16);
hitonari	0:2b57931c6ed4	545	servo1.pulsewidth_us(trimpwm[AIL_R]);
hitonari	0:2b57931c6ed4	546
hitonari	0:2b57931c6ed4	547	servo2.period_ms(16);
hitonari	0:2b57931c6ed4	548	servo2.pulsewidth_us(trimpwm[ELE]);
hitonari	0:2b57931c6ed4	549
hitonari	0:2b57931c6ed4	550	servo3.period_ms(16);
hitonari	0:2b57931c6ed4	551	servo3.pulsewidth_us(trimpwm[THR]);
hitonari	0:2b57931c6ed4	552
hitonari	0:2b57931c6ed4	553	servo4.period_ms(16);
hitonari	0:2b57931c6ed4	554	servo4.pulsewidth_us(trimpwm[RUD]);
hitonari	0:2b57931c6ed4	555
hitonari	0:2b57931c6ed4	556	servo5.period_ms(16);
hitonari	0:2b57931c6ed4	557	servo5.pulsewidth_us(trimpwm[DROP]);
hitonari	0:2b57931c6ed4	558
hitonari	0:2b57931c6ed4	559	servo6.period_ms(16);
hitonari	0:2b57931c6ed4	560	servo6.pulsewidth_us(trimpwm[AIL_L]);
hitonari	0:2b57931c6ed4	561
hitonari	0:2b57931c6ed4	562	int i = 0;
hitonari	0:2b57931c6ed4	563
hitonari	0:2b57931c6ed4	564	for(i = 0;i<6;i++ ){
hitonari	0:2b57931c6ed4	565	autopwm[i]=trimpwm[i];
hitonari	0:2b57931c6ed4	566	}
hitonari	0:2b57931c6ed4	567
hitonari	0:2b57931c6ed4	568	pc.printf("servo initialized\r\n");
hitonari	0:2b57931c6ed4	569	}
hitonari	0:2b57931c6ed4	570
hitonari	0:2b57931c6ed4	571	//Sbus初期化
hitonari	0:2b57931c6ed4	572	void Init_sbus()
hitonari	0:2b57931c6ed4	573	{
hitonari	0:2b57931c6ed4	574	sbus.initialize();
hitonari	0:2b57931c6ed4	575	sbus.setLastfuncPoint(Update_PWM);
hitonari	0:2b57931c6ed4	576	sbus.startInterrupt();
hitonari	0:2b57931c6ed4	577	}
hitonari	0:2b57931c6ed4	578
hitonari	0:2b57931c6ed4	579	void Init_sensors()
hitonari	0:2b57931c6ed4	580	{
hitonari	0:2b57931c6ed4	581	if(mpu6050.setup() == -1) {
hitonari	0:2b57931c6ed4	582	pc.printf("failed initialize\r\n");
hitonari	0:2b57931c6ed4	583	while(1) {
hitonari	0:2b57931c6ed4	584	led1 = 1;
hitonari	0:2b57931c6ed4	585	led2 = 0;
hitonari	0:2b57931c6ed4	586	led3 = 1;
hitonari	0:2b57931c6ed4	587	led4 = 0;
hitonari	0:2b57931c6ed4	588	wait(1);
hitonari	0:2b57931c6ed4	589	led1 = 0;
hitonari	0:2b57931c6ed4	590	led2 = 1;
hitonari	0:2b57931c6ed4	591	led3 = 0;
hitonari	0:2b57931c6ed4	592	led4 = 1;
hitonari	0:2b57931c6ed4	593	wait(1);
hitonari	0:2b57931c6ed4	594	}
hitonari	0:2b57931c6ed4	595	}
hitonari	0:2b57931c6ed4	596	}
hitonari	0:2b57931c6ed4	597
hitonari	0:2b57931c6ed4	598	void Init_PWM()
hitonari	0:2b57931c6ed4	599	{
hitonari	0:2b57931c6ed4	600	pc.printf("PWM initialized\r\n");
hitonari	0:2b57931c6ed4	601	}
hitonari	0:2b57931c6ed4	602
hitonari	0:2b57931c6ed4	603	void DisplayClock()
hitonari	0:2b57931c6ed4	604	{
hitonari	0:2b57931c6ed4	605	pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
hitonari	0:2b57931c6ed4	606	pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
hitonari	0:2b57931c6ed4	607	pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
hitonari	0:2b57931c6ed4	608	pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
hitonari	0:2b57931c6ed4	609	pc.printf("\r\n");
hitonari	0:2b57931c6ed4	610	}
hitonari	0:2b57931c6ed4	611
hitonari	0:2b57931c6ed4	612	void UpdateTargetAngle(float targetAngle[3])
hitonari	0:2b57931c6ed4	613	{
hitonari	0:2b57931c6ed4	614
hitonari	0:2b57931c6ed4	615
hitonari	0:2b57931c6ed4	616	static int16_t count_op = 0;
hitonari	0:2b57931c6ed4	617	#if DEBUG_SEMIAUTO
hitonari	0:2b57931c6ed4	618	switch(operation_mode) {
hitonari	0:2b57931c6ed4	619	case StartUp:
hitonari	0:2b57931c6ed4	620	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	621	count_op++;B
hitonari	0:2b57931c6ed4	622	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	623	operation_mode = SemiAuto;
hitonari	0:2b57931c6ed4	624	pc.printf("Goto SemiAuto mode\r\n");
hitonari	0:2b57931c6ed4	625	count_op = 0;
hitonari	0:2b57931c6ed4	626	}
hitonari	0:2b57931c6ed4	627	} else count_op = 0;
hitonari	0:2b57931c6ed4	628	break;
hitonari	0:2b57931c6ed4	629
hitonari	0:2b57931c6ed4	630	case SemiAuto:
hitonari	0:2b57931c6ed4	631	/* 大会用では以下のif文を入れてoperation_modeを変える
hitonari	0:2b57931c6ed4	632	if(CheckSW_Up(Ch6)){
hitonari	0:2b57931c6ed4	633	count_op++;
hitonari	0:2b57931c6ed4	634	if(count_op>changeModeCount){
hitonari	0:2b57931c6ed4	635	output_status = XXX;
hitonari	0:2b57931c6ed4	636	led2 = 0;
hitonari	0:2b57931c6ed4	637	pc.printf("Goto XXX mode\r\n");
hitonari	0:2b57931c6ed4	638	count_op = 0;
hitonari	0:2b57931c6ed4	639	}else count_op = 0;
hitonari	0:2b57931c6ed4	640	ConvertPWMintoRAD(targetAngle);
hitonari	0:2b57931c6ed4	641	}
hitonari	0:2b57931c6ed4	642	*/
hitonari	0:2b57931c6ed4	643	ConvertPWMintoRAD(targetAngle);
hitonari	0:2b57931c6ed4	644	break;
hitonari	0:2b57931c6ed4	645
hitonari	0:2b57931c6ed4	646	default:
hitonari	0:2b57931c6ed4	647	operation_mode = SemiAuto;
hitonari	0:2b57931c6ed4	648	break;
hitonari	0:2b57931c6ed4	649	}
hitonari	0:2b57931c6ed4	650
hitonari	0:2b57931c6ed4	651	#else
hitonari	0:2b57931c6ed4	652
hitonari	0:2b57931c6ed4	653	switch(operation_mode) {
hitonari	0:2b57931c6ed4	654	case StartUp:
hitonari	0:2b57931c6ed4	655	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) { //ch7;自動・手動切り替え　ch8;自動操縦モード切替
hitonari	0:2b57931c6ed4	656	count_op++;
hitonari	0:2b57931c6ed4	657	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	658	operation_mode = LeftLoop;
hitonari	0:2b57931c6ed4	659	pc.printf("Goto LeftLoop mode\r\n");
hitonari	0:2b57931c6ed4	660	count_op = 0;
hitonari	0:2b57931c6ed4	661	}
hitonari	0:2b57931c6ed4	662	} else count_op = 0;
hitonari	0:2b57931c6ed4	663	break;
hitonari	0:2b57931c6ed4	664
hitonari	0:2b57931c6ed4	665	case LeftLoop:
hitonari	0:2b57931c6ed4	666	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	667	count_op++;
hitonari	0:2b57931c6ed4	668	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	669	operation_mode = GoStraight;
hitonari	0:2b57931c6ed4	670	pc.printf("Goto GoStraight mode\r\n");
hitonari	0:2b57931c6ed4	671	count_op = 0;
hitonari	0:2b57931c6ed4	672	}
hitonari	0:2b57931c6ed4	673	} else count_op = 0;
hitonari	0:2b57931c6ed4	674	UpdateTargetAngle_Leftloop(targetAngle);
hitonari	0:2b57931c6ed4	675	//UpdateTargetAngle_GoStraight_zero(targetAngle);
hitonari	0:2b57931c6ed4	676	break;
hitonari	0:2b57931c6ed4	677
hitonari	0:2b57931c6ed4	678	case GoStraight:
hitonari	0:2b57931c6ed4	679	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	680	count_op++;
hitonari	0:2b57931c6ed4	681	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	682	operation_mode = Moebius;
hitonari	0:2b57931c6ed4	683	pc.printf("Goto Moebius mode\r\n");
hitonari	0:2b57931c6ed4	684	count_op = 0;
hitonari	0:2b57931c6ed4	685	}
hitonari	0:2b57931c6ed4	686	} else count_op = 0;
hitonari	0:2b57931c6ed4	687	//UpdateTargetAngle_GoStraight(targetAngle);
hitonari	0:2b57931c6ed4	688	UpdateTargetAngle_GoStraight_zero(targetAngle);
hitonari	0:2b57931c6ed4	689	//UpdateTargetAngle_Leftloop_short(targetAngle);
hitonari	0:2b57931c6ed4	690	//UpdateTargetAngle_Leftloop_zero(targetAngle);
hitonari	0:2b57931c6ed4	691
hitonari	0:2b57931c6ed4	692
hitonari	0:2b57931c6ed4	693	break;
hitonari	0:2b57931c6ed4	694
hitonari	0:2b57931c6ed4	695	case Moebius:
hitonari	0:2b57931c6ed4	696	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	697	count_op++;
hitonari	0:2b57931c6ed4	698	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	699	operation_mode = Glide;
hitonari	0:2b57931c6ed4	700	pc.printf("Goto Glide mode\r\n");
hitonari	0:2b57931c6ed4	701	count_op = 0;
hitonari	0:2b57931c6ed4	702	}
hitonari	0:2b57931c6ed4	703	} else count_op = 0;
hitonari	0:2b57931c6ed4	704	UpdateTargetAngle_Moebius(targetAngle);
hitonari	0:2b57931c6ed4	705	break;
hitonari	0:2b57931c6ed4	706
hitonari	0:2b57931c6ed4	707	case Glide:
hitonari	0:2b57931c6ed4	708	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	709	count_op++;
hitonari	0:2b57931c6ed4	710	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	711	operation_mode = RightLoop;
hitonari	0:2b57931c6ed4	712	pc.printf("Goto RightLoop mode\r\n");
hitonari	0:2b57931c6ed4	713	count_op = 0;
hitonari	0:2b57931c6ed4	714	}
hitonari	0:2b57931c6ed4	715	} else count_op = 0;
hitonari	0:2b57931c6ed4	716	//UpdateTargetAngle_Rightloop_short(targetAngle);
hitonari	0:2b57931c6ed4	717	//UpdateTargetAngle_Rightloop_zero(targetAngle);
hitonari	0:2b57931c6ed4	718	UpdateTargetAngle_GoStraight_zero(targetAngle);
hitonari	0:2b57931c6ed4	719
hitonari	0:2b57931c6ed4	720	break;
hitonari	0:2b57931c6ed4	721
hitonari	0:2b57931c6ed4	722	case RightLoop:
hitonari	0:2b57931c6ed4	723	if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	724	count_op++;
hitonari	0:2b57931c6ed4	725	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	726	operation_mode = BombwithPC;
hitonari	0:2b57931c6ed4	727	pc.attach(getSF_Serial, Serial::RxIrq);
hitonari	0:2b57931c6ed4	728	autopwm[DROP]=trimpwm[DROP];
hitonari	0:2b57931c6ed4	729	pc.printf("Goto BombwithPC mode\r\n");
hitonari	0:2b57931c6ed4	730	//g_AscendingFlag = false;
hitonari	0:2b57931c6ed4	731	count_op = 0;
hitonari	0:2b57931c6ed4	732	}
hitonari	0:2b57931c6ed4	733	} else count_op = 0;
hitonari	0:2b57931c6ed4	734	Ascending(targetAngle);
hitonari	0:2b57931c6ed4	735	break;
hitonari	0:2b57931c6ed4	736
hitonari	0:2b57931c6ed4	737	case BombwithPC:
hitonari	0:2b57931c6ed4	738	if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) {
hitonari	0:2b57931c6ed4	739	count_op++;
hitonari	0:2b57931c6ed4	740	if(count_op > changeModeCount) {
hitonari	0:2b57931c6ed4	741	operation_mode = LeftLoop;
hitonari	0:2b57931c6ed4	742	pc.printf("Goto Left mode\r\n");
hitonari	0:2b57931c6ed4	743	pc.attach(NULL, Serial::RxIrq);
hitonari	0:2b57931c6ed4	744	count_op = 0;
hitonari	0:2b57931c6ed4	745	}
hitonari	0:2b57931c6ed4	746	} else count_op = 0;
hitonari	0:2b57931c6ed4	747	Take_off_and_landing(targetAngle);
hitonari	0:2b57931c6ed4	748	break;
hitonari	0:2b57931c6ed4	749
hitonari	0:2b57931c6ed4	750	default:
hitonari	0:2b57931c6ed4	751	operation_mode = StartUp;
hitonari	0:2b57931c6ed4	752	break;
hitonari	0:2b57931c6ed4	753	}
hitonari	0:2b57931c6ed4	754	#endif
hitonari	0:2b57931c6ed4	755
hitonari	0:2b57931c6ed4	756	if(CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	757	output_status = Auto;
hitonari	0:2b57931c6ed4	758	led1 = 1;
hitonari	0:2b57931c6ed4	759	} else {
hitonari	0:2b57931c6ed4	760	output_status = Manual;
hitonari	0:2b57931c6ed4	761	led1 = 0;
hitonari	0:2b57931c6ed4	762	}
hitonari	0:2b57931c6ed4	763
hitonari	0:2b57931c6ed4	764
hitonari	0:2b57931c6ed4	765	}
hitonari	0:2b57931c6ed4	766
hitonari	0:2b57931c6ed4	767	int GetParameter(FILE fp, const char paramName,char parameter[])
hitonari	0:2b57931c6ed4	768	{
hitonari	0:2b57931c6ed4	769	int i=0, j=0;
hitonari	0:2b57931c6ed4	770	int strmax = 200;
hitonari	0:2b57931c6ed4	771	char str[strmax];
hitonari	0:2b57931c6ed4	772
hitonari	0:2b57931c6ed4	773	rewind(fp); //ファイル位置を先頭に
hitonari	0:2b57931c6ed4	774	while(1) {
hitonari	0:2b57931c6ed4	775	if (fgets(str, strmax, fp) == NULL) {
hitonari	0:2b57931c6ed4	776	return 0;
hitonari	0:2b57931c6ed4	777	}
hitonari	0:2b57931c6ed4	778	if (!strncmp(str, paramName, strlen(paramName))) {
hitonari	0:2b57931c6ed4	779	while (str[i++] != '=') {}
hitonari	0:2b57931c6ed4	780	while (str[i] != '\n') {
hitonari	0:2b57931c6ed4	781	parameter[j++] = str[i++];
hitonari	0:2b57931c6ed4	782	}
hitonari	0:2b57931c6ed4	783	parameter[j] = '\0';
hitonari	0:2b57931c6ed4	784	return 1;
hitonari	0:2b57931c6ed4	785	}
hitonari	0:2b57931c6ed4	786	}
hitonari	0:2b57931c6ed4	787	}
hitonari	0:2b57931c6ed4	788
hitonari	0:2b57931c6ed4	789
hitonari	0:2b57931c6ed4	790	//sdによる設定
hitonari	0:2b57931c6ed4	791	int SetOptions(float g_kpELE, float g_kiELE, float *g_kdELE,
hitonari	0:2b57931c6ed4	792	float g_kpRUD, float g_kiRUD, float *g_kdRUD,
hitonari	0:2b57931c6ed4	793	float g_rightloopROLL, float g_rightloopPITCH,
hitonari	0:2b57931c6ed4	794	float g_leftloopROLL, float g_leftloopPITCH,
hitonari	0:2b57931c6ed4	795	float g_gostraightROLL, float g_gostraightPITCH,
hitonari	0:2b57931c6ed4	796	float g_takeoffTHR, float g_loopTHR,
hitonari	0:2b57931c6ed4	797	float g_rightloopROLLshort, float g_rightloopPITCHshort,
hitonari	0:2b57931c6ed4	798	float g_leftloopROLLshort, float g_leftloopPITCHshort,
hitonari	0:2b57931c6ed4	799	float g_glideloopROLL, float g_glideloopPITCH,
hitonari	0:2b57931c6ed4	800	float g_kpAIL, float g_kiAIL, float *g_kdAIL,
hitonari	0:2b57931c6ed4	801	int g_rightloopRUD, int g_rightloopshortRUD,
hitonari	0:2b57931c6ed4	802	int g_leftloopRUD, int g_leftloopshortRUD,
hitonari	0:2b57931c6ed4	803	int *g_glideloopRUD,
hitonari	0:2b57931c6ed4	804	float g_rightloopROLL_approach,float g_leftloopROLL_approach,
hitonari	0:2b57931c6ed4	805	int g_rightloopRUD_approach,int g_leftloopRUD_approach,
hitonari	0:2b57931c6ed4	806	float g_rightloopPITCH_approach,float g_leftloopPITCH_approach,
hitonari	0:2b57931c6ed4	807	float *g_ascendingPITCH,
hitonari	0:2b57931c6ed4	808	int g_take_off_THROTTLE, float g_take_off_PITCH,
hitonari	0:2b57931c6ed4	809	float g_rightloop_THR_rate, float g_leftloop_THR_rate,
hitonari	0:2b57931c6ed4	810	float *g_ascending_THR_rate,
hitonari	0:2b57931c6ed4	811	float g_ascending_ROLL, int g_ascending_RUD,
hitonari	0:2b57931c6ed4	812	int g_trim1, int g_trim2, int *g_trim3,
hitonari	0:2b57931c6ed4	813	int g_trim4, int g_trim5, int *g_trim6,
hitonari	0:2b57931c6ed4	814	bool *g_trim_check
hitonari	0:2b57931c6ed4	815	)
hitonari	0:2b57931c6ed4	816	{
hitonari	0:2b57931c6ed4	817
hitonari	0:2b57931c6ed4	818	pc.printf("SDsetup start.\r\n");
hitonari	0:2b57931c6ed4	819
hitonari	0:2b57931c6ed4	820	FILE *fp;
hitonari	0:2b57931c6ed4	821	char parameter[50]; //文字列渡す用の配列
hitonari	0:2b57931c6ed4	822	int SDerrorcount = 0; //取得できなかった数を返す
hitonari	0:2b57931c6ed4	823	const char *paramNames[] = {
hitonari	0:2b57931c6ed4	824	"KP_ELEVATOR",
hitonari	0:2b57931c6ed4	825	"KI_ELEVATOR",
hitonari	0:2b57931c6ed4	826	"KD_ELEVATOR",
hitonari	0:2b57931c6ed4	827	"KP_RUDDER",
hitonari	0:2b57931c6ed4	828	"KI_RUDDER",
hitonari	0:2b57931c6ed4	829	"KD_RUDDER",
hitonari	0:2b57931c6ed4	830	"RIGHTLOOP_ROLL",
hitonari	0:2b57931c6ed4	831	"RIGHTLOOP_PITCH",
hitonari	0:2b57931c6ed4	832	"LEFTLOOP_ROLL",
hitonari	0:2b57931c6ed4	833	"LEFTLOOP_PITCH",
hitonari	0:2b57931c6ed4	834	"GOSTRAIGHT_ROLL",
hitonari	0:2b57931c6ed4	835	"GOSTRAIGHT_PITCH",
hitonari	0:2b57931c6ed4	836	"TAKEOFF_THR_RATE",
hitonari	0:2b57931c6ed4	837	"LOOP_THR_RATE",
hitonari	0:2b57931c6ed4	838	"RIGHTLOOP_ROLL_SHORT",
hitonari	0:2b57931c6ed4	839	"RIGHTLOOP_PITCH_SHORT",
hitonari	0:2b57931c6ed4	840	"LEFTLOOP_ROLL_SHORT",
hitonari	0:2b57931c6ed4	841	"LEFTLOOP_PITCH_SHORT",
hitonari	0:2b57931c6ed4	842	"AUTOGLIDE_ROLL",
hitonari	0:2b57931c6ed4	843	"AUTOGLIDE PITCH",
hitonari	0:2b57931c6ed4	844	"KP_AILERON",
hitonari	0:2b57931c6ed4	845	"KI_AILERON",
hitonari	0:2b57931c6ed4	846	"KD_AILERON",
hitonari	0:2b57931c6ed4	847	"RIGHTLOOP_RUDDER",
hitonari	0:2b57931c6ed4	848	"RIGHTLOOPSHORT_RUDDER",
hitonari	0:2b57931c6ed4	849	"LEFTLOOP_RUDDER",
hitonari	0:2b57931c6ed4	850	"LEFTLOOPSHORT_RUDDER",
hitonari	0:2b57931c6ed4	851	"GLIDELOOP_RUDDER",
hitonari	0:2b57931c6ed4	852	"RIGHTLOOP_ROLL_APPROACH",
hitonari	0:2b57931c6ed4	853	"LEFTLOOP_ROLL_APPROACH",
hitonari	0:2b57931c6ed4	854	"RIGHTLOOP_RUDDER_APPROACH",
hitonari	0:2b57931c6ed4	855	"LEFTLOOP_RUDDER_APPROACH",
hitonari	0:2b57931c6ed4	856	"RIGHTLOOP_PITCH_APPROACH",
hitonari	0:2b57931c6ed4	857	"LEFTLOOP_PITCH_APPROACH",
hitonari	0:2b57931c6ed4	858	"ASCENDING_PITCH",
hitonari	0:2b57931c6ed4	859	"TAKE_OFF_THROTTLE",
hitonari	0:2b57931c6ed4	860	"TAKE_OFF_PITCH",
hitonari	0:2b57931c6ed4	861	"RIGHTLOOP_THR_RATE",
hitonari	0:2b57931c6ed4	862	"LEFTLOOP_THR_RATE",
hitonari	0:2b57931c6ed4	863	"ASCENDING_THR_RATE",
hitonari	0:2b57931c6ed4	864	"ASCENDING_ROLL",
hitonari	0:2b57931c6ed4	865	"ASCENDING_RUD",
hitonari	0:2b57931c6ed4	866	"TRIM1",
hitonari	0:2b57931c6ed4	867	"TRIM2",
hitonari	0:2b57931c6ed4	868	"TRIM3",
hitonari	0:2b57931c6ed4	869	"TRIM4",
hitonari	0:2b57931c6ed4	870	"TRIM5",
hitonari	0:2b57931c6ed4	871	"TRIM6",
hitonari	0:2b57931c6ed4	872	"TRIM_CHECK"
hitonari	0:2b57931c6ed4	873	};
hitonari	0:2b57931c6ed4	874
hitonari	0:2b57931c6ed4	875	fp = fopen("/sd/option.txt","r");
hitonari	0:2b57931c6ed4	876
hitonari	0:2b57931c6ed4	877	if(fp != NULL) { //開けたら
hitonari	0:2b57931c6ed4	878	pc.printf("File was openned.\r\n");
hitonari	0:2b57931c6ed4	879	if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter);
hitonari	0:2b57931c6ed4	880	else {
hitonari	0:2b57931c6ed4	881	*g_kpELE = KP_ELE;
hitonari	0:2b57931c6ed4	882	SDerrorcount++;
hitonari	0:2b57931c6ed4	883	}
hitonari	0:2b57931c6ed4	884	if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter);
hitonari	0:2b57931c6ed4	885	else {
hitonari	0:2b57931c6ed4	886	*g_kiELE = KI_ELE;
hitonari	0:2b57931c6ed4	887	SDerrorcount++;
hitonari	0:2b57931c6ed4	888	}
hitonari	0:2b57931c6ed4	889	if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter);
hitonari	0:2b57931c6ed4	890	else {
hitonari	0:2b57931c6ed4	891	*g_kdELE = KD_ELE;
hitonari	0:2b57931c6ed4	892	SDerrorcount++;
hitonari	0:2b57931c6ed4	893	}
hitonari	0:2b57931c6ed4	894	if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter);
hitonari	0:2b57931c6ed4	895	else {
hitonari	0:2b57931c6ed4	896	*g_kpRUD = KP_RUD;
hitonari	0:2b57931c6ed4	897	SDerrorcount++;
hitonari	0:2b57931c6ed4	898	}
hitonari	0:2b57931c6ed4	899	if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter);
hitonari	0:2b57931c6ed4	900	else {
hitonari	0:2b57931c6ed4	901	*g_kiRUD = KI_RUD;
hitonari	0:2b57931c6ed4	902	SDerrorcount++;
hitonari	0:2b57931c6ed4	903	}
hitonari	0:2b57931c6ed4	904	if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter);
hitonari	0:2b57931c6ed4	905	else {
hitonari	0:2b57931c6ed4	906	*g_kdRUD = KD_RUD;
hitonari	0:2b57931c6ed4	907	SDerrorcount++;
hitonari	0:2b57931c6ed4	908	}
hitonari	0:2b57931c6ed4	909	if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter);
hitonari	0:2b57931c6ed4	910	else {
hitonari	0:2b57931c6ed4	911	*g_rightloopROLL = RIGHT_ROLL;
hitonari	0:2b57931c6ed4	912	SDerrorcount++;
hitonari	0:2b57931c6ed4	913	}
hitonari	0:2b57931c6ed4	914	if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter);
hitonari	0:2b57931c6ed4	915	else {
hitonari	0:2b57931c6ed4	916	*g_rightloopPITCH = RIGHT_PITCH;
hitonari	0:2b57931c6ed4	917	SDerrorcount++;
hitonari	0:2b57931c6ed4	918	}
hitonari	0:2b57931c6ed4	919	if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter);
hitonari	0:2b57931c6ed4	920	else {
hitonari	0:2b57931c6ed4	921	*g_leftloopROLL = LEFT_ROLL;
hitonari	0:2b57931c6ed4	922	SDerrorcount++;
hitonari	0:2b57931c6ed4	923	}
hitonari	0:2b57931c6ed4	924	if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter);
hitonari	0:2b57931c6ed4	925	else {
hitonari	0:2b57931c6ed4	926	*g_leftloopPITCH = LEFT_PITCH;
hitonari	0:2b57931c6ed4	927	SDerrorcount++;
hitonari	0:2b57931c6ed4	928	}
hitonari	0:2b57931c6ed4	929	if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter);
hitonari	0:2b57931c6ed4	930	else {
hitonari	0:2b57931c6ed4	931	*g_gostraightROLL = STRAIGHT_ROLL;
hitonari	0:2b57931c6ed4	932	SDerrorcount++;
hitonari	0:2b57931c6ed4	933	}
hitonari	0:2b57931c6ed4	934	if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter);
hitonari	0:2b57931c6ed4	935	else {
hitonari	0:2b57931c6ed4	936	*g_gostraightPITCH = STRAIGHT_PITCH;
hitonari	0:2b57931c6ed4	937	SDerrorcount++;
hitonari	0:2b57931c6ed4	938	}
hitonari	0:2b57931c6ed4	939	if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter);
hitonari	0:2b57931c6ed4	940	else {
hitonari	0:2b57931c6ed4	941	*g_takeoffTHR = TAKEOFF_THR;
hitonari	0:2b57931c6ed4	942	SDerrorcount++;
hitonari	0:2b57931c6ed4	943	}
hitonari	0:2b57931c6ed4	944	if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter);
hitonari	0:2b57931c6ed4	945	else {
hitonari	0:2b57931c6ed4	946	*g_loopTHR = LOOP_THR;
hitonari	0:2b57931c6ed4	947	SDerrorcount++;
hitonari	0:2b57931c6ed4	948	}
hitonari	0:2b57931c6ed4	949	if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter);
hitonari	0:2b57931c6ed4	950	else {
hitonari	0:2b57931c6ed4	951	*g_rightloopROLLshort = RIGHT_ROLL_SHORT;
hitonari	0:2b57931c6ed4	952	SDerrorcount++;
hitonari	0:2b57931c6ed4	953	}
hitonari	0:2b57931c6ed4	954	if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter);
hitonari	0:2b57931c6ed4	955	else {
hitonari	0:2b57931c6ed4	956	*g_rightloopPITCHshort = RIGHT_PITCH_SHORT;
hitonari	0:2b57931c6ed4	957	SDerrorcount++;
hitonari	0:2b57931c6ed4	958	}
hitonari	0:2b57931c6ed4	959	if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter);
hitonari	0:2b57931c6ed4	960	else {
hitonari	0:2b57931c6ed4	961	*g_leftloopROLLshort = LEFT_ROLL_SHORT;
hitonari	0:2b57931c6ed4	962	SDerrorcount++;
hitonari	0:2b57931c6ed4	963	}
hitonari	0:2b57931c6ed4	964	if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter);
hitonari	0:2b57931c6ed4	965	else {
hitonari	0:2b57931c6ed4	966	*g_leftloopPITCHshort = LEFT_PITCH_SHORT;
hitonari	0:2b57931c6ed4	967	SDerrorcount++;
hitonari	0:2b57931c6ed4	968	}
hitonari	0:2b57931c6ed4	969	if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter);
hitonari	0:2b57931c6ed4	970	else {
hitonari	0:2b57931c6ed4	971	*g_glideloopROLL = GLIDE_ROLL;
hitonari	0:2b57931c6ed4	972	SDerrorcount++;
hitonari	0:2b57931c6ed4	973	}
hitonari	0:2b57931c6ed4	974	if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter);
hitonari	0:2b57931c6ed4	975	else {
hitonari	0:2b57931c6ed4	976	*g_glideloopPITCH = GLIDE_PITCH;
hitonari	0:2b57931c6ed4	977	SDerrorcount++;
hitonari	0:2b57931c6ed4	978	}
hitonari	0:2b57931c6ed4	979	if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter);
hitonari	0:2b57931c6ed4	980	else {
hitonari	0:2b57931c6ed4	981	*g_kpAIL = KP_AIL;
hitonari	0:2b57931c6ed4	982	SDerrorcount++;
hitonari	0:2b57931c6ed4	983	}
hitonari	0:2b57931c6ed4	984	if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter);
hitonari	0:2b57931c6ed4	985	else {
hitonari	0:2b57931c6ed4	986	*g_kiAIL = KI_AIL;
hitonari	0:2b57931c6ed4	987	SDerrorcount++;
hitonari	0:2b57931c6ed4	988	}
hitonari	0:2b57931c6ed4	989	if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter);
hitonari	0:2b57931c6ed4	990	else {
hitonari	0:2b57931c6ed4	991	*g_kdAIL = KP_AIL;
hitonari	0:2b57931c6ed4	992	SDerrorcount++;
hitonari	0:2b57931c6ed4	993	}
hitonari	0:2b57931c6ed4	994	if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter);
hitonari	0:2b57931c6ed4	995	else {
hitonari	0:2b57931c6ed4	996	*g_rightloopRUD = RIGHTLOOP_RUD;
hitonari	0:2b57931c6ed4	997	SDerrorcount++;
hitonari	0:2b57931c6ed4	998	}
hitonari	0:2b57931c6ed4	999	if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter);
hitonari	0:2b57931c6ed4	1000	else {
hitonari	0:2b57931c6ed4	1001	*g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
hitonari	0:2b57931c6ed4	1002	SDerrorcount++;
hitonari	0:2b57931c6ed4	1003	}
hitonari	0:2b57931c6ed4	1004	if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter);
hitonari	0:2b57931c6ed4	1005	else {
hitonari	0:2b57931c6ed4	1006	*g_leftloopshortRUD = LEFTLOOP_RUD;
hitonari	0:2b57931c6ed4	1007	SDerrorcount++;
hitonari	0:2b57931c6ed4	1008	}
hitonari	0:2b57931c6ed4	1009	if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter);
hitonari	0:2b57931c6ed4	1010	else {
hitonari	0:2b57931c6ed4	1011	*g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
hitonari	0:2b57931c6ed4	1012	SDerrorcount++;
hitonari	0:2b57931c6ed4	1013	}
hitonari	0:2b57931c6ed4	1014	if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter);
hitonari	0:2b57931c6ed4	1015	else {
hitonari	0:2b57931c6ed4	1016	*g_glideloopRUD = GLIDELOOP_RUD;
hitonari	0:2b57931c6ed4	1017	SDerrorcount++;
hitonari	0:2b57931c6ed4	1018	}
hitonari	0:2b57931c6ed4	1019	if(GetParameter(fp,paramNames[28],parameter)) *g_rightloopROLL_approach = atof(parameter);
hitonari	0:2b57931c6ed4	1020	else {
hitonari	0:2b57931c6ed4	1021	*g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH;
hitonari	0:2b57931c6ed4	1022	SDerrorcount++;
hitonari	0:2b57931c6ed4	1023	}
hitonari	0:2b57931c6ed4	1024	if(GetParameter(fp,paramNames[29],parameter)) *g_leftloopROLL_approach= atof(parameter);
hitonari	0:2b57931c6ed4	1025	else {
hitonari	0:2b57931c6ed4	1026	*g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
hitonari	0:2b57931c6ed4	1027	SDerrorcount++;
hitonari	0:2b57931c6ed4	1028	}
hitonari	0:2b57931c6ed4	1029	if(GetParameter(fp,paramNames[30],parameter)) *g_rightloopRUD_approach = atof(parameter);
hitonari	0:2b57931c6ed4	1030	else {
hitonari	0:2b57931c6ed4	1031	*g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
hitonari	0:2b57931c6ed4	1032	SDerrorcount++;
hitonari	0:2b57931c6ed4	1033	}
hitonari	0:2b57931c6ed4	1034	if(GetParameter(fp,paramNames[31],parameter)) *g_leftloopRUD_approach= atof(parameter);
hitonari	0:2b57931c6ed4	1035	else {
hitonari	0:2b57931c6ed4	1036	*g_leftloopRUD_approach= LEFTLOOPRUD_APPROACH;
hitonari	0:2b57931c6ed4	1037	SDerrorcount++;
hitonari	0:2b57931c6ed4	1038	}
hitonari	0:2b57931c6ed4	1039	if(GetParameter(fp,paramNames[32],parameter)) *g_rightloopPITCH_approach = atof(parameter);
hitonari	0:2b57931c6ed4	1040	else {
hitonari	0:2b57931c6ed4	1041	*g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
hitonari	0:2b57931c6ed4	1042	SDerrorcount++;
hitonari	0:2b57931c6ed4	1043	}
hitonari	0:2b57931c6ed4	1044	if(GetParameter(fp,paramNames[33],parameter)) *g_leftloopPITCH_approach = atof(parameter);
hitonari	0:2b57931c6ed4	1045	else {
hitonari	0:2b57931c6ed4	1046	*g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
hitonari	0:2b57931c6ed4	1047	SDerrorcount++;
hitonari	0:2b57931c6ed4	1048	}
hitonari	0:2b57931c6ed4	1049	if(GetParameter(fp,paramNames[34],parameter)) *g_ascendingPITCH = atof(parameter);
hitonari	0:2b57931c6ed4	1050	else {
hitonari	0:2b57931c6ed4	1051	*g_ascendingPITCH = ASCENDING_PITCH;
hitonari	0:2b57931c6ed4	1052	SDerrorcount++;
hitonari	0:2b57931c6ed4	1053	}
hitonari	0:2b57931c6ed4	1054	if(GetParameter(fp,paramNames[35],parameter)) *g_take_off_THROTTLE = atof(parameter);
hitonari	0:2b57931c6ed4	1055	else {
hitonari	0:2b57931c6ed4	1056	*g_take_off_THROTTLE = TAKE_OFF_THROTTLE;
hitonari	0:2b57931c6ed4	1057	SDerrorcount++;
hitonari	0:2b57931c6ed4	1058	}
hitonari	0:2b57931c6ed4	1059	if(GetParameter(fp,paramNames[36],parameter)) *g_take_off_PITCH = atof(parameter);
hitonari	0:2b57931c6ed4	1060	else {
hitonari	0:2b57931c6ed4	1061	*g_take_off_PITCH = TAKE_OFF_PITCH;
hitonari	0:2b57931c6ed4	1062	SDerrorcount++;
hitonari	0:2b57931c6ed4	1063	}
hitonari	0:2b57931c6ed4	1064	if(GetParameter(fp,paramNames[37],parameter)) *g_rightloop_THR_rate = atof(parameter);
hitonari	0:2b57931c6ed4	1065	else {
hitonari	0:2b57931c6ed4	1066	*g_rightloop_THR_rate = RIGHTLOOP_THR_RATE;
hitonari	0:2b57931c6ed4	1067	SDerrorcount++;
hitonari	0:2b57931c6ed4	1068	}
hitonari	0:2b57931c6ed4	1069	if(GetParameter(fp,paramNames[38],parameter)) *g_leftloop_THR_rate = atof(parameter);
hitonari	0:2b57931c6ed4	1070	else {
hitonari	0:2b57931c6ed4	1071	*g_leftloop_THR_rate = LEFTLOOP_THR_RATE;
hitonari	0:2b57931c6ed4	1072	SDerrorcount++;
hitonari	0:2b57931c6ed4	1073	}
hitonari	0:2b57931c6ed4	1074	if(GetParameter(fp,paramNames[39],parameter)) *g_ascending_THR_rate = atof(parameter);
hitonari	0:2b57931c6ed4	1075	else {
hitonari	0:2b57931c6ed4	1076	*g_ascending_THR_rate = ASCENDING_THR_RATE;
hitonari	0:2b57931c6ed4	1077	SDerrorcount++;
hitonari	0:2b57931c6ed4	1078	}
hitonari	0:2b57931c6ed4	1079	if(GetParameter(fp,paramNames[40],parameter)) *g_ascending_ROLL = atof(parameter);
hitonari	0:2b57931c6ed4	1080	else {
hitonari	0:2b57931c6ed4	1081	*g_ascending_ROLL = ASCENDING_ROLL;
hitonari	0:2b57931c6ed4	1082	SDerrorcount++;
hitonari	0:2b57931c6ed4	1083	}
hitonari	0:2b57931c6ed4	1084	if(GetParameter(fp,paramNames[41],parameter)) *g_ascending_RUD= atof(parameter);
hitonari	0:2b57931c6ed4	1085	else {
hitonari	0:2b57931c6ed4	1086	*g_ascending_RUD = ASCENDING_RUD;
hitonari	0:2b57931c6ed4	1087	SDerrorcount++;
hitonari	0:2b57931c6ed4	1088	}
hitonari	0:2b57931c6ed4	1089	if(GetParameter(fp,paramNames[42],parameter)) *g_trim1 = atof(parameter);
hitonari	0:2b57931c6ed4	1090	else {
hitonari	0:2b57931c6ed4	1091	*g_trim1 = TRIM1;
hitonari	0:2b57931c6ed4	1092	SDerrorcount++;
hitonari	0:2b57931c6ed4	1093	}
hitonari	0:2b57931c6ed4	1094	if(GetParameter(fp,paramNames[43],parameter)) *g_trim2 = atof(parameter);
hitonari	0:2b57931c6ed4	1095	else {
hitonari	0:2b57931c6ed4	1096	*g_trim2 = TRIM2;
hitonari	0:2b57931c6ed4	1097	SDerrorcount++;
hitonari	0:2b57931c6ed4	1098	}
hitonari	0:2b57931c6ed4	1099	if(GetParameter(fp,paramNames[44],parameter)) *g_trim3 = atof(parameter);
hitonari	0:2b57931c6ed4	1100	else {
hitonari	0:2b57931c6ed4	1101	*g_trim3 = TRIM3;
hitonari	0:2b57931c6ed4	1102	SDerrorcount++;
hitonari	0:2b57931c6ed4	1103	}
hitonari	0:2b57931c6ed4	1104	if(GetParameter(fp,paramNames[45],parameter)) *g_trim4 = atof(parameter);
hitonari	0:2b57931c6ed4	1105	else {
hitonari	0:2b57931c6ed4	1106	*g_trim4 = TRIM4;
hitonari	0:2b57931c6ed4	1107	SDerrorcount++;
hitonari	0:2b57931c6ed4	1108	}
hitonari	0:2b57931c6ed4	1109	if(GetParameter(fp,paramNames[46],parameter)) *g_trim5 = atof(parameter);
hitonari	0:2b57931c6ed4	1110	else {
hitonari	0:2b57931c6ed4	1111	*g_trim5 = TRIM5;
hitonari	0:2b57931c6ed4	1112	SDerrorcount++;
hitonari	0:2b57931c6ed4	1113	}
hitonari	0:2b57931c6ed4	1114	if(GetParameter(fp,paramNames[47],parameter)) *g_trim6 = atof(parameter);
hitonari	0:2b57931c6ed4	1115	else {
hitonari	0:2b57931c6ed4	1116	*g_trim6 = TRIM6;
hitonari	0:2b57931c6ed4	1117	SDerrorcount++;
hitonari	0:2b57931c6ed4	1118	}
hitonari	0:2b57931c6ed4	1119	if(GetParameter(fp,paramNames[48],parameter)) *g_trim_check = atof(parameter);
hitonari	0:2b57931c6ed4	1120	else {
hitonari	0:2b57931c6ed4	1121	*g_trim_check = TRIM_CHECK;
hitonari	0:2b57931c6ed4	1122	SDerrorcount++;
hitonari	0:2b57931c6ed4	1123	}
hitonari	0:2b57931c6ed4	1124
hitonari	0:2b57931c6ed4	1125
hitonari	0:2b57931c6ed4	1126	fclose(fp);
hitonari	0:2b57931c6ed4	1127
hitonari	0:2b57931c6ed4	1128	} else { //ファイルがなかったら
hitonari	0:2b57931c6ed4	1129	pc.printf("fp was null.\r\n");
hitonari	0:2b57931c6ed4	1130	*g_kpELE = KP_ELE;
hitonari	0:2b57931c6ed4	1131	*g_kiELE = KI_ELE;
hitonari	0:2b57931c6ed4	1132	*g_kdELE = KD_ELE;
hitonari	0:2b57931c6ed4	1133	*g_kpRUD = KP_RUD;
hitonari	0:2b57931c6ed4	1134	*g_kiRUD = KI_RUD;
hitonari	0:2b57931c6ed4	1135	*g_kdRUD = KD_RUD;
hitonari	0:2b57931c6ed4	1136	*g_rightloopROLL = RIGHT_ROLL;
hitonari	0:2b57931c6ed4	1137	*g_rightloopPITCH = RIGHT_PITCH;
hitonari	0:2b57931c6ed4	1138	*g_leftloopROLL = LEFT_ROLL;
hitonari	0:2b57931c6ed4	1139	*g_leftloopPITCH = LEFT_PITCH;
hitonari	0:2b57931c6ed4	1140	*g_gostraightROLL = STRAIGHT_ROLL;
hitonari	0:2b57931c6ed4	1141	*g_gostraightPITCH = STRAIGHT_PITCH;
hitonari	0:2b57931c6ed4	1142	*g_takeoffTHR = TAKEOFF_THR;
hitonari	0:2b57931c6ed4	1143	*g_loopTHR = LOOP_THR;
hitonari	0:2b57931c6ed4	1144	*g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく！！
hitonari	0:2b57931c6ed4	1145	*g_kiAIL = KI_AIL;
hitonari	0:2b57931c6ed4	1146	*g_kdAIL = KD_AIL;
hitonari	0:2b57931c6ed4	1147	*g_rightloopRUD = RIGHTLOOP_RUD;
hitonari	0:2b57931c6ed4	1148	*g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
hitonari	0:2b57931c6ed4	1149	*g_leftloopRUD = LEFTLOOP_RUD;
hitonari	0:2b57931c6ed4	1150	*g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
hitonari	0:2b57931c6ed4	1151	*g_glideloopRUD = GLIDELOOP_RUD;
hitonari	0:2b57931c6ed4	1152	*g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH;
hitonari	0:2b57931c6ed4	1153	*g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
hitonari	0:2b57931c6ed4	1154	*g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
hitonari	0:2b57931c6ed4	1155	*g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH;
hitonari	0:2b57931c6ed4	1156	*g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
hitonari	0:2b57931c6ed4	1157	*g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
hitonari	0:2b57931c6ed4	1158	*g_ascendingPITCH = ASCENDING_PITCH;
hitonari	0:2b57931c6ed4	1159	*g_take_off_THROTTLE = TAKE_OFF_THROTTLE;
hitonari	0:2b57931c6ed4	1160	*g_take_off_PITCH = TAKE_OFF_PITCH;
hitonari	0:2b57931c6ed4	1161	*g_rightloop_THR_rate = RIGHTLOOP_THR_RATE;
hitonari	0:2b57931c6ed4	1162	*g_leftloop_THR_rate = LEFTLOOP_THR_RATE;
hitonari	0:2b57931c6ed4	1163	*g_ascending_THR_rate = ASCENDING_THR_RATE;
hitonari	0:2b57931c6ed4	1164	*g_ascending_ROLL = ASCENDING_ROLL;
hitonari	0:2b57931c6ed4	1165	*g_ascending_RUD = ASCENDING_RUD;
hitonari	0:2b57931c6ed4	1166	*g_trim1 = TRIM1;
hitonari	0:2b57931c6ed4	1167	*g_trim2 = TRIM2;
hitonari	0:2b57931c6ed4	1168	*g_trim3 = TRIM3;
hitonari	0:2b57931c6ed4	1169	*g_trim4 = TRIM4;
hitonari	0:2b57931c6ed4	1170	*g_trim5 = TRIM5;
hitonari	0:2b57931c6ed4	1171	*g_trim6 = TRIM6;
hitonari	0:2b57931c6ed4	1172	*g_trim_check = TRIM_CHECK;
hitonari	0:2b57931c6ed4	1173
hitonari	0:2b57931c6ed4	1174	SDerrorcount = -1;
hitonari	0:2b57931c6ed4	1175	}
hitonari	0:2b57931c6ed4	1176	pc.printf("SDsetup finished.\r\n");
hitonari	0:2b57931c6ed4	1177	if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
hitonari	0:2b57931c6ed4	1178	else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
hitonari	0:2b57931c6ed4	1179	else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
hitonari	0:2b57931c6ed4	1180
hitonari	0:2b57931c6ed4	1181	pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", g_kpELE, g_kiELE, *g_kdELE);
hitonari	0:2b57931c6ed4	1182	pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", g_kpRUD, g_kiRUD, *g_kdRUD);
hitonari	0:2b57931c6ed4	1183	pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", g_rightloopROLL, g_rightloopPITCH);
hitonari	0:2b57931c6ed4	1184	pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", g_leftloopROLL, g_leftloopPITCH);
hitonari	0:2b57931c6ed4	1185	pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", g_gostraightROLL, g_gostraightPITCH);
hitonari	0:2b57931c6ed4	1186	pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", g_takeoffTHR, g_loopTHR);
hitonari	0:2b57931c6ed4	1187	pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", g_rightloopROLLshort, g_rightloopPITCHshort);
hitonari	0:2b57931c6ed4	1188	pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", g_leftloopROLLshort, g_leftloopPITCHshort);
hitonari	0:2b57931c6ed4	1189	pc.printf("glideROLL = %f, glidePITCH = %f\r\n", g_glideloopROLL, g_glideloopPITCH);
hitonari	0:2b57931c6ed4	1190	pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",g_kpAIL,g_kiAIL,*g_kdAIL);
hitonari	0:2b57931c6ed4	1191	pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",g_rightloopRUD,g_rightloopshortRUD);
hitonari	0:2b57931c6ed4	1192	pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",g_leftloopRUD,g_leftloopshortRUD);
hitonari	0:2b57931c6ed4	1193	pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD);
hitonari	0:2b57931c6ed4	1194	pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",g_rightloopROLL_approach,g_leftloopROLL_approach);
hitonari	0:2b57931c6ed4	1195	pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",g_rightloopRUD_approach,g_leftloopRUD_approach);
hitonari	0:2b57931c6ed4	1196	pc.printf("RIGHTLOOP_PITCH_APPROACH = %f, LEFTLOOP_PITCH_APPROACH = %f\r\n",g_rightloopPITCH_approach,g_leftloopPITCH_approach);
hitonari	0:2b57931c6ed4	1197	pc.printf("ASCENDING_PITCH = %f\r\n",*g_ascendingPITCH);
hitonari	0:2b57931c6ed4	1198	pc.printf("TAKE_OFF_THROTTLE = %d, TAKE_OFF_PITCH = %f\r\n",g_take_off_THROTTLE,g_take_off_PITCH);
hitonari	0:2b57931c6ed4	1199	pc.printf("RIGHTLOOP_THR_RATE = %f, LEFTLOOP_THR_RATE = %f, ASCENDING_THR_RATE = %f\r\n", g_rightloop_THR_rate, g_leftloop_THR_rate, *g_ascending_THR_rate);
hitonari	0:2b57931c6ed4	1200	pc.printf("ASCENDING_ROLL = %f,ASCENDING_RUD = %d\r\n",g_ascending_ROLL,g_ascending_RUD);
hitonari	0:2b57931c6ed4	1201	pc.printf("TRIM1 = %d, TRIM2 = %d, TRIM3 = %d\r\n",g_trim1,g_trim2,*g_trim3);
hitonari	0:2b57931c6ed4	1202	pc.printf("TRIM4 = %d, TRIM5 = %d, TRIM6 = %d\r\n",g_trim4,g_trim5,*g_trim6);
hitonari	0:2b57931c6ed4	1203	pc.printf("TRIM_CHECK = %d\r\n",*g_trim_check);
hitonari	0:2b57931c6ed4	1204
hitonari	0:2b57931c6ed4	1205	return SDerrorcount;
hitonari	0:2b57931c6ed4	1206	}
hitonari	0:2b57931c6ed4	1207
hitonari	0:2b57931c6ed4	1208	void CalculateControlValue(float targetAngle[3], float controlValue[3])
hitonari	0:2b57931c6ed4	1209	{
hitonari	0:2b57931c6ed4	1210
hitonari	0:2b57931c6ed4	1211	static int t_last;
hitonari	0:2b57931c6ed4	1212	int t_now;
hitonari	0:2b57931c6ed4	1213	float dt;
hitonari	0:2b57931c6ed4	1214
hitonari	0:2b57931c6ed4	1215	t_now = t.read_us();
hitonari	0:2b57931c6ed4	1216	dt = (float)((t_now - t_last)/1000000.0f) ;
hitonari	0:2b57931c6ed4	1217	t_last = t_now;
hitonari	0:2b57931c6ed4	1218
hitonari	0:2b57931c6ed4	1219
hitonari	0:2b57931c6ed4	1220	//controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
hitonari	0:2b57931c6ed4	1221	controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御
hitonari	0:2b57931c6ed4	1222	controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
hitonari	0:2b57931c6ed4	1223
hitonari	0:2b57931c6ed4	1224	}
hitonari	0:2b57931c6ed4	1225
hitonari	0:2b57931c6ed4	1226	void UpdateAutoPWM(float controlValue[3])
hitonari	0:2b57931c6ed4	1227	{
hitonari	0:2b57931c6ed4	1228	NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1229	int16_t addpwm[2]; //-500~500
hitonari	0:2b57931c6ed4	1230	addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正レバー:機首上げ正
hitonari	0:2b57931c6ed4	1231	addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正（8月13日時点；左回転が正！）レバー:右回転正
hitonari	0:2b57931c6ed4	1232
hitonari	0:2b57931c6ed4	1233	autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite
hitonari	0:2b57931c6ed4	1234	autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL];
hitonari	0:2b57931c6ed4	1235	//autopwm[THR] = oldTHR;
hitonari	0:2b57931c6ed4	1236
hitonari	0:2b57931c6ed4	1237	autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
hitonari	0:2b57931c6ed4	1238	autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
hitonari	0:2b57931c6ed4	1239
hitonari	0:2b57931c6ed4	1240	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1241	}
hitonari	0:2b57931c6ed4	1242
hitonari	0:2b57931c6ed4	1243	inline float CalcRatio(float value, float trim, float limit)
hitonari	0:2b57931c6ed4	1244	{
hitonari	0:2b57931c6ed4	1245	return (value - trim) / (limit - trim);
hitonari	0:2b57931c6ed4	1246	}
hitonari	0:2b57931c6ed4	1247
hitonari	0:2b57931c6ed4	1248	bool CheckSW_Up(Channel ch)
hitonari	0:2b57931c6ed4	1249	{
hitonari	0:2b57931c6ed4	1250
hitonari	0:2b57931c6ed4	1251	if(SWITCH_CHECK < sbus.manualpwm[ch]) {
hitonari	0:2b57931c6ed4	1252	return true;
hitonari	0:2b57931c6ed4	1253	} else {
hitonari	0:2b57931c6ed4	1254	return false;
hitonari	0:2b57931c6ed4	1255	}
hitonari	0:2b57931c6ed4	1256
hitonari	0:2b57931c6ed4	1257	}
hitonari	0:2b57931c6ed4	1258
hitonari	0:2b57931c6ed4	1259	int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min)
hitonari	0:2b57931c6ed4	1260	{
hitonari	0:2b57931c6ed4	1261	if(value > max) return max;
hitonari	0:2b57931c6ed4	1262	if(value < min) return min;
hitonari	0:2b57931c6ed4	1263	return value;
hitonari	0:2b57931c6ed4	1264	}
hitonari	0:2b57931c6ed4	1265
hitonari	0:2b57931c6ed4	1266	inline int16_t SetTHRinRatio(float ratio)
hitonari	0:2b57931c6ed4	1267	{
hitonari	0:2b57931c6ed4	1268	return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
hitonari	0:2b57931c6ed4	1269	}
hitonari	0:2b57931c6ed4	1270
hitonari	0:2b57931c6ed4	1271
hitonari	0:2b57931c6ed4	1272
hitonari	0:2b57931c6ed4	1273	/---SBUS割り込み処理---/
hitonari	0:2b57931c6ed4	1274
hitonari	0:2b57931c6ed4	1275	//udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
hitonari	0:2b57931c6ed4	1276	//各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
hitonari	0:2b57931c6ed4	1277	void Update_PWM()
hitonari	0:2b57931c6ed4	1278	{
hitonari	0:2b57931c6ed4	1279	NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1280	static int16_t pwm[6];
hitonari	0:2b57931c6ed4	1281	static int16_t temppwm[6]= {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
hitonari	0:2b57931c6ed4	1282	static int16_t FailsafeCounter=0;
hitonari	0:2b57931c6ed4	1283	static int16_t ResetCounter=0;
hitonari	0:2b57931c6ed4	1284	static int16_t OKCounter=0;
hitonari	0:2b57931c6ed4	1285
hitonari	0:2b57931c6ed4	1286	if(sbus.flg_ch_update == true) {
hitonari	0:2b57931c6ed4	1287
hitonari	0:2b57931c6ed4	1288	switch(output_status) { //マニュアルモード,自動モード,自動着陸もモードを切替
hitonari	0:2b57931c6ed4	1289	case Manual:
hitonari	0:2b57931c6ed4	1290	if(OKCounter!=0) break;
hitonari	0:2b57931c6ed4	1291	for(uint8_t i=0; i<6; i++) {
hitonari	0:2b57931c6ed4	1292	pwm[i] = sbus.manualpwm[i];
hitonari	0:2b57931c6ed4	1293	}
hitonari	0:2b57931c6ed4	1294	oldTHR = sbus.manualpwm[THR];
hitonari	0:2b57931c6ed4	1295	//pc.printf("update_manual\r\n");
hitonari	0:2b57931c6ed4	1296	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1297	break;
hitonari	0:2b57931c6ed4	1298
hitonari	0:2b57931c6ed4	1299	case Auto:
hitonari	0:2b57931c6ed4	1300	if(OKCounter!=0) break;
hitonari	0:2b57931c6ed4	1301	pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL];
hitonari	0:2b57931c6ed4	1302	pwm[ELE] = autopwm[ELE];
hitonari	0:2b57931c6ed4	1303	pwm[THR] = autopwm[THR];
hitonari	0:2b57931c6ed4	1304	pwm[RUD] = autopwm[RUD];
hitonari	0:2b57931c6ed4	1305	pwm[DROP] = autopwm[DROP];
hitonari	0:2b57931c6ed4	1306	pwm[AIL_L] = autopwm[AIL_L];
hitonari	0:2b57931c6ed4	1307
hitonari	0:2b57931c6ed4	1308	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1309	break;
hitonari	0:2b57931c6ed4	1310
hitonari	0:2b57931c6ed4	1311	default:
hitonari	0:2b57931c6ed4	1312	if(OKCounter!=0) break;
hitonari	0:2b57931c6ed4	1313	for(uint8_t i=0; i<6; i++) {
hitonari	0:2b57931c6ed4	1314	pwm[i] = sbus.manualpwm[i];
hitonari	0:2b57931c6ed4	1315	} //pc.printf("update_manual\r\n");
hitonari	0:2b57931c6ed4	1316	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1317	break;
hitonari	0:2b57931c6ed4	1318	}
hitonari	0:2b57931c6ed4	1319
hitonari	0:2b57931c6ed4	1320	for(uint8_t i=0; i<6; i++) {
hitonari	0:2b57931c6ed4	1321	if(pwm[i]<1000 \|\| pwm[i]>2000) pwm[i]=temppwm[i];
hitonari	0:2b57931c6ed4	1322	temppwm[i]=pwm[i];
hitonari	0:2b57931c6ed4	1323	}
hitonari	0:2b57931c6ed4	1324
hitonari	0:2b57931c6ed4	1325	}
hitonari	0:2b57931c6ed4	1326	//else(sbus.flg_ch_update == false) pc.printf("0\r\n");
hitonari	0:2b57931c6ed4	1327	/* if(sbus.failsafe_status==SBUS_SIGNAL_OK){
hitonari	0:2b57931c6ed4	1328	pc.printf("OK\r\n");
hitonari	0:2b57931c6ed4	1329	}
hitonari	0:2b57931c6ed4	1330	*/
hitonari	0:2b57931c6ed4	1331	//pc.printf("%d\r\n",sbus.failsafe_status);
hitonari	0:2b57931c6ed4	1332
hitonari	0:2b57931c6ed4	1333	if(sbus.failsafe_status==SBUS_SIGNAL_LOST\|\|sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
hitonari	0:2b57931c6ed4	1334	else ResetCounter++;
hitonari	0:2b57931c6ed4	1335
hitonari	0:2b57931c6ed4	1336	if(ResetCounter>7) {
hitonari	0:2b57931c6ed4	1337	ResetCounter=0;
hitonari	0:2b57931c6ed4	1338	FailsafeCounter=0;
hitonari	0:2b57931c6ed4	1339	}
hitonari	0:2b57931c6ed4	1340
hitonari	0:2b57931c6ed4	1341	if(FailsafeCounter>10) {
hitonari	0:2b57931c6ed4	1342	ResetCounter=0;
hitonari	0:2b57931c6ed4	1343	for(uint8_t i=0; i<6; i++) pwm[i] = trimpwm[i];
hitonari	0:2b57931c6ed4	1344
hitonari	0:2b57931c6ed4	1345	if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++;
hitonari	0:2b57931c6ed4	1346	else OKCounter=0;
hitonari	0:2b57931c6ed4	1347
hitonari	0:2b57931c6ed4	1348	if(OKCounter>10) {
hitonari	0:2b57931c6ed4	1349	OKCounter=0;
hitonari	0:2b57931c6ed4	1350	FailsafeCounter=0;
hitonari	0:2b57931c6ed4	1351	}
hitonari	0:2b57931c6ed4	1352	//pc.printf("OKCounter=%d, FailsafeCounter=%d, sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status);
hitonari	0:2b57931c6ed4	1353	}
hitonari	0:2b57931c6ed4	1354	//if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;}
hitonari	0:2b57931c6ed4	1355
hitonari	0:2b57931c6ed4	1356
hitonari	0:2b57931c6ed4	1357	sbus.flg_ch_update = false;
hitonari	0:2b57931c6ed4	1358	Output_PWM(pwm);
hitonari	0:2b57931c6ed4	1359	}
hitonari	0:2b57931c6ed4	1360
hitonari	0:2b57931c6ed4	1361
hitonari	0:2b57931c6ed4	1362
hitonari	0:2b57931c6ed4	1363
hitonari	0:2b57931c6ed4	1364	//pwmをサーボに出力。
hitonari	0:2b57931c6ed4	1365	void Output_PWM(int16_t pwm[5])
hitonari	0:2b57931c6ed4	1366	{
hitonari	0:2b57931c6ed4	1367	NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1368	servo1.pulsewidth_us(pwm[0]);
hitonari	0:2b57931c6ed4	1369	servo2.pulsewidth_us(pwm[1]);
hitonari	0:2b57931c6ed4	1370	servo3.pulsewidth_us(pwm[2]);
hitonari	0:2b57931c6ed4	1371	servo4.pulsewidth_us(pwm[3]);
hitonari	0:2b57931c6ed4	1372	servo5.pulsewidth_us(pwm[4]);
hitonari	0:2b57931c6ed4	1373	servo6.pulsewidth_us(pwm[5]);
hitonari	0:2b57931c6ed4	1374	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1375
hitonari	0:2b57931c6ed4	1376	}
hitonari	0:2b57931c6ed4	1377
hitonari	0:2b57931c6ed4	1378	void ResetTrim()
hitonari	0:2b57931c6ed4	1379	{
hitonari	0:2b57931c6ed4	1380	for(uint8_t i=0; i<6; i++) { //i=4から書き換え＿投下サーボは入ってない模様
hitonari	0:2b57931c6ed4	1381	trimpwm[i] = sbus.manualpwm[i];
hitonari	0:2b57931c6ed4	1382	}
hitonari	0:2b57931c6ed4	1383	pc.printf("reset PWM trim\r\n");
hitonari	0:2b57931c6ed4	1384	}
hitonari	0:2b57931c6ed4	1385
hitonari	0:2b57931c6ed4	1386
hitonari	0:2b57931c6ed4	1387	void SensingMPU()
hitonari	0:2b57931c6ed4	1388	{
hitonari	0:2b57931c6ed4	1389	//static int16_t deltaT = 0, t_start = 0;
hitonari	0:2b57931c6ed4	1390	//t_start = t.read_us();
hitonari	0:2b57931c6ed4	1391
hitonari	0:2b57931c6ed4	1392	float rpy[3] = {0}, oldrpy[3] = {0};
hitonari	0:2b57931c6ed4	1393	static uint16_t count_changeRPY = 0;
hitonari	0:2b57931c6ed4	1394	static bool flg_checkoutlier = false;
hitonari	0:2b57931c6ed4	1395	NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1396	NVIC_DisableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1397	NVIC_DisableIRQ(TIM5_IRQn);
hitonari	0:2b57931c6ed4	1398	NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
hitonari	0:2b57931c6ed4	1399	NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
hitonari	0:2b57931c6ed4	1400
hitonari	0:2b57931c6ed4	1401	mpu6050.getRollPitchYaw_Skipper(rpy);
hitonari	0:2b57931c6ed4	1402
hitonari	0:2b57931c6ed4	1403	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1404	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1405	NVIC_EnableIRQ(TIM5_IRQn);
hitonari	0:2b57931c6ed4	1406	NVIC_EnableIRQ(EXTI0_IRQn);
hitonari	0:2b57931c6ed4	1407	NVIC_EnableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	1408
hitonari	0:2b57931c6ed4	1409
hitonari	0:2b57931c6ed4	1410	//外れ値対策
hitonari	0:2b57931c6ed4	1411	for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
hitonari	0:2b57931c6ed4	1412	rpy[ROLL] -= FirstROLL;
hitonari	0:2b57931c6ed4	1413	rpy[PITCH] -= FirstPITCH;
hitonari	0:2b57931c6ed4	1414	rpy[YAW] -= FirstYAW;
hitonari	0:2b57931c6ed4	1415
hitonari	0:2b57931c6ed4	1416	for(uint8_t i=0; i<3; i++) {
hitonari	0:2b57931c6ed4	1417	if(rpy[i] < nowAngle[i]-10 \|\| rpy[i] > nowAngle[i]+10) {
hitonari	0:2b57931c6ed4	1418	flg_checkoutlier = true;
hitonari	0:2b57931c6ed4	1419	}
hitonari	0:2b57931c6ed4	1420	}
hitonari	0:2b57931c6ed4	1421	if(!flg_checkoutlier \|\| count_changeRPY >= 2) {
hitonari	0:2b57931c6ed4	1422	for(uint8_t i=0; i<3; i++) {
hitonari	0:2b57931c6ed4	1423	nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
hitonari	0:2b57931c6ed4	1424	}
hitonari	0:2b57931c6ed4	1425	count_changeRPY = 0;
hitonari	0:2b57931c6ed4	1426	} else count_changeRPY++;
hitonari	0:2b57931c6ed4	1427	flg_checkoutlier = false;
hitonari	0:2b57931c6ed4	1428
hitonari	0:2b57931c6ed4	1429	}
hitonari	0:2b57931c6ed4	1430
hitonari	0:2b57931c6ed4	1431	float TranslateNewYaw(float beforeYaw, float newzeroYaw)
hitonari	0:2b57931c6ed4	1432	{
hitonari	0:2b57931c6ed4	1433	float newYaw = beforeYaw - newzeroYaw;
hitonari	0:2b57931c6ed4	1434
hitonari	0:2b57931c6ed4	1435	if(newYaw<-180.0f) newYaw += 360.0f;
hitonari	0:2b57931c6ed4	1436	else if(newYaw>180.0f) newYaw -= 360.0f;
hitonari	0:2b57931c6ed4	1437	return newYaw;
hitonari	0:2b57931c6ed4	1438	}
hitonari	0:2b57931c6ed4	1439
hitonari	0:2b57931c6ed4	1440
hitonari	0:2b57931c6ed4	1441	void getSF_Serial()
hitonari	0:2b57931c6ed4	1442	{
hitonari	0:2b57931c6ed4	1443	//NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1444	//NVIC_DisableIRQ(EXTI0_IRQn);
hitonari	0:2b57931c6ed4	1445	//NVIC_DisableIRQ(TIM5_IRQn);
hitonari	0:2b57931c6ed4	1446
hitonari	0:2b57931c6ed4	1447
hitonari	0:2b57931c6ed4	1448	static char SFbuf[16]= {'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
hitonari	0:2b57931c6ed4	1449
hitonari	0:2b57931c6ed4	1450	static int bufcounter=0;
hitonari	0:2b57931c6ed4	1451
hitonari	0:2b57931c6ed4	1452	float targetyaw = 0;
hitonari	0:2b57931c6ed4	1453
hitonari	0:2b57931c6ed4	1454
hitonari	0:2b57931c6ed4	1455
hitonari	0:2b57931c6ed4	1456	if(pc.readable()) { // 受信確認
hitonari	0:2b57931c6ed4	1457
hitonari	0:2b57931c6ed4	1458	SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
hitonari	0:2b57931c6ed4	1459	if(SFbuf[0]!='S') {
hitonari	0:2b57931c6ed4	1460	//pc.printf("x");
hitonari	0:2b57931c6ed4	1461	return;
hitonari	0:2b57931c6ed4	1462	}
hitonari	0:2b57931c6ed4	1463
hitonari	0:2b57931c6ed4	1464
hitonari	0:2b57931c6ed4	1465
hitonari	0:2b57931c6ed4	1466	pc.printf("%c",SFbuf[bufcounter]);
hitonari	0:2b57931c6ed4	1467
hitonari	0:2b57931c6ed4	1468	if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
hitonari	0:2b57931c6ed4	1469
hitonari	0:2b57931c6ed4	1470	if(bufcounter==5 && SFbuf[4]=='F') {
hitonari	0:2b57931c6ed4	1471
hitonari	0:2b57931c6ed4	1472	g_landingcommand = SFbuf[1];
hitonari	0:2b57931c6ed4	1473	//pc.printf("%c",g_landingcommand);
hitonari	0:2b57931c6ed4	1474	wait_ms(10);
hitonari	0:2b57931c6ed4	1475	if(g_landingcommand=='Y'){
hitonari	0:2b57931c6ed4	1476	targetyaw = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
hitonari	0:2b57931c6ed4	1477	if(targetyaw>=0 && targetyaw<=360){
hitonari	0:2b57931c6ed4	1478	g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
hitonari	0:2b57931c6ed4	1479	}
hitonari	0:2b57931c6ed4	1480	}
hitonari	0:2b57931c6ed4	1481	//if(g_landingcommand=='Y')g_SerialTargetYAW =0.0f;
hitonari	0:2b57931c6ed4	1482	bufcounter = 0;
hitonari	0:2b57931c6ed4	1483	memset(SFbuf, 0, sizeof(SFbuf));
hitonari	0:2b57931c6ed4	1484	NVIC_ClearPendingIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1485	//pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
hitonari	0:2b57931c6ed4	1486	}
hitonari	0:2b57931c6ed4	1487
hitonari	0:2b57931c6ed4	1488	else if(bufcounter>=5) {
hitonari	0:2b57931c6ed4	1489	//pc.printf("Communication Falsed.\r\n");
hitonari	0:2b57931c6ed4	1490	memset(SFbuf, 0, sizeof(SFbuf));
hitonari	0:2b57931c6ed4	1491	bufcounter = 0;
hitonari	0:2b57931c6ed4	1492	NVIC_ClearPendingIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1493	}
hitonari	0:2b57931c6ed4	1494	}
hitonari	0:2b57931c6ed4	1495
hitonari	0:2b57931c6ed4	1496	//NVIC_EnableIRQ(TIM5_IRQn);
hitonari	0:2b57931c6ed4	1497	//NVIC_EnableIRQ(EXTI0_IRQn);
hitonari	0:2b57931c6ed4	1498	//NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	1499	}
hitonari	0:2b57931c6ed4	1500
hitonari	0:2b57931c6ed4	1501	float ConvertByteintoFloat(char high, char low)
hitonari	0:2b57931c6ed4	1502	{
hitonari	0:2b57931c6ed4	1503
hitonari	0:2b57931c6ed4	1504	//int16_t intvalue = (int16_t)high*256 + (int16_t)low;
hitonari	0:2b57931c6ed4	1505	int16_t intvalue = (int16_t)(((int16_t)high << 8) \| low); // Turn the MSB and LSB into a signed 16-bit value
hitonari	0:2b57931c6ed4	1506	float floatvalue = (float)intvalue-256;
hitonari	0:2b57931c6ed4	1507	return floatvalue;
hitonari	0:2b57931c6ed4	1508	}
hitonari	0:2b57931c6ed4	1509
hitonari	0:2b57931c6ed4	1510
hitonari	0:2b57931c6ed4	1511	//超音波割り込み
hitonari	0:2b57931c6ed4	1512
hitonari	0:2b57931c6ed4	1513	void UpdateDist()
hitonari	0:2b57931c6ed4	1514	{
hitonari	0:2b57931c6ed4	1515	g_distance = usensor.get_dist_cm();
hitonari	0:2b57931c6ed4	1516	usensor.start();
hitonari	0:2b57931c6ed4	1517	}
hitonari	0:2b57931c6ed4	1518
hitonari	0:2b57931c6ed4	1519	//8の字旋回
hitonari	0:2b57931c6ed4	1520	void UpdateTargetAngle_Moebius(float targetAngle[3])
hitonari	0:2b57931c6ed4	1521	{
hitonari	0:2b57931c6ed4	1522	static uint8_t RotateCounter=0;
hitonari	0:2b57931c6ed4	1523	static bool flg_setInStartAuto = false;
hitonari	0:2b57931c6ed4	1524	static float FirstYAW_Moebius = 0.0;
hitonari	0:2b57931c6ed4	1525	float newYaw_Moebius;
hitonari	0:2b57931c6ed4	1526
hitonari	0:2b57931c6ed4	1527	if(!flg_setInStartAuto && CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1528	FirstYAW_Moebius = nowAngle[YAW];
hitonari	0:2b57931c6ed4	1529	RotateCounter = 0;
hitonari	0:2b57931c6ed4	1530	flg_setInStartAuto = true;
hitonari	0:2b57931c6ed4	1531	} else if(!CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1532	flg_setInStartAuto = false;
hitonari	0:2b57931c6ed4	1533	led4 = 0;
hitonari	0:2b57931c6ed4	1534	}
hitonari	0:2b57931c6ed4	1535	autopwm[THR]=oldTHR;
hitonari	0:2b57931c6ed4	1536
hitonari	0:2b57931c6ed4	1537	newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);
hitonari	0:2b57931c6ed4	1538
hitonari	0:2b57931c6ed4	1539	if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {
hitonari	0:2b57931c6ed4	1540	RotateCounter++;
hitonari	0:2b57931c6ed4	1541	led4 = 1;
hitonari	0:2b57931c6ed4	1542	pc.printf("Rotate 90\r\n");
hitonari	0:2b57931c6ed4	1543	}
hitonari	0:2b57931c6ed4	1544	if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {
hitonari	0:2b57931c6ed4	1545	RotateCounter++;
hitonari	0:2b57931c6ed4	1546	led4 = 0;
hitonari	0:2b57931c6ed4	1547	pc.printf("Rotate 180\r\n");
hitonari	0:2b57931c6ed4	1548	}
hitonari	0:2b57931c6ed4	1549	if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-20.0) {
hitonari	0:2b57931c6ed4	1550	RotateCounter++;
hitonari	0:2b57931c6ed4	1551	led4 = 1;
hitonari	0:2b57931c6ed4	1552	pc.printf("Rotate 270\r\n");
hitonari	0:2b57931c6ed4	1553	}
hitonari	0:2b57931c6ed4	1554	if(RotateCounter == 3 && newYaw_Moebius >-10.0 && newYaw_Moebius < 90.0) {
hitonari	0:2b57931c6ed4	1555	RotateCounter++;
hitonari	0:2b57931c6ed4	1556	led4 = 0;
hitonari	0:2b57931c6ed4	1557	pc.printf("Change Rotate direction\r\n");
hitonari	0:2b57931c6ed4	1558	}
hitonari	0:2b57931c6ed4	1559
hitonari	0:2b57931c6ed4	1560
hitonari	0:2b57931c6ed4	1561	if(RotateCounter <= 3) UpdateTargetAngle_Rightloop(targetAngle);
hitonari	0:2b57931c6ed4	1562	else UpdateTargetAngle_Leftloop(targetAngle); //左旋回
hitonari	0:2b57931c6ed4	1563
hitonari	0:2b57931c6ed4	1564	}
hitonari	0:2b57931c6ed4	1565
hitonari	0:2b57931c6ed4	1566
hitonari	0:2b57931c6ed4	1567
hitonari	0:2b57931c6ed4	1568	//離陸-投下-着陸一連
hitonari	0:2b57931c6ed4	1569	void Take_off_and_landing(float targetAngle[3])
hitonari	0:2b57931c6ed4	1570	{
hitonari	0:2b57931c6ed4	1571
hitonari	0:2b57931c6ed4	1572	if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
hitonari	0:2b57931c6ed4	1573
hitonari	0:2b57931c6ed4	1574	switch(bombing_mode) {
hitonari	0:2b57931c6ed4	1575	case Takeoff:
hitonari	0:2b57931c6ed4	1576	static bool flg_setFirstYaw = false;
hitonari	0:2b57931c6ed4	1577	static int TakeoffCount = 0;
hitonari	0:2b57931c6ed4	1578
hitonari	0:2b57931c6ed4	1579	if(!flg_setFirstYaw && CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1580	FirstYAW = nowAngle[YAW];
hitonari	0:2b57931c6ed4	1581	flg_setFirstYaw = true;
hitonari	0:2b57931c6ed4	1582	} else if(flg_setFirstYaw && !CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1583	flg_setFirstYaw = false;
hitonari	0:2b57931c6ed4	1584	}
hitonari	0:2b57931c6ed4	1585
hitonari	0:2b57931c6ed4	1586	UpdateTargetAngle_Takeoff(targetAngle);
hitonari	0:2b57931c6ed4	1587	NVIC_DisableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	1588
hitonari	0:2b57931c6ed4	1589
hitonari	0:2b57931c6ed4	1590	//if(g_distance>120) TakeoffCount++;
hitonari	0:2b57931c6ed4	1591	// else TakeoffCount = 0;
hitonari	0:2b57931c6ed4	1592	// if(TakeoffCount>3) Chicken_Drop();
hitonari	0:2b57931c6ed4	1593
hitonari	0:2b57931c6ed4	1594	//if(g_distance>180)TakeoffCount++;
hitonari	0:2b57931c6ed4	1595	//else TakeoffCount = 0;
hitonari	0:2b57931c6ed4	1596
hitonari	0:2b57931c6ed4	1597	//if(TakeoffCount>2)Chicken_Drop();
hitonari	0:2b57931c6ed4	1598
hitonari	0:2b57931c6ed4	1599
hitonari	0:2b57931c6ed4	1600	//if(g_distance>210)Chicken_Drop();
hitonari	0:2b57931c6ed4	1601
hitonari	0:2b57931c6ed4	1602
hitonari	0:2b57931c6ed4	1603
hitonari	0:2b57931c6ed4	1604	if(g_distance>120) TakeoffCount++;
hitonari	0:2b57931c6ed4	1605	else TakeoffCount = 0;
hitonari	0:2b57931c6ed4	1606	//Chicken_Drop();
hitonari	0:2b57931c6ed4	1607	NVIC_EnableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	1608	if(g_landingcommand == 'B'){
hitonari	0:2b57931c6ed4	1609	Chicken_Drop();
hitonari	0:2b57931c6ed4	1610	}
hitonari	0:2b57931c6ed4	1611	if(TakeoffCount>5) {
hitonari	0:2b57931c6ed4	1612
hitonari	0:2b57931c6ed4	1613	//autopwm[THR] = SetTHRinRatio(0.3);
hitonari	0:2b57931c6ed4	1614	autopwm[THR] = 1700;//g_take_off_THROTTLE;//1180+32020.3;
hitonari	0:2b57931c6ed4	1615	targetAngle[PITCH]=g_gostraightPITCH;
hitonari	0:2b57931c6ed4	1616	autopwm[RUD]=1370;//trimpwm[RUD];
hitonari	0:2b57931c6ed4	1617	targetAngle[ROLL]=g_gostraightROLL;
hitonari	0:2b57931c6ed4	1618	Chicken_Drop();
hitonari	0:2b57931c6ed4	1619	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	1620	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	1621	}
hitonari	0:2b57931c6ed4	1622	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	1623
hitonari	0:2b57931c6ed4	1624	//pc.printf("Now go to Approach mode!!");
hitonari	0:2b57931c6ed4	1625	bombing_mode = Approach;
hitonari	0:2b57931c6ed4	1626	g_landingcommand = 'T';
hitonari	0:2b57931c6ed4	1627	}
hitonari	0:2b57931c6ed4	1628
hitonari	0:2b57931c6ed4	1629
hitonari	0:2b57931c6ed4	1630
hitonari	0:2b57931c6ed4	1631	break;
hitonari	0:2b57931c6ed4	1632
hitonari	0:2b57931c6ed4	1633	//case Chicken:
hitonari	0:2b57931c6ed4	1634	//break;
hitonari	0:2b57931c6ed4	1635	/*
hitonari	0:2b57931c6ed4	1636	case Transition:
hitonari	0:2b57931c6ed4	1637	static int ApproachCount = 0;
hitonari	0:2b57931c6ed4	1638	targetAngle[YAW]=180.0;
hitonari	0:2b57931c6ed4	1639	int Judge = Rotate(targetAngle, g_SerialTargetYAW);
hitonari	0:2b57931c6ed4	1640
hitonari	0:2b57931c6ed4	1641	if(Judge==0) ApproachCount++;
hitonari	0:2b57931c6ed4	1642	if(ApproachCount>5) bombing_mode = Approach;
hitonari	0:2b57931c6ed4	1643	break;
hitonari	0:2b57931c6ed4	1644	*/
hitonari	0:2b57931c6ed4	1645	case Approach:
hitonari	0:2b57931c6ed4	1646
hitonari	0:2b57931c6ed4	1647	//autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
hitonari	0:2b57931c6ed4	1648	autopwm[THR] = minpwm[THR];//1180+32020.3;
hitonari	0:2b57931c6ed4	1649	if(g_Finflag == true){
hitonari	0:2b57931c6ed4	1650	autopwm[THR] = minpwm[THR];
hitonari	0:2b57931c6ed4	1651	autopwm[ELE] = minpwm[ELE];
hitonari	0:2b57931c6ed4	1652	autopwm[RUD]=trimpwm[RUD];
hitonari	0:2b57931c6ed4	1653	autopwm[AIL_R] = minpwm[AIL_R];
hitonari	0:2b57931c6ed4	1654	autopwm[AIL_L]= maxpwm[AIL_L];
hitonari	0:2b57931c6ed4	1655	targetAngle[PITCH] = -80;
hitonari	0:2b57931c6ed4	1656	targetAngle[ROLL] = -80;
hitonari	0:2b57931c6ed4	1657	}else{
hitonari	0:2b57931c6ed4	1658	UpdateTargetAngle_Approach(targetAngle);
hitonari	0:2b57931c6ed4	1659	}
hitonari	0:2b57931c6ed4	1660
hitonari	0:2b57931c6ed4	1661	break;
hitonari	0:2b57931c6ed4	1662
hitonari	0:2b57931c6ed4	1663	default:
hitonari	0:2b57931c6ed4	1664	bombing_mode = Takeoff;
hitonari	0:2b57931c6ed4	1665	break;
hitonari	0:2b57931c6ed4	1666	}
hitonari	0:2b57931c6ed4	1667
hitonari	0:2b57931c6ed4	1668	}
hitonari	0:2b57931c6ed4	1669
hitonari	0:2b57931c6ed4	1670	//離陸モード
hitonari	0:2b57931c6ed4	1671	void UpdateTargetAngle_Takeoff(float targetAngle[3])
hitonari	0:2b57931c6ed4	1672	{
hitonari	0:2b57931c6ed4	1673	//pc.printf("%d \r\n",g_distance);
hitonari	0:2b57931c6ed4	1674	static int tELE_start = 0;
hitonari	0:2b57931c6ed4	1675	static bool flg_ELEup = false;
hitonari	0:2b57931c6ed4	1676	int t_def = 0;
hitonari	0:2b57931c6ed4	1677
hitonari	0:2b57931c6ed4	1678	autopwm[RUD] = 1390;//trimpwm[RUD];
hitonari	0:2b57931c6ed4	1679	targetAngle[ROLL]=g_gostraightROLL;
hitonari	0:2b57931c6ed4	1680
hitonari	0:2b57931c6ed4	1681	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	1682	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	1683	}
hitonari	0:2b57931c6ed4	1684	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	1685
hitonari	0:2b57931c6ed4	1686
hitonari	0:2b57931c6ed4	1687	if(!flg_ELEup && CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1688	tELE_start = t.read_ms();
hitonari	0:2b57931c6ed4	1689	flg_ELEup = true;
hitonari	0:2b57931c6ed4	1690	pc.printf("timer start!!!!\r\n");
hitonari	0:2b57931c6ed4	1691	} else if(!CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1692	tELE_start = 0;
hitonari	0:2b57931c6ed4	1693	flg_ELEup = false;
hitonari	0:2b57931c6ed4	1694	//pc.printf("lost!!!!\r\n");
hitonari	0:2b57931c6ed4	1695	}
hitonari	0:2b57931c6ed4	1696	if(flg_ELEup) {
hitonari	0:2b57931c6ed4	1697	t_def = t.read_ms() - tELE_start;
hitonari	0:2b57931c6ed4	1698
hitonari	0:2b57931c6ed4	1699	//1.5秒経過すればELE上げ舵へ
hitonari	0:2b57931c6ed4	1700	if(t_def>500) targetAngle[PITCH]= g_take_off_PITCH;
hitonari	0:2b57931c6ed4	1701	else {
hitonari	0:2b57931c6ed4	1702	t_def = 0;
hitonari	0:2b57931c6ed4	1703	targetAngle[PITCH]=g_gostraightPITCH;
hitonari	0:2b57931c6ed4	1704	}
hitonari	0:2b57931c6ed4	1705	}
hitonari	0:2b57931c6ed4	1706	targetAngle[ROLL] = g_gostraightROLL;
hitonari	0:2b57931c6ed4	1707	//targetAngle[PITCH] = g_loopTHR;
hitonari	0:2b57931c6ed4	1708	autopwm[THR] = g_take_off_THROTTLE;//SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合
hitonari	0:2b57931c6ed4	1709	}
hitonari	0:2b57931c6ed4	1710
hitonari	0:2b57931c6ed4	1711
hitonari	0:2b57931c6ed4	1712	//ヨーを目標値にして許容角度になるまで水平旋回
hitonari	0:2b57931c6ed4	1713	int Rotate(float targetAngle[3], float TargetYAW)
hitonari	0:2b57931c6ed4	1714	{
hitonari	0:2b57931c6ed4	1715	float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
hitonari	0:2b57931c6ed4	1716
hitonari	0:2b57931c6ed4	1717	if(diffYaw > LIMIT_STRAIGHT_YAW) {
hitonari	0:2b57931c6ed4	1718
hitonari	0:2b57931c6ed4	1719	if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
hitonari	0:2b57931c6ed4	1720	else UpdateTargetAngle_Rightloop(targetAngle);
hitonari	0:2b57931c6ed4	1721
hitonari	0:2b57931c6ed4	1722	//UpdateTargetAngle_Rightloop_short(targetAngle);
hitonari	0:2b57931c6ed4	1723	return 1;
hitonari	0:2b57931c6ed4	1724	} else if(diffYaw < -LIMIT_STRAIGHT_YAW) {
hitonari	0:2b57931c6ed4	1725
hitonari	0:2b57931c6ed4	1726	if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
hitonari	0:2b57931c6ed4	1727	else UpdateTargetAngle_Leftloop(targetAngle);
hitonari	0:2b57931c6ed4	1728
hitonari	0:2b57931c6ed4	1729	//UpdateTargetAngle_Leftloop_short(targetAngle);
hitonari	0:2b57931c6ed4	1730
hitonari	0:2b57931c6ed4	1731	return 1;
hitonari	0:2b57931c6ed4	1732	} else {
hitonari	0:2b57931c6ed4	1733	UpdateTargetAngle_GoStraight(targetAngle);
hitonari	0:2b57931c6ed4	1734	return 0;
hitonari	0:2b57931c6ed4	1735	}
hitonari	0:2b57931c6ed4	1736	}
hitonari	0:2b57931c6ed4	1737
hitonari	0:2b57931c6ed4	1738	int Rotate_zero(float targetAngle[3], float TargetYAW){
hitonari	0:2b57931c6ed4	1739	float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]);
hitonari	0:2b57931c6ed4	1740
hitonari	0:2b57931c6ed4	1741	if(diffYaw > LIMIT_STRAIGHT_YAW) {
hitonari	0:2b57931c6ed4	1742
hitonari	0:2b57931c6ed4	1743	/*if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle);
hitonari	0:2b57931c6ed4	1744	else UpdateTargetAngle_Rightloop(targetAngle);
hitonari	0:2b57931c6ed4	1745	*/
hitonari	0:2b57931c6ed4	1746	UpdateTargetAngle_Rightloop_zero(targetAngle);
hitonari	0:2b57931c6ed4	1747	return 1;
hitonari	0:2b57931c6ed4	1748	} else if(diffYaw < -LIMIT_STRAIGHT_YAW) {
hitonari	0:2b57931c6ed4	1749
hitonari	0:2b57931c6ed4	1750	/*if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle);
hitonari	0:2b57931c6ed4	1751	else UpdateTargetAngle_Leftloop(targetAngle);
hitonari	0:2b57931c6ed4	1752	*/
hitonari	0:2b57931c6ed4	1753	UpdateTargetAngle_Leftloop_zero(targetAngle);
hitonari	0:2b57931c6ed4	1754
hitonari	0:2b57931c6ed4	1755	return 1;
hitonari	0:2b57931c6ed4	1756	} else {
hitonari	0:2b57931c6ed4	1757	UpdateTargetAngle_GoStraight_zero(targetAngle);
hitonari	0:2b57931c6ed4	1758	return 0;
hitonari	0:2b57931c6ed4	1759	}
hitonari	0:2b57931c6ed4	1760	}
hitonari	0:2b57931c6ed4	1761
hitonari	0:2b57931c6ed4	1762	//チキラー投下
hitonari	0:2b57931c6ed4	1763	void Chicken_Drop()
hitonari	0:2b57931c6ed4	1764	{
hitonari	0:2b57931c6ed4	1765	//if(CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1766	autopwm[DROP] = maxpwm[DROP];
hitonari	0:2b57931c6ed4	1767	//pc.printf("Bombed!\r\n");
hitonari	0:2b57931c6ed4	1768	//RerurnChickenServo1.attach(&ReturnChickenServo1, 3);
hitonari	0:2b57931c6ed4	1769	//operation_mode = Approach;
hitonari	0:2b57931c6ed4	1770	//buzzer = 0;
hitonari	0:2b57931c6ed4	1771	//pc.printf("Goto LeftLoop mode\r\n");
hitonari	0:2b57931c6ed4	1772	//}
hitonari	0:2b57931c6ed4	1773	}
hitonari	0:2b57931c6ed4	1774
hitonari	0:2b57931c6ed4	1775	void ReturnChickenServo1()
hitonari	0:2b57931c6ed4	1776	{
hitonari	0:2b57931c6ed4	1777	autopwm[DROP] = 1344;
hitonari	0:2b57931c6ed4	1778	pc.printf("first reverse\r\n");
hitonari	0:2b57931c6ed4	1779	RerurnChickenServo2.attach(&ReturnChickenServo2, 1);
hitonari	0:2b57931c6ed4	1780	}
hitonari	0:2b57931c6ed4	1781
hitonari	0:2b57931c6ed4	1782	void ReturnChickenServo2()
hitonari	0:2b57931c6ed4	1783	{
hitonari	0:2b57931c6ed4	1784	autopwm[DROP] = 1392;
hitonari	0:2b57931c6ed4	1785	pc.printf("second reverse\r\n");
hitonari	0:2b57931c6ed4	1786	}
hitonari	0:2b57931c6ed4	1787
hitonari	0:2b57931c6ed4	1788	//着陸モード（PCからの指令に従う）
hitonari	0:2b57931c6ed4	1789	void UpdateTargetAngle_Approach(float targetAngle[3])
hitonari	0:2b57931c6ed4	1790	{
hitonari	0:2b57931c6ed4	1791
hitonari	0:2b57931c6ed4	1792	static bool Zeroflag=false;//着陸時のスロットル動作確認用
hitonari	0:2b57931c6ed4	1793
hitonari	0:2b57931c6ed4	1794	NVIC_DisableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1795
hitonari	0:2b57931c6ed4	1796	if(CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	1797	output_status = Auto;
hitonari	0:2b57931c6ed4	1798	led1 = 1;
hitonari	0:2b57931c6ed4	1799	} else {
hitonari	0:2b57931c6ed4	1800	output_status = Manual;
hitonari	0:2b57931c6ed4	1801	led1 = 0;
hitonari	0:2b57931c6ed4	1802	Zeroflag=true;
hitonari	0:2b57931c6ed4	1803	//g_landingcommand='G';
hitonari	0:2b57931c6ed4	1804	}
hitonari	0:2b57931c6ed4	1805
hitonari	0:2b57931c6ed4	1806	//pc.printf("Zeroflag = %d\r\n",Zeroflag);
hitonari	0:2b57931c6ed4	1807
hitonari	0:2b57931c6ed4	1808	switch(g_landingcommand) {
hitonari	0:2b57931c6ed4	1809	case 'R': //右旋回セヨ
hitonari	0:2b57931c6ed4	1810	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1811	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1812	UpdateTargetAngle_Rightloop_zero(targetAngle);
hitonari	0:2b57931c6ed4	1813	}
hitonari	0:2b57931c6ed4	1814	else{
hitonari	0:2b57931c6ed4	1815	UpdateTargetAngle_Rightloop(targetAngle);
hitonari	0:2b57931c6ed4	1816	/*
hitonari	0:2b57931c6ed4	1817	targetAngle[ROLL] = g_rightloopROLL_approach;
hitonari	0:2b57931c6ed4	1818	targetAngle[PITCH] = g_rightloopPITCH_approach ;
hitonari	0:2b57931c6ed4	1819	autopwm[RUD]=g_rightloopRUD_approach; //RUD固定
hitonari	0:2b57931c6ed4	1820	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	1821	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
hitonari	0:2b57931c6ed4	1822	}
hitonari	0:2b57931c6ed4	1823	else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	1824	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
hitonari	0:2b57931c6ed4	1825	*/
hitonari	0:2b57931c6ed4	1826	}
hitonari	0:2b57931c6ed4	1827
hitonari	0:2b57931c6ed4	1828	break;
hitonari	0:2b57931c6ed4	1829
hitonari	0:2b57931c6ed4	1830	case 'L': //左旋回セヨ
hitonari	0:2b57931c6ed4	1831	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1832	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1833	UpdateTargetAngle_Leftloop_zero(targetAngle);
hitonari	0:2b57931c6ed4	1834	}else{
hitonari	0:2b57931c6ed4	1835	UpdateTargetAngle_Leftloop(targetAngle);
hitonari	0:2b57931c6ed4	1836	/*targetAngle[ROLL] = g_leftloopROLL_approach;
hitonari	0:2b57931c6ed4	1837	targetAngle[PITCH] = g_leftloopPITCH_approach;
hitonari	0:2b57931c6ed4	1838	autopwm[RUD]=g_leftloopRUD_approach;
hitonari	0:2b57931c6ed4	1839	if(autopwm[AIL_R]<trimpwm[AIL_R]){
hitonari	0:2b57931c6ed4	1840	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
hitonari	0:2b57931c6ed4	1841	}
hitonari	0:2b57931c6ed4	1842	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	1843	*/
hitonari	0:2b57931c6ed4	1844	}
hitonari	0:2b57931c6ed4	1845
hitonari	0:2b57931c6ed4	1846	break;
hitonari	0:2b57931c6ed4	1847
hitonari	0:2b57931c6ed4	1848	case 'G': //直進セヨ
hitonari	0:2b57931c6ed4	1849	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1850	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1851	UpdateTargetAngle_GoStraight_zero(targetAngle);
hitonari	0:2b57931c6ed4	1852	}
hitonari	0:2b57931c6ed4	1853	else{
hitonari	0:2b57931c6ed4	1854	UpdateTargetAngle_GoStraight(targetAngle);
hitonari	0:2b57931c6ed4	1855	/*
hitonari	0:2b57931c6ed4	1856	targetAngle[ROLL] = g_gostraightROLL;
hitonari	0:2b57931c6ed4	1857	targetAngle[PITCH] = g_gostraightPITCH;
hitonari	0:2b57931c6ed4	1858	*/
hitonari	0:2b57931c6ed4	1859	}
hitonari	0:2b57931c6ed4	1860
hitonari	0:2b57931c6ed4	1861	break;
hitonari	0:2b57931c6ed4	1862
hitonari	0:2b57931c6ed4	1863	case 'Y': //指定ノヨー方向ニ移動セヨ
hitonari	0:2b57931c6ed4	1864	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1865	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1866	Rotate_zero(targetAngle, g_SerialTargetYAW);
hitonari	0:2b57931c6ed4	1867	//autopwm[THR]=minpwm[THR];
hitonari	0:2b57931c6ed4	1868	}else{
hitonari	0:2b57931c6ed4	1869	Rotate(targetAngle, g_SerialTargetYAW);
hitonari	0:2b57931c6ed4	1870	//autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
hitonari	0:2b57931c6ed4	1871	}
hitonari	0:2b57931c6ed4	1872	break;
hitonari	0:2b57931c6ed4	1873
hitonari	0:2b57931c6ed4	1874	case 'T': //0度ノヨー方向ニ移動セヨ
hitonari	0:2b57931c6ed4	1875	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1876	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1877	Rotate_zero(targetAngle, 0);
hitonari	0:2b57931c6ed4	1878	//autopwm[THR]=minpwm[THR];
hitonari	0:2b57931c6ed4	1879	}else{
hitonari	0:2b57931c6ed4	1880	Rotate(targetAngle, 0);
hitonari	0:2b57931c6ed4	1881	//autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
hitonari	0:2b57931c6ed4	1882	}
hitonari	0:2b57931c6ed4	1883	break;
hitonari	0:2b57931c6ed4	1884
hitonari	0:2b57931c6ed4	1885	case 'U': //機首ヲ上ゲヨ
hitonari	0:2b57931c6ed4	1886	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1887	static int UpCounter=0;
hitonari	0:2b57931c6ed4	1888	UpCounter++;
hitonari	0:2b57931c6ed4	1889	if(UpCounter==3) {
hitonari	0:2b57931c6ed4	1890	g_Finflag = true;
hitonari	0:2b57931c6ed4	1891	}
hitonari	0:2b57931c6ed4	1892
hitonari	0:2b57931c6ed4	1893	break;
hitonari	0:2b57931c6ed4	1894
hitonari	0:2b57931c6ed4	1895	case 'Z': //ヤレバワカル
hitonari	0:2b57931c6ed4	1896	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1897	autopwm[THR] = minpwm[THR];
hitonari	0:2b57931c6ed4	1898	autopwm[ELE] = minpwm[ELE];
hitonari	0:2b57931c6ed4	1899	autopwm[RUD]=trimpwm[RUD];
hitonari	0:2b57931c6ed4	1900	autopwm[AIL_R] = minpwm[AIL_R];
hitonari	0:2b57931c6ed4	1901	autopwm[AIL_L]= maxpwm[AIL_L];
hitonari	0:2b57931c6ed4	1902	targetAngle[ROLL] = -80;
hitonari	0:2b57931c6ed4	1903
hitonari	0:2b57931c6ed4	1904	break;
hitonari	0:2b57931c6ed4	1905
hitonari	0:2b57931c6ed4	1906	/*case 'B': //ブザーヲ鳴ラセ
hitonari	0:2b57931c6ed4	1907	//buzzer = 1;
hitonari	0:2b57931c6ed4	1908	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1909	break;*/
hitonari	0:2b57931c6ed4	1910
hitonari	0:2b57931c6ed4	1911	case 'B': //物資ヲ落トセ
hitonari	0:2b57931c6ed4	1912	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1913	Chicken_Drop();
hitonari	0:2b57931c6ed4	1914	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1915	Rotate_zero(targetAngle, 0);
hitonari	0:2b57931c6ed4	1916	//autopwm[THR]=minpwm[THR];
hitonari	0:2b57931c6ed4	1917	}else{
hitonari	0:2b57931c6ed4	1918	Rotate(targetAngle, 0);
hitonari	0:2b57931c6ed4	1919	//autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
hitonari	0:2b57931c6ed4	1920	}
hitonari	0:2b57931c6ed4	1921
hitonari	0:2b57931c6ed4	1922	break;
hitonari	0:2b57931c6ed4	1923
hitonari	0:2b57931c6ed4	1924	case 'C': //停止セヨ
hitonari	0:2b57931c6ed4	1925	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1926	/*targetAngle[ROLL] = g_gostraightROLL;
hitonari	0:2b57931c6ed4	1927	targetAngle[PITCH] = g_gostraightPITCH;
hitonari	0:2b57931c6ed4	1928	autopwm[THR] = minpwm[THR];
hitonari	0:2b57931c6ed4	1929	*/
hitonari	0:2b57931c6ed4	1930	Zeroflag = true;
hitonari	0:2b57931c6ed4	1931	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1932	Rotate_zero(targetAngle, 0);
hitonari	0:2b57931c6ed4	1933	//autopwm[THR]=minpwm[THR];
hitonari	0:2b57931c6ed4	1934	}else{
hitonari	0:2b57931c6ed4	1935	Rotate(targetAngle, 0);
hitonari	0:2b57931c6ed4	1936	//autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
hitonari	0:2b57931c6ed4	1937	}
hitonari	0:2b57931c6ed4	1938
hitonari	0:2b57931c6ed4	1939	break;
hitonari	0:2b57931c6ed4	1940
hitonari	0:2b57931c6ed4	1941	case 'X': //再起動セヨ
hitonari	0:2b57931c6ed4	1942	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1943	/*
hitonari	0:2b57931c6ed4	1944	targetAngle[ROLL] = g_gostraightROLL;
hitonari	0:2b57931c6ed4	1945	targetAngle[PITCH] = g_gostraightPITCH;
hitonari	0:2b57931c6ed4	1946	autopwm[THR] = minpwm[THR];
hitonari	0:2b57931c6ed4	1947	*/
hitonari	0:2b57931c6ed4	1948	Zeroflag = false;
hitonari	0:2b57931c6ed4	1949	if(Zeroflag==true){
hitonari	0:2b57931c6ed4	1950	Rotate_zero(targetAngle, 0);
hitonari	0:2b57931c6ed4	1951	//autopwm[THR]=minpwm[THR];
hitonari	0:2b57931c6ed4	1952	}else{
hitonari	0:2b57931c6ed4	1953	Rotate(targetAngle, 0);
hitonari	0:2b57931c6ed4	1954	//autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05);
hitonari	0:2b57931c6ed4	1955	}
hitonari	0:2b57931c6ed4	1956
hitonari	0:2b57931c6ed4	1957	break;
hitonari	0:2b57931c6ed4	1958
hitonari	0:2b57931c6ed4	1959	default :
hitonari	0:2b57931c6ed4	1960	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	1961	break;
hitonari	0:2b57931c6ed4	1962
hitonari	0:2b57931c6ed4	1963
hitonari	0:2b57931c6ed4	1964	}
hitonari	0:2b57931c6ed4	1965
hitonari	0:2b57931c6ed4	1966	}
hitonari	0:2b57931c6ed4	1967
hitonari	0:2b57931c6ed4	1968	void checkHeight(float targetAngle[3])
hitonari	0:2b57931c6ed4	1969	{
hitonari	0:2b57931c6ed4	1970
hitonari	0:2b57931c6ed4	1971	static int targetHeight = 200;
hitonari	0:2b57931c6ed4	1972
hitonari	0:2b57931c6ed4	1973	NVIC_DisableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	1974	if(g_distance < targetHeight + ALLOWHEIGHT) {
hitonari	0:2b57931c6ed4	1975	UpdateTargetAngle_NoseUP(targetAngle);
hitonari	0:2b57931c6ed4	1976	if(CheckSW_Up(Ch7)) led2 = 1;
hitonari	0:2b57931c6ed4	1977	} else if(g_distance > targetHeight - ALLOWHEIGHT) {
hitonari	0:2b57931c6ed4	1978	UpdateTargetAngle_NoseDOWN(targetAngle);
hitonari	0:2b57931c6ed4	1979	if(CheckSW_Up(Ch7)) led2 = 1;
hitonari	0:2b57931c6ed4	1980	} else led2=0;
hitonari	0:2b57931c6ed4	1981	NVIC_EnableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	1982	}
hitonari	0:2b57931c6ed4	1983
hitonari	0:2b57931c6ed4	1984	void UpdateTargetAngle_NoseUP(float targetAngle[3])
hitonari	0:2b57931c6ed4	1985	{
hitonari	0:2b57931c6ed4	1986
hitonari	0:2b57931c6ed4	1987	//targetAngle[PITCH] += 2.0f;
hitonari	0:2b57931c6ed4	1988	//if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
hitonari	0:2b57931c6ed4	1989	autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05);
hitonari	0:2b57931c6ed4	1990	//pc.printf("nose UP");
hitonari	0:2b57931c6ed4	1991	}
hitonari	0:2b57931c6ed4	1992
hitonari	0:2b57931c6ed4	1993	void UpdateTargetAngle_NoseDOWN(float targetAngle[3])
hitonari	0:2b57931c6ed4	1994	{
hitonari	0:2b57931c6ed4	1995
hitonari	0:2b57931c6ed4	1996	//targetAngle[PITCH] -= 2.0f;
hitonari	0:2b57931c6ed4	1997	autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05);
hitonari	0:2b57931c6ed4	1998	//pc.printf("nose DOWN");
hitonari	0:2b57931c6ed4	1999	}
hitonari	0:2b57931c6ed4	2000
hitonari	0:2b57931c6ed4	2001	//直進
hitonari	0:2b57931c6ed4	2002	void UpdateTargetAngle_GoStraight(float targetAngle[3])
hitonari	0:2b57931c6ed4	2003	{
hitonari	0:2b57931c6ed4	2004
hitonari	0:2b57931c6ed4	2005	autopwm[RUD] = trimpwm[RUD];
hitonari	0:2b57931c6ed4	2006	targetAngle[ROLL] = g_gostraightROLL;
hitonari	0:2b57931c6ed4	2007	targetAngle[PITCH] = g_gostraightPITCH;
hitonari	0:2b57931c6ed4	2008	autopwm[THR] = SetTHRinRatio(g_loopTHR);
hitonari	0:2b57931c6ed4	2009
hitonari	0:2b57931c6ed4	2010	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	2011	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2012	}
hitonari	0:2b57931c6ed4	2013	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	2014
hitonari	0:2b57931c6ed4	2015	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
hitonari	0:2b57931c6ed4	2016
hitonari	0:2b57931c6ed4	2017	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2018	}
hitonari	0:2b57931c6ed4	2019
hitonari	0:2b57931c6ed4	2020	//直進(着陸時スロットル０のとき）
hitonari	0:2b57931c6ed4	2021	void UpdateTargetAngle_GoStraight_zero(float targetAngle[3])
hitonari	0:2b57931c6ed4	2022	{
hitonari	0:2b57931c6ed4	2023
hitonari	0:2b57931c6ed4	2024	autopwm[RUD] = trimpwm[RUD];
hitonari	0:2b57931c6ed4	2025	targetAngle[ROLL] = g_gostraightROLL;
hitonari	0:2b57931c6ed4	2026	targetAngle[PITCH] = -7.0;//g_gostraightPITCH;
hitonari	0:2b57931c6ed4	2027	autopwm[THR] = minpwm[THR];
hitonari	0:2b57931c6ed4	2028
hitonari	0:2b57931c6ed4	2029	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	2030	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2031	}
hitonari	0:2b57931c6ed4	2032	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	2033
hitonari	0:2b57931c6ed4	2034	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2035	}
hitonari	0:2b57931c6ed4	2036
hitonari	0:2b57931c6ed4	2037	//右旋回
hitonari	0:2b57931c6ed4	2038	void UpdateTargetAngle_Rightloop(float targetAngle[3]) //右旋回
hitonari	0:2b57931c6ed4	2039	{
hitonari	0:2b57931c6ed4	2040	targetAngle[ROLL] = g_rightloopROLL;
hitonari	0:2b57931c6ed4	2041	targetAngle[PITCH] = g_rightloopPITCH;
hitonari	0:2b57931c6ed4	2042	autopwm[RUD]=g_rightloopRUD; //RUD固定
hitonari	0:2b57931c6ed4	2043	autopwm[THR] = SetTHRinRatio(g_rightloop_THR_rate); //手動スロットル記憶
hitonari	0:2b57931c6ed4	2044
hitonari	0:2b57931c6ed4	2045	/*
hitonari	0:2b57931c6ed4	2046	if (nowAngle[ROLL]>20.0\|\|nowAngle[ROLL]<-20.0){
hitonari	0:2b57931c6ed4	2047	t2.start();
hitonari	0:2b57931c6ed4	2048	pc.printf("Timer start.");
hitonari	0:2b57931c6ed4	2049	}
hitonari	0:2b57931c6ed4	2050	if(0.0<t2.read()<5.0){
hitonari	0:2b57931c6ed4	2051	//pc.printf("tagetAngle is changed.");
hitonari	0:2b57931c6ed4	2052	targetAngle[ROLL] = rightloopROLL2;
hitonari	0:2b57931c6ed4	2053	}
hitonari	0:2b57931c6ed4	2054	else {
hitonari	0:2b57931c6ed4	2055	t2.stop();
hitonari	0:2b57931c6ed4	2056	t2.reset();
hitonari	0:2b57931c6ed4	2057	pc.printf("Timer stopped.");
hitonari	0:2b57931c6ed4	2058	targetAngle[ROLL] = g_rightloopROLL;
hitonari	0:2b57931c6ed4	2059	}
hitonari	0:2b57931c6ed4	2060	*/
hitonari	0:2b57931c6ed4	2061	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	2062	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2063	}
hitonari	0:2b57931c6ed4	2064	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	2065
hitonari	0:2b57931c6ed4	2066	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
hitonari	0:2b57931c6ed4	2067
hitonari	0:2b57931c6ed4	2068
hitonari	0:2b57931c6ed4	2069	//checkHeight(targetAngle);
hitonari	0:2b57931c6ed4	2070	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2071	}
hitonari	0:2b57931c6ed4	2072
hitonari	0:2b57931c6ed4	2073	void UpdateTargetAngle_Rightloop_UP(float targetAngle[3]) //右上昇旋回
hitonari	0:2b57931c6ed4	2074	{
hitonari	0:2b57931c6ed4	2075	//autopwm[ELE]=maxpwm[ELE];
hitonari	0:2b57931c6ed4	2076	targetAngle[ROLL] = g_ascending_ROLL;
hitonari	0:2b57931c6ed4	2077	targetAngle[PITCH] = g_ascendingPITCH;
hitonari	0:2b57931c6ed4	2078	autopwm[RUD]=g_ascending_RUD; //RUD固定
hitonari	0:2b57931c6ed4	2079	autopwm[THR] = SetTHRinRatio(g_ascending_THR_rate); //手動スロットル記憶
hitonari	0:2b57931c6ed4	2080
hitonari	0:2b57931c6ed4	2081	/*
hitonari	0:2b57931c6ed4	2082	if (nowAngle[ROLL]>20.0\|\|nowAngle[ROLL]<-20.0){
hitonari	0:2b57931c6ed4	2083	t2.start();
hitonari	0:2b57931c6ed4	2084	pc.printf("Timer start.");
hitonari	0:2b57931c6ed4	2085	}
hitonari	0:2b57931c6ed4	2086	if(0.0<t2.read()<5.0){
hitonari	0:2b57931c6ed4	2087	//pc.printf("tagetAngle is changed.");
hitonari	0:2b57931c6ed4	2088	targetAngle[ROLL] = rightloopROLL2;
hitonari	0:2b57931c6ed4	2089	}
hitonari	0:2b57931c6ed4	2090	else {
hitonari	0:2b57931c6ed4	2091	t2.stop();
hitonari	0:2b57931c6ed4	2092	t2.reset();
hitonari	0:2b57931c6ed4	2093	pc.printf("Timer stopped.");
hitonari	0:2b57931c6ed4	2094	targetAngle[ROLL] = g_rightloopROLL;
hitonari	0:2b57931c6ed4	2095	}
hitonari	0:2b57931c6ed4	2096	*/
hitonari	0:2b57931c6ed4	2097	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	2098	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2099	}
hitonari	0:2b57931c6ed4	2100	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	2101
hitonari	0:2b57931c6ed4	2102	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
hitonari	0:2b57931c6ed4	2103
hitonari	0:2b57931c6ed4	2104
hitonari	0:2b57931c6ed4	2105	//checkHeight(targetAngle);
hitonari	0:2b57931c6ed4	2106	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2107	}
hitonari	0:2b57931c6ed4	2108
hitonari	0:2b57931c6ed4	2109	//右旋回（着陸時スロットル０の時）
hitonari	0:2b57931c6ed4	2110	void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]) //右旋回
hitonari	0:2b57931c6ed4	2111	{
hitonari	0:2b57931c6ed4	2112	autopwm[THR]=minpwm[THR];
hitonari	0:2b57931c6ed4	2113	targetAngle[ROLL] = g_rightloopROLL_approach;
hitonari	0:2b57931c6ed4	2114	targetAngle[PITCH] = g_rightloopPITCH_approach ;
hitonari	0:2b57931c6ed4	2115	autopwm[RUD]=g_rightloopRUD_approach; //RUD固定
hitonari	0:2b57931c6ed4	2116	if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節
hitonari	0:2b57931c6ed4	2117	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2118	}
hitonari	0:2b57931c6ed4	2119	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
hitonari	0:2b57931c6ed4	2120
hitonari	0:2b57931c6ed4	2121	autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
hitonari	0:2b57931c6ed4	2122
hitonari	0:2b57931c6ed4	2123	//checkHeight(targetAngle);
hitonari	0:2b57931c6ed4	2124	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2125	}
hitonari	0:2b57931c6ed4	2126
hitonari	0:2b57931c6ed4	2127	void UpdateTargetAngle_Rightloop_short(float targetAngle[3]) //右旋回
hitonari	0:2b57931c6ed4	2128	{
hitonari	0:2b57931c6ed4	2129
hitonari	0:2b57931c6ed4	2130	targetAngle[ROLL] = g_rightloopROLLshort;
hitonari	0:2b57931c6ed4	2131	targetAngle[PITCH] = g_rightloopPITCHshort;
hitonari	0:2b57931c6ed4	2132	autopwm[RUD]=g_rightloopshortRUD;
hitonari	0:2b57931c6ed4	2133	autopwm[THR] = SetTHRinRatio(g_rightloop_THR_rate);
hitonari	0:2b57931c6ed4	2134	if(autopwm[AIL_R]<trimpwm[AIL_R]){
hitonari	0:2b57931c6ed4	2135	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
hitonari	0:2b57931c6ed4	2136	}
hitonari	0:2b57931c6ed4	2137	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2138
hitonari	0:2b57931c6ed4	2139	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2140	}
hitonari	0:2b57931c6ed4	2141
hitonari	0:2b57931c6ed4	2142
hitonari	0:2b57931c6ed4	2143	//左旋回
hitonari	0:2b57931c6ed4	2144	void UpdateTargetAngle_Leftloop(float targetAngle[3])
hitonari	0:2b57931c6ed4	2145	{
hitonari	0:2b57931c6ed4	2146
hitonari	0:2b57931c6ed4	2147	targetAngle[ROLL] = g_leftloopROLL;
hitonari	0:2b57931c6ed4	2148	targetAngle[PITCH] = g_leftloopPITCH;
hitonari	0:2b57931c6ed4	2149	//autopwm[ELE] = 1700;
hitonari	0:2b57931c6ed4	2150	autopwm[RUD]=g_leftloopRUD;
hitonari	0:2b57931c6ed4	2151	autopwm[THR] = SetTHRinRatio(g_leftloop_THR_rate);
hitonari	0:2b57931c6ed4	2152	if(autopwm[AIL_R]<trimpwm[AIL_R]){
hitonari	0:2b57931c6ed4	2153	autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
hitonari	0:2b57931c6ed4	2154	}
hitonari	0:2b57931c6ed4	2155	else autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2156
hitonari	0:2b57931c6ed4	2157	//checkHeight(targetAngle);
hitonari	0:2b57931c6ed4	2158
hitonari	0:2b57931c6ed4	2159	//printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2160	}
hitonari	0:2b57931c6ed4	2161
hitonari	0:2b57931c6ed4	2162	//左旋回（着陸時スロットル０のとき）
hitonari	0:2b57931c6ed4	2163	void UpdateTargetAngle_Leftloop_zero(float targetAngle[3])
hitonari	0:2b57931c6ed4	2164	{
hitonari	0:2b57931c6ed4	2165
hitonari	0:2b57931c6ed4	2166	targetAngle[ROLL] = g_leftloopROLL_approach;
hitonari	0:2b57931c6ed4	2167	targetAngle[PITCH] = g_leftloopPITCH_approach;
hitonari	0:2b57931c6ed4	2168	autopwm[RUD]=g_leftloopRUD_approach;
hitonari	0:2b57931c6ed4	2169	autopwm[THR] = minpwm[THR];
hitonari	0:2b57931c6ed4	2170	if(autopwm[AIL_R]<trimpwm[AIL_R]){
hitonari	0:2b57931c6ed4	2171	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
hitonari	0:2b57931c6ed4	2172	}
hitonari	0:2b57931c6ed4	2173	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2174
hitonari	0:2b57931c6ed4	2175	//autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop;
hitonari	0:2b57931c6ed4	2176	//checkHeight(targetAngle);
hitonari	0:2b57931c6ed4	2177
hitonari	0:2b57931c6ed4	2178	//printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2179	}
hitonari	0:2b57931c6ed4	2180
hitonari	0:2b57931c6ed4	2181	void UpdateTargetAngle_Leftloop_short(float targetAngle[3])
hitonari	0:2b57931c6ed4	2182	{
hitonari	0:2b57931c6ed4	2183
hitonari	0:2b57931c6ed4	2184	targetAngle[ROLL] = g_leftloopROLLshort;
hitonari	0:2b57931c6ed4	2185	targetAngle[PITCH] = g_leftloopPITCHshort;
hitonari	0:2b57931c6ed4	2186	autopwm[RUD]=g_leftloopRUD;
hitonari	0:2b57931c6ed4	2187	autopwm[THR] = SetTHRinRatio(g_leftloop_THR_rate);
hitonari	0:2b57931c6ed4	2188	if(autopwm[AIL_R]<trimpwm[AIL_R]){
hitonari	0:2b57931c6ed4	2189	autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
hitonari	0:2b57931c6ed4	2190	}
hitonari	0:2b57931c6ed4	2191	else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時
hitonari	0:2b57931c6ed4	2192
hitonari	0:2b57931c6ed4	2193	//pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
hitonari	0:2b57931c6ed4	2194	}
hitonari	0:2b57931c6ed4	2195
hitonari	0:2b57931c6ed4	2196	void Ascending(float targetAngle[3]){
hitonari	0:2b57931c6ed4	2197	static uint8_t RotateCounter=0;
hitonari	0:2b57931c6ed4	2198	static bool LapCounter = false;
hitonari	0:2b57931c6ed4	2199	static bool UpFlag = false;
hitonari	0:2b57931c6ed4	2200	static bool PressFlag = false;
hitonari	0:2b57931c6ed4	2201	static float FirstHeight = 0;
hitonari	0:2b57931c6ed4	2202	static bool flg_setInStartAuto = false;
hitonari	0:2b57931c6ed4	2203	//static float Time = 0.0;
hitonari	0:2b57931c6ed4	2204	static float FirstYaw_Ascending = 0;
hitonari	0:2b57931c6ed4	2205	float newYaw_Ascending;
hitonari	0:2b57931c6ed4	2206
hitonari	0:2b57931c6ed4	2207	//pc.printf("FirstHeight = %f g_z_comp = %f g_AscendingFlag = %d\r\n",FirstHeight,g_z_comp,g_AscendingFlag);
hitonari	0:2b57931c6ed4	2208
hitonari	0:2b57931c6ed4	2209	getBMP280();
hitonari	0:2b57931c6ed4	2210
hitonari	0:2b57931c6ed4	2211	if(!flg_setInStartAuto && CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	2212	FirstYaw_Ascending = nowAngle[YAW];
hitonari	0:2b57931c6ed4	2213	RotateCounter = 0;
hitonari	0:2b57931c6ed4	2214	flg_setInStartAuto = true;
hitonari	0:2b57931c6ed4	2215	LapCounter = false;
hitonari	0:2b57931c6ed4	2216	UpFlag = false;
hitonari	0:2b57931c6ed4	2217	PressFlag = false;
hitonari	0:2b57931c6ed4	2218	//tBMP.reset();
hitonari	0:2b57931c6ed4	2219	} else if(!CheckSW_Up(Ch7)) {
hitonari	0:2b57931c6ed4	2220	flg_setInStartAuto = false;
hitonari	0:2b57931c6ed4	2221	led4 = 0;
hitonari	0:2b57931c6ed4	2222	}
hitonari	0:2b57931c6ed4	2223
hitonari	0:2b57931c6ed4	2224	newYaw_Ascending = TranslateNewYaw(nowAngle[YAW], FirstYaw_Ascending);
hitonari	0:2b57931c6ed4	2225
hitonari	0:2b57931c6ed4	2226	if(RotateCounter == 0 && newYaw_Ascending >90.0 && newYaw_Ascending < 180.0) {
hitonari	0:2b57931c6ed4	2227	RotateCounter++;
hitonari	0:2b57931c6ed4	2228	led4 = 1;
hitonari	0:2b57931c6ed4	2229	pc.printf("Rotate 90\r\n");
hitonari	0:2b57931c6ed4	2230	}
hitonari	0:2b57931c6ed4	2231	if(RotateCounter == 1 && newYaw_Ascending >-180.0 && newYaw_Ascending < -90.0) {
hitonari	0:2b57931c6ed4	2232	RotateCounter++;
hitonari	0:2b57931c6ed4	2233	led4 = 0;
hitonari	0:2b57931c6ed4	2234	pc.printf("Rotate 180\r\n");
hitonari	0:2b57931c6ed4	2235	}
hitonari	0:2b57931c6ed4	2236	if(RotateCounter == 2 && newYaw_Ascending >-90.0 && newYaw_Ascending <-20.0) {
hitonari	0:2b57931c6ed4	2237	RotateCounter++;
hitonari	0:2b57931c6ed4	2238	led4 = 1;
hitonari	0:2b57931c6ed4	2239	pc.printf("Rotate 270\r\n");
hitonari	0:2b57931c6ed4	2240	}
hitonari	0:2b57931c6ed4	2241	if(RotateCounter == 3 && newYaw_Ascending >-10.0 && newYaw_Ascending < 90.0) {
hitonari	0:2b57931c6ed4	2242	RotateCounter++;
hitonari	0:2b57931c6ed4	2243	led4 = 0;
hitonari	0:2b57931c6ed4	2244	if(LapCounter == false){
hitonari	0:2b57931c6ed4	2245	RotateCounter = 0;
hitonari	0:2b57931c6ed4	2246	LapCounter = true;
hitonari	0:2b57931c6ed4	2247	pc.printf("Lap2\r\n");
hitonari	0:2b57931c6ed4	2248	}
hitonari	0:2b57931c6ed4	2249	}
hitonari	0:2b57931c6ed4	2250
hitonari	0:2b57931c6ed4	2251	if(RotateCounter <= 3){
hitonari	0:2b57931c6ed4	2252	UpdateTargetAngle_Rightloop(targetAngle);
hitonari	0:2b57931c6ed4	2253	}else if(3 < RotateCounter && UpFlag == false){
hitonari	0:2b57931c6ed4	2254	if(PressFlag == false){
hitonari	0:2b57931c6ed4	2255	FirstHeight = g_z_comp;
hitonari	0:2b57931c6ed4	2256	//tBMP.start();
hitonari	0:2b57931c6ed4	2257	PressFlag = true;
hitonari	0:2b57931c6ed4	2258	led4 = 1;
hitonari	0:2b57931c6ed4	2259	}
hitonari	0:2b57931c6ed4	2260	//Time = tBMP.read();
hitonari	0:2b57931c6ed4	2261	if(g_z_comp > FirstHeight+3000){
hitonari	0:2b57931c6ed4	2262	UpFlag = true;
hitonari	0:2b57931c6ed4	2263	led4 = 0;
hitonari	0:2b57931c6ed4	2264	}
hitonari	0:2b57931c6ed4	2265	UpdateTargetAngle_Rightloop_UP(targetAngle);
hitonari	0:2b57931c6ed4	2266	}else{
hitonari	0:2b57931c6ed4	2267	UpdateTargetAngle_Rightloop(targetAngle);
hitonari	0:2b57931c6ed4	2268	}
hitonari	0:2b57931c6ed4	2269
hitonari	0:2b57931c6ed4	2270
hitonari	0:2b57931c6ed4	2271	}
hitonari	0:2b57931c6ed4	2272
hitonari	0:2b57931c6ed4	2273	void Sbusprintf()
hitonari	0:2b57931c6ed4	2274	{
hitonari	0:2b57931c6ed4	2275
hitonari	0:2b57931c6ed4	2276	for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]);
hitonari	0:2b57931c6ed4	2277	pc.printf("\r\n");
hitonari	0:2b57931c6ed4	2278
hitonari	0:2b57931c6ed4	2279	}
hitonari	0:2b57931c6ed4	2280
hitonari	0:2b57931c6ed4	2281
hitonari	0:2b57931c6ed4	2282	//---sensing---
hitonari	0:2b57931c6ed4	2283	//センサーの値を読み込み、各種フィルタをかける(認知)
hitonari	0:2b57931c6ed4	2284	void sensing()
hitonari	0:2b57931c6ed4	2285	{
hitonari	0:2b57931c6ed4	2286	// If intPin goes high, all data registers have new data
hitonari	0:2b57931c6ed4	2287	if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
hitonari	0:2b57931c6ed4	2288
hitonari	0:2b57931c6ed4	2289	mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
hitonari	0:2b57931c6ed4	2290	// Now we'll calculate the accleration value into actual g's
hitonari	0:2b57931c6ed4	2291	ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
hitonari	0:2b57931c6ed4	2292	ay = (float)accelCount[1]*aRes - accelBias[1];
hitonari	0:2b57931c6ed4	2293	az = (float)accelCount[2]*aRes - accelBias[2];
hitonari	0:2b57931c6ed4	2294
hitonari	0:2b57931c6ed4	2295	mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
hitonari	0:2b57931c6ed4	2296	// Calculate the gyro value into actual degrees per second
hitonari	0:2b57931c6ed4	2297	gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
hitonari	0:2b57931c6ed4	2298	gy = (float)gyroCount[1]*gRes - gyroBias[1];
hitonari	0:2b57931c6ed4	2299	gz = (float)gyroCount[2]*gRes - gyroBias[2];
hitonari	0:2b57931c6ed4	2300
hitonari	0:2b57931c6ed4	2301	mpu9250.readMagData(magCount); // Read the x/y/z adc values
hitonari	0:2b57931c6ed4	2302	// Calculate the magnetometer values in milliGauss
hitonari	0:2b57931c6ed4	2303	// Include factory calibration per data sheet and user environmental corrections
hitonari	0:2b57931c6ed4	2304	mx = (float)magCount[0]mResmagCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
hitonari	0:2b57931c6ed4	2305	my = (float)magCount[1]mResmagCalibration[1] - magbias[1];
hitonari	0:2b57931c6ed4	2306	mz = (float)magCount[2]mResmagCalibration[2] - magbias[2];
hitonari	0:2b57931c6ed4	2307	}
hitonari	0:2b57931c6ed4	2308
hitonari	0:2b57931c6ed4	2309	Now = t.read_us();
hitonari	0:2b57931c6ed4	2310	deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
hitonari	0:2b57931c6ed4	2311	lastUpdate = Now;
hitonari	0:2b57931c6ed4	2312
hitonari	0:2b57931c6ed4	2313	g_sum += deltat;
hitonari	0:2b57931c6ed4	2314	g_sumCount++;
hitonari	0:2b57931c6ed4	2315
hitonari	0:2b57931c6ed4	2316	// if(lastUpdate - firstUpdate > 10000000.0f) {
hitonari	0:2b57931c6ed4	2317	// beta = 0.04; // decrease filter gain after stabilized
hitonari	0:2b57931c6ed4	2318	// zeta = 0.015; // increasey bias drift gain after stabilized
hitonari	0:2b57931c6ed4	2319	// }
hitonari	0:2b57931c6ed4	2320
hitonari	0:2b57931c6ed4	2321	// Pass gyro rate as rad/s
hitonari	0:2b57931c6ed4	2322	mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gz*PI/180.0f, mx, my, mz);
hitonari	0:2b57931c6ed4	2323	// mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gz*PI/180.0f, my, mx, mz);
hitonari	0:2b57931c6ed4	2324
hitonari	0:2b57931c6ed4	2325	mpu9250.MahonyQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gz*PI/180.0f, mx, my, mz);
hitonari	0:2b57931c6ed4	2326	// mpu9250.MahonyQuaternionUpdate(ax, ay, az, gxPI/180.0f, gyPI/180.0f, gz*PI/180.0f, my, mx, mz);
hitonari	0:2b57931c6ed4	2327
hitonari	0:2b57931c6ed4	2328	/* Auto_Loop()で50ms待ってるから要らないと思う
hitonari	0:2b57931c6ed4	2329	// Serial print and/or display at 0.5 s rate independent of data rates
hitonari	0:2b57931c6ed4	2330	delt_t = t.read_ms() - count;
hitonari	0:2b57931c6ed4	2331	if (delt_t > 500) { // update LCD once per half-second independent of read rate
hitonari	0:2b57931c6ed4	2332	*/
hitonari	0:2b57931c6ed4	2333
hitonari	0:2b57931c6ed4	2334	//pc.printf("ax = %f", 1000*ax);
hitonari	0:2b57931c6ed4	2335	//pc.printf(" ay = %f", 1000*ay);
hitonari	0:2b57931c6ed4	2336	//pc.printf(" az = %f mg\n\r", 1000*az);
hitonari	0:2b57931c6ed4	2337
hitonari	0:2b57931c6ed4	2338	//pc.printf("gx = %f", gx);
hitonari	0:2b57931c6ed4	2339	//pc.printf(" gy = %f", gy);
hitonari	0:2b57931c6ed4	2340	//pc.printf(" gz = %f deg/s\n\r", gz);
hitonari	0:2b57931c6ed4	2341
hitonari	0:2b57931c6ed4	2342	//pc.printf("mx = %f,", mx);
hitonari	0:2b57931c6ed4	2343	//pc.printf(" my = %f,", my);
hitonari	0:2b57931c6ed4	2344	//pc.printf(" mz = %f mG\n\r", mz);
hitonari	0:2b57931c6ed4	2345
hitonari	0:2b57931c6ed4	2346	tempCount = mpu9250.readTempData(); // Read the adc values
hitonari	0:2b57931c6ed4	2347	temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
hitonari	0:2b57931c6ed4	2348	//pc.printf(" temperature = %f C\n\r", temperature);
hitonari	0:2b57931c6ed4	2349
hitonari	0:2b57931c6ed4	2350	//pc.printf("q0 = %f\n\r", q[0]);
hitonari	0:2b57931c6ed4	2351	//pc.printf("q1 = %f\n\r", q[1]);
hitonari	0:2b57931c6ed4	2352	//pc.printf("q2 = %f\n\r", q[2]);
hitonari	0:2b57931c6ed4	2353	//pc.printf("q3 = %f\n\r", q[3]);
hitonari	0:2b57931c6ed4	2354
hitonari	0:2b57931c6ed4	2355
hitonari	0:2b57931c6ed4	2356	// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
hitonari	0:2b57931c6ed4	2357	// In this coordinate system, the positive z-axis is down toward Earth.
hitonari	0:2b57931c6ed4	2358	// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
hitonari	0:2b57931c6ed4	2359	// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
hitonari	0:2b57931c6ed4	2360	// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
hitonari	0:2b57931c6ed4	2361	// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
hitonari	0:2b57931c6ed4	2362	// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
hitonari	0:2b57931c6ed4	2363	// applied in the correct order which for this configuration is yaw, pitch, and then roll.
hitonari	0:2b57931c6ed4	2364	// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
hitonari	0:2b57931c6ed4	2365	yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
hitonari	0:2b57931c6ed4	2366	roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
hitonari	0:2b57931c6ed4	2367	pitch = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
hitonari	0:2b57931c6ed4	2368	pitch *= 180.0f / PI;
hitonari	0:2b57931c6ed4	2369	yaw *= 180.0f / PI;
hitonari	0:2b57931c6ed4	2370	yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
hitonari	0:2b57931c6ed4	2371	roll *= 180.0f / PI;
hitonari	0:2b57931c6ed4	2372
hitonari	0:2b57931c6ed4	2373	//pc.printf("$%d %d %d;",(int)(yaw100),(int)(pitch100),(int)(roll*100));
hitonari	0:2b57931c6ed4	2374	//pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll);
hitonari	0:2b57931c6ed4	2375	//pc.printf("average rate = %f\n\r", (float) sumCount/sum);
hitonari	0:2b57931c6ed4	2376
hitonari	0:2b57931c6ed4	2377	count = t.read_ms();
hitonari	0:2b57931c6ed4	2378	g_sum = 0;
hitonari	0:2b57931c6ed4	2379	g_sumCount = 0;
hitonari	0:2b57931c6ed4	2380	//}
hitonari	0:2b57931c6ed4	2381	}
hitonari	0:2b57931c6ed4	2382
hitonari	0:2b57931c6ed4	2383	void SetupBMP280(){
hitonari	0:2b57931c6ed4	2384	bmp.initialize(BMP280::INDOOR_NAVIGATION);
hitonari	0:2b57931c6ed4	2385
hitonari	0:2b57931c6ed4	2386	for(int sn = 0; sn < 100; sn++){
hitonari	0:2b57931c6ed4	2387	g_press = bmp.getPressure();
hitonari	0:2b57931c6ed4	2388	if(sn > 0){
hitonari	0:2b57931c6ed4	2389	g_InitPress[sn] = g_press;
hitonari	0:2b57931c6ed4	2390	g_InitPressAve += g_InitPress[sn];
hitonari	0:2b57931c6ed4	2391	float press_ratio = g_st_press / g_press;
hitonari	0:2b57931c6ed4	2392	g_InitBMPHeight[sn] = ((powf(press_ratio, 0.19) - 1) * 298.15 * 1000) / 0.0065;
hitonari	0:2b57931c6ed4	2393	g_InitBMPHeightAve += g_InitBMPHeight[sn];
hitonari	0:2b57931c6ed4	2394
hitonari	0:2b57931c6ed4	2395	}
hitonari	0:2b57931c6ed4	2396	pc.printf("%f,%f\r\n", g_InitBMPHeight[sn],g_InitPress[sn]);
hitonari	0:2b57931c6ed4	2397	wait(0.05);
hitonari	0:2b57931c6ed4	2398	}
hitonari	0:2b57931c6ed4	2399
hitonari	0:2b57931c6ed4	2400	g_InitPressAve /= 100.0;
hitonari	0:2b57931c6ed4	2401
hitonari	0:2b57931c6ed4	2402	g_InitBMPHeightAve /= 100.0;
hitonari	0:2b57931c6ed4	2403
hitonari	0:2b57931c6ed4	2404	pc.printf("\r\n%f,%f\r\n\r\n", g_InitBMPHeightAve,g_InitPressAve);
hitonari	0:2b57931c6ed4	2405
hitonari	0:2b57931c6ed4	2406	pc.printf("bmp280 initialized\r\n");
hitonari	0:2b57931c6ed4	2407
hitonari	0:2b57931c6ed4	2408	g_BMPheight = g_InitBMPHeightAve;
hitonari	0:2b57931c6ed4	2409
hitonari	0:2b57931c6ed4	2410	tBMP.start();//dtを計測
hitonari	0:2b57931c6ed4	2411
hitonari	0:2b57931c6ed4	2412	sensing();//最初だけ空回し
hitonari	0:2b57931c6ed4	2413
hitonari	0:2b57931c6ed4	2414	g_tempaz[0] = az * g_grav * 1000;
hitonari	0:2b57931c6ed4	2415
hitonari	0:2b57931c6ed4	2416	}
hitonari	0:2b57931c6ed4	2417
hitonari	0:2b57931c6ed4	2418	void getBMP280(){
hitonari	0:2b57931c6ed4	2419	float z_angle, gyro_ver, z_angle_prev;
hitonari	0:2b57931c6ed4	2420	static float press_rel_coef = 0.03;
hitonari	0:2b57931c6ed4	2421	float press_ratio = g_st_press / g_press;
hitonari	0:2b57931c6ed4	2422
hitonari	0:2b57931c6ed4	2423	g_z_PressHeight = g_BMPheight;
hitonari	0:2b57931c6ed4	2424
hitonari	0:2b57931c6ed4	2425	z_angle = atan2(sqrt(ax * ax + ay * ay), az);
hitonari	0:2b57931c6ed4	2426	gyro_ver = cos(z_angle); //垂直方向の加速度
hitonari	0:2b57931c6ed4	2427
hitonari	0:2b57931c6ed4	2428	g_dt = tBMP.read_us();
hitonari	0:2b57931c6ed4	2429	g_dt /= 1000000;
hitonari	0:2b57931c6ed4	2430	tBMP.reset();
hitonari	0:2b57931c6ed4	2431	tBMP.start();
hitonari	0:2b57931c6ed4	2432
hitonari	0:2b57931c6ed4	2433
hitonari	0:2b57931c6ed4	2434	g_z_AccelHeight = g_z_prev - g_v_prev * g_dt - (az - gyro_ver) * g_grav 1000 g_dt * g_dt / 2;
hitonari	0:2b57931c6ed4	2435
hitonari	0:2b57931c6ed4	2436	g_z_comp = g_z_PressHeight* press_rel_coef + g_z_AccelHeight* (1 - press_rel_coef);//係数は足して1になるように適当に設定,単位はmm
hitonari	0:2b57931c6ed4	2437
hitonari	0:2b57931c6ed4	2438	//pc.printf("%f\r\n",g_z_comp/1000);
hitonari	0:2b57931c6ed4	2439
hitonari	0:2b57931c6ed4	2440	g_BMPheight_int = g_z_comp;
hitonari	0:2b57931c6ed4	2441
hitonari	0:2b57931c6ed4	2442	g_press = bmp.getPressure();
hitonari	0:2b57931c6ed4	2443
hitonari	0:2b57931c6ed4	2444	g_BMPheight = ((powf(press_ratio, 0.19) - 1) * 298.15 *1000) / 0.0065 - g_InitBMPHeightAve;
hitonari	0:2b57931c6ed4	2445
hitonari	0:2b57931c6ed4	2446	NVIC_DisableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	2447	NVIC_DisableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	2448	NVIC_DisableIRQ(TIM5_IRQn);
hitonari	0:2b57931c6ed4	2449	NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
hitonari	0:2b57931c6ed4	2450	NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
hitonari	0:2b57931c6ed4	2451	sensing();
hitonari	0:2b57931c6ed4	2452	NVIC_EnableIRQ(USART1_IRQn);
hitonari	0:2b57931c6ed4	2453	NVIC_EnableIRQ(USART2_IRQn);
hitonari	0:2b57931c6ed4	2454	NVIC_EnableIRQ(TIM5_IRQn);
hitonari	0:2b57931c6ed4	2455	NVIC_EnableIRQ(EXTI0_IRQn);
hitonari	0:2b57931c6ed4	2456	NVIC_EnableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	2457
hitonari	0:2b57931c6ed4	2458	g_v_prev = (g_z_comp - g_z_prev) / g_dt / 1000000;
hitonari	0:2b57931c6ed4	2459
hitonari	0:2b57931c6ed4	2460	g_z_prev = g_z_comp;
hitonari	0:2b57931c6ed4	2461
hitonari	0:2b57931c6ed4	2462
hitonari	0:2b57931c6ed4	2463	//wait_ms(1.0);
hitonari	0:2b57931c6ed4	2464
hitonari	0:2b57931c6ed4	2465	}
hitonari	0:2b57931c6ed4	2466
hitonari	0:2b57931c6ed4	2467
hitonari	0:2b57931c6ed4	2468
hitonari	0:2b57931c6ed4	2469	//デバッグ用=
hitonari	0:2b57931c6ed4	2470	void DebugPrint()
hitonari	0:2b57931c6ed4	2471	{
hitonari	0:2b57931c6ed4	2472	/*
hitonari	0:2b57931c6ed4	2473	static int16_t deltaT = 0, t_start = 0;
hitonari	0:2b57931c6ed4	2474	//deltaT = t.read_u2s() - t_start;
hitonari	0:2b57931c6ed4	2475	pc.printf("t:%d us, ",deltaT);
hitonari	0:2b57931c6ed4	2476	pc.printf("\r\n");
hitonari	0:2b57931c6ed4	2477	t_start = t.read_us();
hitonari	0:2b57931c6ed4	2478
hitonari	0:2b57931c6ed4	2479	for(uint8_t i=0; i<6; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
hitonari	0:2b57931c6ed4	2480	if(g_trim_check){
hitonari	0:2b57931c6ed4	2481	for(uint8_t i=0; i<6; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
hitonari	0:2b57931c6ed4	2482	pc.printf("\r\n");
hitonari	0:2b57931c6ed4	2483	}
hitonari	0:2b57931c6ed4	2484
hitonari	0:2b57931c6ed4	2485	pc.printf("trimpwm[AIL_R]:%d ELE:%d THR:%d RUD:%d 4:%d AIL_L:%d",trimpwm[AIL_R],trimpwm[ELE],trimpwm[THR],trimpwm[RUD],trimpwm[4],trimpwm[AIL_L]);
hitonari	0:2b57931c6ed4	2486	for(uint8_t i=0; i<8; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
hitonari	0:2b57931c6ed4	2487	for(uint8_t i=1; i<4; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]);
hitonari	0:2b57931c6ed4	2488	pc.printf("\r\n");
hitonari	0:2b57931c6ed4	2489	for(uint8_t i=0; i<3; i++) pc.printf("nowangle[%d]:%3.2f\t",i,nowAngle[i]);
hitonari	0:2b57931c6ed4	2490	for(uint8_t i=0; i<2; i++) pc.printf("nowangle[%d]:%3.2f\t",i,nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
hitonari	0:2b57931c6ed4	2491	pc.printf("nowangle[pitch]:%3.2f\t\r\n",nowAngle[2]);
hitonari	0:2b57931c6ed4	2492	pc.printf("autopwm[ELE]:%d\t\r\n",autopwm[ELE]);
hitonari	0:2b57931c6ed4	2493	pc.printf("autpwm[RUD]:%d\t",autopwm[RUD]);*/
hitonari	0:2b57931c6ed4	2494	NVIC_DisableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	2495	pc.printf("g_distance:%d\t",g_distance);/*
hitonari	0:2b57931c6ed4	2496	NVIC_EnableIRQ(EXTI9_5_IRQn);
hitonari	0:2b57931c6ed4	2497	pc.printf("Mode: %c: ",g_buf[0]);
hitonari	0:2b57931c6ed4	2498	if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
hitonari	0:2b57931c6ed4	2499	pc.printf("%f",g_z_comp);
hitonari	0:2b57931c6ed4	2500	pc.printf("\r\n%f,%f\r\n", g_InitBMPHeightAve,g_InitPressAve);
hitonari	0:2b57931c6ed4	2501	pc.printf("\r\n"); */
hitonari	0:2b57931c6ed4	2502	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	13 Sep 2022
Imports:	2
Forks:	0
Commits:	3
Dependents:	0
Dependencies:	5
Followers:	23

main.cpp@0:2b57931c6ed4, 2022-09-13 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning