航空研究会 / Mbed 2 deprecated sbus_test_2018

a

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2018_Control by 航空研究会

main.cpp@1:09a162f4f6ce, 2018-09-07 (annotated)

Committer:
TUATBM
Date:
Fri Sep 07 03:42:49 2018 +0000
Revision:
1:09a162f4f6ce
Parent:
0:813f5cd20cf1 
a

Who changed what in which revision?

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