航空研究会 / Mbed 2 deprecated AutoFlight2017_now_copy

s

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2017_now_copy by Bot Furukawa

main.cpp@2:e7025f2cf0e1, 2018-09-08 (annotated)

Committer:
TUATBM
Date:
Sat Sep 08 09:21:46 2018 +0000
Revision:
2:e7025f2cf0e1
Parent:
1:f31e17341659 
a

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