航空研究会 / Mbed 2 deprecated AutoFlight2017_now_copy

s

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2017_now_copy by Bot Furukawa

main.cpp@1:f31e17341659, 2018-08-28 (annotated)

Committer:
TUATBM
Date:
Tue Aug 28 07:12:16 2018 +0000
Revision:
1:f31e17341659
Parent:
0:92024886c0be
Child:
2:e7025f2cf0e1
aaa

Who changed what in which revision?

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