9/13 updated

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_4_Approach by 航空研究会

Committer:
HARUKIDELTA
Date:
Thu Sep 13 06:49:58 2018 +0000
Revision:
5:eca3adcd8f9c
Parent:
4:b66ab0bee119
SD setting

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