航空研究会 / Mbed 2 deprecated sbus_test_2018

a

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2018_Control by 航空研究会

main.cpp@0:813f5cd20cf1, 2018-09-05 (annotated)

Committer:
HARUKIDELTA
Date:
Wed Sep 05 05:40:11 2018 +0000
Revision:
0:813f5cd20cf1
Child:
1:09a162f4f6ce
as

Who changed what in which revision?

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