確認用

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_29 by 航空研究会

Committer:
HARUKIDELTA
Date:
Wed Sep 19 06:37:31 2018 +0000
Revision:
22:438bedf24707
Parent:
21:18fe7bf9e187
Child:
23:4928a6fd9eee
AILERON

Who changed what in which revision?

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