test
Dependencies: BMP280 HCSR04_from_mtmt MPU6050_2 mbed SDFileSystem3
main.cpp@0:2b57931c6ed4, 2022-09-13 (annotated)
- Committer:
- hitonari
- Date:
- Tue Sep 13 03:12:26 2022 +0000
- Revision:
- 0:2b57931c6ed4
- Child:
- 1:e7079f9771d3
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hitonari | 0:2b57931c6ed4 | 1 | //mbed |
hitonari | 0:2b57931c6ed4 | 2 | #include "mbed.h" |
hitonari | 0:2b57931c6ed4 | 3 | #include "FATFileSystem.h" |
hitonari | 0:2b57931c6ed4 | 4 | #include "SDFileSystem.h" |
hitonari | 0:2b57931c6ed4 | 5 | //C |
hitonari | 0:2b57931c6ed4 | 6 | #include "math.h" |
hitonari | 0:2b57931c6ed4 | 7 | //sensor |
hitonari | 0:2b57931c6ed4 | 8 | #include "MPU6050_DMP6.h" |
hitonari | 0:2b57931c6ed4 | 9 | //#include "MPU9250.h" |
hitonari | 0:2b57931c6ed4 | 10 | #include "MPU9250_BMP.h" |
hitonari | 0:2b57931c6ed4 | 11 | #include "BMP280.h" |
hitonari | 0:2b57931c6ed4 | 12 | #include "hcsr04.h" |
hitonari | 0:2b57931c6ed4 | 13 | //device |
hitonari | 0:2b57931c6ed4 | 14 | #include "sbus.h" |
hitonari | 0:2b57931c6ed4 | 15 | //config |
hitonari | 0:2b57931c6ed4 | 16 | #include "SkipperSv2.h" |
hitonari | 0:2b57931c6ed4 | 17 | #include "falfalla.h" |
hitonari | 0:2b57931c6ed4 | 18 | //other |
hitonari | 0:2b57931c6ed4 | 19 | #include "pid.h" |
hitonari | 0:2b57931c6ed4 | 20 | |
hitonari | 0:2b57931c6ed4 | 21 | #define DEBUG_SEMIAUTO 0 |
hitonari | 0:2b57931c6ed4 | 22 | #define DEBUG_PRINT_INLOOP 1 |
hitonari | 0:2b57931c6ed4 | 23 | |
hitonari | 0:2b57931c6ed4 | 24 | #define KP_ELE 16.5//15.0 //2.0 |
hitonari | 0:2b57931c6ed4 | 25 | #define KI_ELE 0.0 |
hitonari | 0:2b57931c6ed4 | 26 | #define KD_ELE 0.1 //0/0 |
hitonari | 0:2b57931c6ed4 | 27 | #define KP_RUD 3.0 |
hitonari | 0:2b57931c6ed4 | 28 | #define KI_RUD 0.0 |
hitonari | 0:2b57931c6ed4 | 29 | #define KD_RUD 0.0 |
hitonari | 0:2b57931c6ed4 | 30 | #define KP_AIL 0.11//0.1 |
hitonari | 0:2b57931c6ed4 | 31 | #define KI_AIL 0.2 |
hitonari | 0:2b57931c6ed4 | 32 | #define KD_AIL 0.2 |
hitonari | 0:2b57931c6ed4 | 33 | |
hitonari | 0:2b57931c6ed4 | 34 | //#define g_AIL_L_Ratio_rightloop 0.5 |
hitonari | 0:2b57931c6ed4 | 35 | |
hitonari | 0:2b57931c6ed4 | 36 | #define GAIN_CONTROLVALUE_TO_PWM 3.0 |
hitonari | 0:2b57931c6ed4 | 37 | |
hitonari | 0:2b57931c6ed4 | 38 | #define RIGHT_ROLL 5.0//10 |
hitonari | 0:2b57931c6ed4 | 39 | #define RIGHT_PITCH 45//-20.0//-22//-24.0 //5.0 |
hitonari | 0:2b57931c6ed4 | 40 | #define LEFT_ROLL 15//-38.0//30 |
hitonari | 0:2b57931c6ed4 | 41 | |
hitonari | 0:2b57931c6ed4 | 42 | #define LEFT_PITCH 10//-15.0 |
hitonari | 0:2b57931c6ed4 | 43 | #define STRAIGHT_ROLL 0.0//12.0//8.0 |
hitonari | 0:2b57931c6ed4 | 44 | #define STRAIGHT_PITCH 3.0 |
hitonari | 0:2b57931c6ed4 | 45 | #define TAKEOFF_THR 0.8 |
hitonari | 0:2b57931c6ed4 | 46 | #define LOOP_THR 0.6 |
hitonari | 0:2b57931c6ed4 | 47 | |
hitonari | 0:2b57931c6ed4 | 48 | //#define g_rightloopRUD 1500 |
hitonari | 0:2b57931c6ed4 | 49 | |
hitonari | 0:2b57931c6ed4 | 50 | #define RIGHT_ROLL_SHORT 10.0 |
hitonari | 0:2b57931c6ed4 | 51 | #define RIGHT_PITCH_SHORT -25.0 |
hitonari | 0:2b57931c6ed4 | 52 | #define LEFT_ROLL_SHORT -30.0 |
hitonari | 0:2b57931c6ed4 | 53 | #define LEFT_PITCH_SHORT -20.0 |
hitonari | 0:2b57931c6ed4 | 54 | #define RIGHTLOOPROLL_APPROACH 10.0 |
hitonari | 0:2b57931c6ed4 | 55 | #define LEFTLOOPROLL_APPROACH -30.0 |
hitonari | 0:2b57931c6ed4 | 56 | #define RIGHTLOOPPITCH_APPROACH -25.0 |
hitonari | 0:2b57931c6ed4 | 57 | #define LEFTLOOPPITCH_APPROACH -30.0 |
hitonari | 0:2b57931c6ed4 | 58 | |
hitonari | 0:2b57931c6ed4 | 59 | #define ASCENDING_PITCH -35.0 |
hitonari | 0:2b57931c6ed4 | 60 | #define TAKE_OFF_THROTTLE 1650 |
hitonari | 0:2b57931c6ed4 | 61 | #define TAKE_OFF_PITCH 50//-40.0 |
hitonari | 0:2b57931c6ed4 | 62 | |
hitonari | 0:2b57931c6ed4 | 63 | #define rightloopROLL2 -10.0 |
hitonari | 0:2b57931c6ed4 | 64 | |
hitonari | 0:2b57931c6ed4 | 65 | /*#define rightloopRUD 1300 //1250 |
hitonari | 0:2b57931c6ed4 | 66 | #define rightloopshortRUD 1250 |
hitonari | 0:2b57931c6ed4 | 67 | #define leftloopRUD 1500 |
hitonari | 0:2b57931c6ed4 | 68 | #define leftloopshortRUD 1500 |
hitonari | 0:2b57931c6ed4 | 69 | #define glideloopRUD 1300 |
hitonari | 0:2b57931c6ed4 | 70 | */ |
hitonari | 0:2b57931c6ed4 | 71 | #define AIL_R_correctionrightloop 0 |
hitonari | 0:2b57931c6ed4 | 72 | #define AIL_L_correctionrightloop 0 |
hitonari | 0:2b57931c6ed4 | 73 | #define AIL_L_correctionrightloopshort 0 |
hitonari | 0:2b57931c6ed4 | 74 | #define AIL_L_correctionleftloop -0 |
hitonari | 0:2b57931c6ed4 | 75 | #define AIL_L_correctionleftloopshort 0 |
hitonari | 0:2b57931c6ed4 | 76 | |
hitonari | 0:2b57931c6ed4 | 77 | |
hitonari | 0:2b57931c6ed4 | 78 | #define RIGHTLOOP_RUD 1190//1150 //1200 |
hitonari | 0:2b57931c6ed4 | 79 | #define RIGHTLOOPSHORT_RUD 1250 |
hitonari | 0:2b57931c6ed4 | 80 | #define LEFTLOOP_RUD 1560 //1680//1750//1700 |
hitonari | 0:2b57931c6ed4 | 81 | #define LEFTLOOPSHORT_RUD 1500 |
hitonari | 0:2b57931c6ed4 | 82 | #define GLIDELOOP_RUD 1300 |
hitonari | 0:2b57931c6ed4 | 83 | #define RIGHTLOOPRUD_APPROACH 1500 |
hitonari | 0:2b57931c6ed4 | 84 | #define LEFTLOOPRUD_APPROACH 1500 |
hitonari | 0:2b57931c6ed4 | 85 | #define RIGHTLOOP_THR_RATE 0.64//0.66 |
hitonari | 0:2b57931c6ed4 | 86 | #define LEFTLOOP_THR_RATE 0.49//0.62//0.66 |
hitonari | 0:2b57931c6ed4 | 87 | #define ASCENDING_THR_RATE 0.75 |
hitonari | 0:2b57931c6ed4 | 88 | #define ASCENDING_ROLL 16.0 |
hitonari | 0:2b57931c6ed4 | 89 | #define ASCENDING_RUD 1200 |
hitonari | 0:2b57931c6ed4 | 90 | |
hitonari | 0:2b57931c6ed4 | 91 | |
hitonari | 0:2b57931c6ed4 | 92 | #define AIL_L_CORRECTION_RIGHTLOOP 0 |
hitonari | 0:2b57931c6ed4 | 93 | #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0 |
hitonari | 0:2b57931c6ed4 | 94 | #define AIL_L_CORRECTION_LEFTLOOP 0 |
hitonari | 0:2b57931c6ed4 | 95 | #define AIL_L_CORRECTION_LEFTLOOPSHORT 0 |
hitonari | 0:2b57931c6ed4 | 96 | |
hitonari | 0:2b57931c6ed4 | 97 | |
hitonari | 0:2b57931c6ed4 | 98 | |
hitonari | 0:2b57931c6ed4 | 99 | #define TRIM1 1480 |
hitonari | 0:2b57931c6ed4 | 100 | #define TRIM2 1500 |
hitonari | 0:2b57931c6ed4 | 101 | #define TRIM3 1150 |
hitonari | 0:2b57931c6ed4 | 102 | #define TRIM4 1500 |
hitonari | 0:2b57931c6ed4 | 103 | #define TRIM5 1300 |
hitonari | 0:2b57931c6ed4 | 104 | #define TRIM6 1500 |
hitonari | 0:2b57931c6ed4 | 105 | #define TRIM_CHECK 0 |
hitonari | 0:2b57931c6ed4 | 106 | |
hitonari | 0:2b57931c6ed4 | 107 | #define GLIDE_ROLL -12.0 |
hitonari | 0:2b57931c6ed4 | 108 | #define GLIDE_PITCH -3.0 |
hitonari | 0:2b57931c6ed4 | 109 | |
hitonari | 0:2b57931c6ed4 | 110 | |
hitonari | 0:2b57931c6ed4 | 111 | #define AIL_L_RatioRising 0.5 |
hitonari | 0:2b57931c6ed4 | 112 | #define AIL_L_RatioDescent 2 |
hitonari | 0:2b57931c6ed4 | 113 | |
hitonari | 0:2b57931c6ed4 | 114 | //コンパスキャリブレーション |
hitonari | 0:2b57931c6ed4 | 115 | //SkipperS2基板 |
hitonari | 0:2b57931c6ed4 | 116 | /* |
hitonari | 0:2b57931c6ed4 | 117 | #define MAGBIAS_X -35.0 |
hitonari | 0:2b57931c6ed4 | 118 | #define MAGBIAS_Y 535.0 |
hitonari | 0:2b57931c6ed4 | 119 | #define MAGBIAS_Z -50.0 |
hitonari | 0:2b57931c6ed4 | 120 | */ |
hitonari | 0:2b57931c6ed4 | 121 | //S2v2 1番基板 |
hitonari | 0:2b57931c6ed4 | 122 | #define MAGBIAS_X 395.0 |
hitonari | 0:2b57931c6ed4 | 123 | #define MAGBIAS_Y 505.0 |
hitonari | 0:2b57931c6ed4 | 124 | #define MAGBIAS_Z -725.0 |
hitonari | 0:2b57931c6ed4 | 125 | //S2v2 2番基板 |
hitonari | 0:2b57931c6ed4 | 126 | /* |
hitonari | 0:2b57931c6ed4 | 127 | #define MAGBIAS_X 185.0 |
hitonari | 0:2b57931c6ed4 | 128 | #define MAGBIAS_Y 220.0 |
hitonari | 0:2b57931c6ed4 | 129 | #define MAGBIAS_Z -350.0 |
hitonari | 0:2b57931c6ed4 | 130 | */ |
hitonari | 0:2b57931c6ed4 | 131 | |
hitonari | 0:2b57931c6ed4 | 132 | #define ELEMENT 1 |
hitonari | 0:2b57931c6ed4 | 133 | #define LIMIT_STRAIGHT_YAW 5.0 |
hitonari | 0:2b57931c6ed4 | 134 | #define THRESHOLD_TURNINGRADIUS_YAW 60.0 |
hitonari | 0:2b57931c6ed4 | 135 | #define ALLOWHEIGHT 15 |
hitonari | 0:2b57931c6ed4 | 136 | |
hitonari | 0:2b57931c6ed4 | 137 | //気圧用 |
hitonari | 0:2b57931c6ed4 | 138 | #define SBUS_SIGNAL_OK 0x00 |
hitonari | 0:2b57931c6ed4 | 139 | #define SBUS_SIGNAL_LOST 0x01 |
hitonari | 0:2b57931c6ed4 | 140 | #define SBUS_SIGNAL_FAILSAFE 0x03 |
hitonari | 0:2b57931c6ed4 | 141 | MPU9250 mpu9250; |
hitonari | 0:2b57931c6ed4 | 142 | float g_sum = 0; //グローバルなので要注意!! |
hitonari | 0:2b57931c6ed4 | 143 | uint32_t g_sumCount = 0; |
hitonari | 0:2b57931c6ed4 | 144 | float g_grav = 9.80665; |
hitonari | 0:2b57931c6ed4 | 145 | float g_st_press = 1018.5; |
hitonari | 0:2b57931c6ed4 | 146 | float g_press, g_BMPheight; |
hitonari | 0:2b57931c6ed4 | 147 | int g_BMPheight_int = 0; |
hitonari | 0:2b57931c6ed4 | 148 | float g_InitPress[101]; |
hitonari | 0:2b57931c6ed4 | 149 | float g_InitPressAve = 0; |
hitonari | 0:2b57931c6ed4 | 150 | float g_InitBMPHeight[101]; |
hitonari | 0:2b57931c6ed4 | 151 | float g_InitBMPHeightAve = 0; |
hitonari | 0:2b57931c6ed4 | 152 | int g_temporary; |
hitonari | 0:2b57931c6ed4 | 153 | float g_dt = 0.0; |
hitonari | 0:2b57931c6ed4 | 154 | float g_v_prev = 0.0; |
hitonari | 0:2b57931c6ed4 | 155 | float g_z_prev; |
hitonari | 0:2b57931c6ed4 | 156 | float g_z_PressHeight, g_z_AccelHeight; |
hitonari | 0:2b57931c6ed4 | 157 | float g_z_comp; |
hitonari | 0:2b57931c6ed4 | 158 | float g_tempaz[2]; |
hitonari | 0:2b57931c6ed4 | 159 | |
hitonari | 0:2b57931c6ed4 | 160 | BMP280 bmp(PC_9, PA_8); |
hitonari | 0:2b57931c6ed4 | 161 | Timer tBMP; |
hitonari | 0:2b57931c6ed4 | 162 | |
hitonari | 0:2b57931c6ed4 | 163 | |
hitonari | 0:2b57931c6ed4 | 164 | #ifndef PI |
hitonari | 0:2b57931c6ed4 | 165 | #define PI 3.14159265358979 |
hitonari | 0:2b57931c6ed4 | 166 | #endif |
hitonari | 0:2b57931c6ed4 | 167 | |
hitonari | 0:2b57931c6ed4 | 168 | const int16_t lengthdivpwm = 320; |
hitonari | 0:2b57931c6ed4 | 169 | const int16_t changeModeCount = 6; |
hitonari | 0:2b57931c6ed4 | 170 | |
hitonari | 0:2b57931c6ed4 | 171 | |
hitonari | 0:2b57931c6ed4 | 172 | SBUS sbus(PA_9, PA_10); //SBUS |
hitonari | 0:2b57931c6ed4 | 173 | |
hitonari | 0:2b57931c6ed4 | 174 | PwmOut servo1(PC_6); // TIM3_CH1 //old echo |
hitonari | 0:2b57931c6ed4 | 175 | PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 |
hitonari | 0:2b57931c6ed4 | 176 | PwmOut servo3(PB_0); // TIM3_CH3 |
hitonari | 0:2b57931c6ed4 | 177 | PwmOut servo4(PB_1); // TIM3_CH4 |
hitonari | 0:2b57931c6ed4 | 178 | PwmOut servo5(PB_6); // TIM4_CH1 |
hitonari | 0:2b57931c6ed4 | 179 | PwmOut servo6(PB_7); // TIM4_CH2 //old trigger |
hitonari | 0:2b57931c6ed4 | 180 | //PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo |
hitonari | 0:2b57931c6ed4 | 181 | //PwmOut servo8(PB_9); // TIM4_CH4 //new trigger |
hitonari | 0:2b57931c6ed4 | 182 | |
hitonari | 0:2b57931c6ed4 | 183 | RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX |
hitonari | 0:2b57931c6ed4 | 184 | //RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用 |
hitonari | 0:2b57931c6ed4 | 185 | SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); |
hitonari | 0:2b57931c6ed4 | 186 | |
hitonari | 0:2b57931c6ed4 | 187 | DigitalOut led1(PA_0); //黄色のコネクタ |
hitonari | 0:2b57931c6ed4 | 188 | DigitalOut led2(PA_1); |
hitonari | 0:2b57931c6ed4 | 189 | DigitalOut led3(PB_5); |
hitonari | 0:2b57931c6ed4 | 190 | DigitalOut led4(PB_4); |
hitonari | 0:2b57931c6ed4 | 191 | |
hitonari | 0:2b57931c6ed4 | 192 | //InterruptIn switch2(PC_14); |
hitonari | 0:2b57931c6ed4 | 193 | MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン,シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine |
hitonari | 0:2b57931c6ed4 | 194 | HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8 |
hitonari | 0:2b57931c6ed4 | 195 | |
hitonari | 0:2b57931c6ed4 | 196 | PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL); |
hitonari | 0:2b57931c6ed4 | 197 | PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); |
hitonari | 0:2b57931c6ed4 | 198 | PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD); |
hitonari | 0:2b57931c6ed4 | 199 | |
hitonari | 0:2b57931c6ed4 | 200 | enum Channel {AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8}; |
hitonari | 0:2b57931c6ed4 | 201 | enum Angle {ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 |
hitonari | 0:2b57931c6ed4 | 202 | enum OperationMode {StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide}; |
hitonari | 0:2b57931c6ed4 | 203 | enum BombingMode {Takeoff, Chicken, Transition, Approach}; |
hitonari | 0:2b57931c6ed4 | 204 | enum OutputStatus {Manual, Auto}; |
hitonari | 0:2b57931c6ed4 | 205 | |
hitonari | 0:2b57931c6ed4 | 206 | static OutputStatus output_status = Manual; |
hitonari | 0:2b57931c6ed4 | 207 | OperationMode operation_mode = StartUp; |
hitonari | 0:2b57931c6ed4 | 208 | BombingMode bombing_mode = Takeoff; |
hitonari | 0:2b57931c6ed4 | 209 | |
hitonari | 0:2b57931c6ed4 | 210 | static int16_t autopwm[8] = {1514,1435,1180,1512,1176,1800};//6:1848 |
hitonari | 0:2b57931c6ed4 | 211 | |
hitonari | 0:2b57931c6ed4 | 212 | //1号機14SG |
hitonari | 0:2b57931c6ed4 | 213 | |
hitonari | 0:2b57931c6ed4 | 214 | static int16_t trimpwm[6] = {1428,1646,1176,1476,1344,1478};//1500 |
hitonari | 0:2b57931c6ed4 | 215 | int16_t maxpwm[6] = {1786,1848,1848,1889,1520,1848}; |
hitonari | 0:2b57931c6ed4 | 216 | int16_t minpwm[6] = {1114,1384,500,1293,1280,1176};///3-1376 |
hitonari | 0:2b57931c6ed4 | 217 | const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; |
hitonari | 0:2b57931c6ed4 | 218 | |
hitonari | 0:2b57931c6ed4 | 219 | |
hitonari | 0:2b57931c6ed4 | 220 | //2号機 |
hitonari | 0:2b57931c6ed4 | 221 | /* |
hitonari | 0:2b57931c6ed4 | 222 | static int16_t trimpwm[6] = {1508,1559,1184,1664,1072,1952}; |
hitonari | 0:2b57931c6ed4 | 223 | int16_t maxpwm[6] = {1840,1884,1840,1992,1728,1952}; |
hitonari | 0:2b57931c6ed4 | 224 | int16_t minpwm[6] = {1183,1228,1176,1420,1420,1072}; |
hitonari | 0:2b57931c6ed4 | 225 | const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; |
hitonari | 0:2b57931c6ed4 | 226 | */ |
hitonari | 0:2b57931c6ed4 | 227 | |
hitonari | 0:2b57931c6ed4 | 228 | int16_t oldTHR = 1000; |
hitonari | 0:2b57931c6ed4 | 229 | |
hitonari | 0:2b57931c6ed4 | 230 | int16_t g_AIL_L_Ratio_rightloop = 0.5; |
hitonari | 0:2b57931c6ed4 | 231 | |
hitonari | 0:2b57931c6ed4 | 232 | |
hitonari | 0:2b57931c6ed4 | 233 | static float nowAngle[3] = {0,0,0}; |
hitonari | 0:2b57931c6ed4 | 234 | const float trimAngle[3] = {0.0, 0.0, 0.0}; |
hitonari | 0:2b57931c6ed4 | 235 | const float maxAngle[2] = {90, 90}; |
hitonari | 0:2b57931c6ed4 | 236 | const float minAngle[2] = {-90, -90}; |
hitonari | 0:2b57931c6ed4 | 237 | |
hitonari | 0:2b57931c6ed4 | 238 | float FirstROLL = 0.0, FirstPITCH = 0.0,FirstYAW = 0.0; |
hitonari | 0:2b57931c6ed4 | 239 | |
hitonari | 0:2b57931c6ed4 | 240 | unsigned int g_distance; |
hitonari | 0:2b57931c6ed4 | 241 | Ticker USsensor; |
hitonari | 0:2b57931c6ed4 | 242 | static char g_buf[16]; |
hitonari | 0:2b57931c6ed4 | 243 | char g_landingcommand='T'; |
hitonari | 0:2b57931c6ed4 | 244 | float g_SerialTargetYAW; |
hitonari | 0:2b57931c6ed4 | 245 | |
hitonari | 0:2b57931c6ed4 | 246 | |
hitonari | 0:2b57931c6ed4 | 247 | Timer t; |
hitonari | 0:2b57931c6ed4 | 248 | Timer t2; |
hitonari | 0:2b57931c6ed4 | 249 | Timeout RerurnChickenServo1; |
hitonari | 0:2b57931c6ed4 | 250 | Timeout RerurnChickenServo2; |
hitonari | 0:2b57931c6ed4 | 251 | |
hitonari | 0:2b57931c6ed4 | 252 | /*-----関数のプロトタイプ宣言-----*/ |
hitonari | 0:2b57931c6ed4 | 253 | void setup(); |
hitonari | 0:2b57931c6ed4 | 254 | void loop(); |
hitonari | 0:2b57931c6ed4 | 255 | |
hitonari | 0:2b57931c6ed4 | 256 | void Init_PWM(); |
hitonari | 0:2b57931c6ed4 | 257 | void Init_servo(); //サーボ初期化 |
hitonari | 0:2b57931c6ed4 | 258 | void Init_sbus(); //SBUS初期化 |
hitonari | 0:2b57931c6ed4 | 259 | void Init_sensors(); |
hitonari | 0:2b57931c6ed4 | 260 | void DisplayClock(); //クロック状態確認 |
hitonari | 0:2b57931c6ed4 | 261 | void SetupBMP280(); |
hitonari | 0:2b57931c6ed4 | 262 | void Set_trim(); |
hitonari | 0:2b57931c6ed4 | 263 | |
hitonari | 0:2b57931c6ed4 | 264 | //センサの値取得 |
hitonari | 0:2b57931c6ed4 | 265 | void SensingMPU(); |
hitonari | 0:2b57931c6ed4 | 266 | void UpdateDist(); |
hitonari | 0:2b57931c6ed4 | 267 | void sensing(); |
hitonari | 0:2b57931c6ed4 | 268 | void getBMP280(); |
hitonari | 0:2b57931c6ed4 | 269 | |
hitonari | 0:2b57931c6ed4 | 270 | //void offsetRollPitch(float FirstROLL, float FirstPITCH); |
hitonari | 0:2b57931c6ed4 | 271 | //void TransYaw(float FirstYAW); |
hitonari | 0:2b57931c6ed4 | 272 | float TranslateNewYaw(float beforeYaw, float newzeroYaw); |
hitonari | 0:2b57931c6ed4 | 273 | void UpdateTargetAngle(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 274 | void CalculateControlValue(float targetAngle[3], float controlValue[3]); |
hitonari | 0:2b57931c6ed4 | 275 | void UpdateAutoPWM(float controlValue[3]); |
hitonari | 0:2b57931c6ed4 | 276 | void ConvertPWMintoRAD(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 277 | inline float CalcRatio(float value, float trim, float limit); |
hitonari | 0:2b57931c6ed4 | 278 | bool CheckSW_Up(Channel ch); |
hitonari | 0:2b57931c6ed4 | 279 | int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min); |
hitonari | 0:2b57931c6ed4 | 280 | inline int16_t SetTHRinRatio(float ratio); |
hitonari | 0:2b57931c6ed4 | 281 | void Ascending(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 282 | |
hitonari | 0:2b57931c6ed4 | 283 | //sbus割り込み |
hitonari | 0:2b57931c6ed4 | 284 | void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 |
hitonari | 0:2b57931c6ed4 | 285 | void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力 |
hitonari | 0:2b57931c6ed4 | 286 | |
hitonari | 0:2b57931c6ed4 | 287 | //シリアル割り込み |
hitonari | 0:2b57931c6ed4 | 288 | void getSF_Serial(); |
hitonari | 0:2b57931c6ed4 | 289 | float ConvertByteintoFloat(char high, char low); |
hitonari | 0:2b57931c6ed4 | 290 | |
hitonari | 0:2b57931c6ed4 | 291 | bool g_Finflag = false; |
hitonari | 0:2b57931c6ed4 | 292 | |
hitonari | 0:2b57931c6ed4 | 293 | |
hitonari | 0:2b57931c6ed4 | 294 | //SD設定 |
hitonari | 0:2b57931c6ed4 | 295 | int GetParameter(FILE *fp, const char *paramName,char parameter[]); |
hitonari | 0:2b57931c6ed4 | 296 | int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, |
hitonari | 0:2b57931c6ed4 | 297 | float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, |
hitonari | 0:2b57931c6ed4 | 298 | float *g_rightloopROLL, float *g_rightloopPITCH, |
hitonari | 0:2b57931c6ed4 | 299 | float *g_leftloopROLL, float *g_leftloopPITCH, |
hitonari | 0:2b57931c6ed4 | 300 | float *g_gostraightROLL, float *g_gostraightPITCH, |
hitonari | 0:2b57931c6ed4 | 301 | float *g_takeoffTHR, float *g_loopTHR, |
hitonari | 0:2b57931c6ed4 | 302 | float *g_rightloopROLLshort, float *g_rightloopPITCHshort, |
hitonari | 0:2b57931c6ed4 | 303 | float *g_leftloopROLLshort, float *g_leftloopPITCHshort, |
hitonari | 0:2b57931c6ed4 | 304 | float *g_glideloopROLL, float *g_glideloopPITCH, |
hitonari | 0:2b57931c6ed4 | 305 | float *g_kpAIL, float *g_kiAIL, float *g_kdAIL, |
hitonari | 0:2b57931c6ed4 | 306 | int *g_rightloopRUD, int *g_rightloopshortRUD, |
hitonari | 0:2b57931c6ed4 | 307 | int *g_leftloopRUD, int *g_leftloopshortRUD, |
hitonari | 0:2b57931c6ed4 | 308 | int *g_glideRUD, |
hitonari | 0:2b57931c6ed4 | 309 | float *g_rightloopROLL_approach, float *g_leftloopROLL_approach, |
hitonari | 0:2b57931c6ed4 | 310 | int *g_rightloopRUD_approach, int *g_leftloopRUD_approach, |
hitonari | 0:2b57931c6ed4 | 311 | float *g_rightloopPITCH_approach, float *g_leftloopPITCH_approach, |
hitonari | 0:2b57931c6ed4 | 312 | float *g_ascendingPITCH, |
hitonari | 0:2b57931c6ed4 | 313 | int *g_take_off_THROTTLE, float *g_take_off_PITCH, |
hitonari | 0:2b57931c6ed4 | 314 | float *g_rightloop_THR_rate, float *g_leftloop_THR_rate, |
hitonari | 0:2b57931c6ed4 | 315 | float *g_ascendding_thr_rate, |
hitonari | 0:2b57931c6ed4 | 316 | float *g_ascending_ROLL, int *g_ascending_RUD, |
hitonari | 0:2b57931c6ed4 | 317 | int *g_trim1, int *g_trim2, int *g_trim3, |
hitonari | 0:2b57931c6ed4 | 318 | int *g_trim4, int *g_trim5, int *g_trim6, |
hitonari | 0:2b57931c6ed4 | 319 | bool *g_trim_check |
hitonari | 0:2b57931c6ed4 | 320 | ); |
hitonari | 0:2b57931c6ed4 | 321 | //switch2割り込み |
hitonari | 0:2b57931c6ed4 | 322 | void ResetTrim(); |
hitonari | 0:2b57931c6ed4 | 323 | |
hitonari | 0:2b57931c6ed4 | 324 | //自動操縦 |
hitonari | 0:2b57931c6ed4 | 325 | void UpdateTargetAngle_GoStraight(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 326 | void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]); //着陸時にスロットルが0の時の直進 |
hitonari | 0:2b57931c6ed4 | 327 | void UpdateTargetAngle_Rightloop(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 328 | void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 329 | void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが0の時の右旋回 |
hitonari | 0:2b57931c6ed4 | 330 | void UpdateTargetAngle_Rightloop_UP(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 331 | void UpdateTargetAngle_Leftloop(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 332 | void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 333 | void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]); //着陸時にスロットルが0の時の左旋回 |
hitonari | 0:2b57931c6ed4 | 334 | void UpdateTargetAngle_Moebius(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 335 | void UpdateTargetAngle_Takeoff(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 336 | void UpdateTargetAngle_Approach(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 337 | void Take_off_and_landing(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 338 | |
hitonari | 0:2b57931c6ed4 | 339 | |
hitonari | 0:2b57931c6ed4 | 340 | |
hitonari | 0:2b57931c6ed4 | 341 | int Rotate(float targetAngle[3], float TargetYAW); |
hitonari | 0:2b57931c6ed4 | 342 | int Rotate_zero(float targetAngle[3], float TargetYAW); |
hitonari | 0:2b57931c6ed4 | 343 | |
hitonari | 0:2b57931c6ed4 | 344 | //投下 |
hitonari | 0:2b57931c6ed4 | 345 | void Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 346 | void ReturnChickenServo1(); |
hitonari | 0:2b57931c6ed4 | 347 | void ReturnChickenServo2(); |
hitonari | 0:2b57931c6ed4 | 348 | |
hitonari | 0:2b57931c6ed4 | 349 | //超音波による高度補正 |
hitonari | 0:2b57931c6ed4 | 350 | void checkHeight(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 351 | void UpdateTargetAngle_NoseUP(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 352 | void UpdateTargetAngle_NoseDOWN(float targetAngle[3]); |
hitonari | 0:2b57931c6ed4 | 353 | |
hitonari | 0:2b57931c6ed4 | 354 | //デバッグ用 |
hitonari | 0:2b57931c6ed4 | 355 | void Sbusprintf(); |
hitonari | 0:2b57931c6ed4 | 356 | void DebugPrint(); |
hitonari | 0:2b57931c6ed4 | 357 | |
hitonari | 0:2b57931c6ed4 | 358 | /*---関数のプロトタイプ宣言終わり---*/ |
hitonari | 0:2b57931c6ed4 | 359 | |
hitonari | 0:2b57931c6ed4 | 360 | int main() |
hitonari | 0:2b57931c6ed4 | 361 | { |
hitonari | 0:2b57931c6ed4 | 362 | setup(); |
hitonari | 0:2b57931c6ed4 | 363 | |
hitonari | 0:2b57931c6ed4 | 364 | |
hitonari | 0:2b57931c6ed4 | 365 | while(1) { |
hitonari | 0:2b57931c6ed4 | 366 | |
hitonari | 0:2b57931c6ed4 | 367 | loop(); |
hitonari | 0:2b57931c6ed4 | 368 | |
hitonari | 0:2b57931c6ed4 | 369 | |
hitonari | 0:2b57931c6ed4 | 370 | NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 371 | if(!CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 372 | led3=0; |
hitonari | 0:2b57931c6ed4 | 373 | |
hitonari | 0:2b57931c6ed4 | 374 | } else { |
hitonari | 0:2b57931c6ed4 | 375 | led3=1; |
hitonari | 0:2b57931c6ed4 | 376 | } |
hitonari | 0:2b57931c6ed4 | 377 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 378 | } |
hitonari | 0:2b57931c6ed4 | 379 | |
hitonari | 0:2b57931c6ed4 | 380 | } |
hitonari | 0:2b57931c6ed4 | 381 | |
hitonari | 0:2b57931c6ed4 | 382 | void setup() |
hitonari | 0:2b57931c6ed4 | 383 | { |
hitonari | 0:2b57931c6ed4 | 384 | //buzzer = 0; |
hitonari | 0:2b57931c6ed4 | 385 | led1 = 1; |
hitonari | 0:2b57931c6ed4 | 386 | led2 = 1; |
hitonari | 0:2b57931c6ed4 | 387 | led3 = 1; |
hitonari | 0:2b57931c6ed4 | 388 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 389 | |
hitonari | 0:2b57931c6ed4 | 390 | SetOptions(&g_kpELE, &g_kiELE, &g_kdELE, |
hitonari | 0:2b57931c6ed4 | 391 | &g_kpRUD, &g_kiRUD, &g_kdRUD, |
hitonari | 0:2b57931c6ed4 | 392 | &g_rightloopROLL, &g_rightloopPITCH, |
hitonari | 0:2b57931c6ed4 | 393 | &g_leftloopROLL, &g_leftloopPITCH, |
hitonari | 0:2b57931c6ed4 | 394 | &g_gostraightROLL, &g_gostraightPITCH, |
hitonari | 0:2b57931c6ed4 | 395 | &g_takeoffTHR, &g_loopTHR, |
hitonari | 0:2b57931c6ed4 | 396 | &g_rightloopROLLshort, &g_rightloopPITCHshort, |
hitonari | 0:2b57931c6ed4 | 397 | &g_leftloopROLLshort, &g_leftloopPITCHshort, |
hitonari | 0:2b57931c6ed4 | 398 | &g_glideloopROLL, &g_glideloopPITCH, |
hitonari | 0:2b57931c6ed4 | 399 | &g_kpAIL, &g_kiAIL,&g_kdAIL, |
hitonari | 0:2b57931c6ed4 | 400 | &g_rightloopRUD, &g_rightloopshortRUD, |
hitonari | 0:2b57931c6ed4 | 401 | &g_leftloopRUD, &g_leftloopshortRUD, |
hitonari | 0:2b57931c6ed4 | 402 | &g_glideloopRUD, |
hitonari | 0:2b57931c6ed4 | 403 | &g_rightloopROLL_approach,&g_leftloopROLL_approach, |
hitonari | 0:2b57931c6ed4 | 404 | &g_rightloopRUD_approach,&g_leftloopRUD_approach, |
hitonari | 0:2b57931c6ed4 | 405 | &g_rightloopPITCH_approach,&g_leftloopPITCH_approach, |
hitonari | 0:2b57931c6ed4 | 406 | &g_ascendingPITCH, |
hitonari | 0:2b57931c6ed4 | 407 | &g_take_off_THROTTLE, &g_take_off_PITCH, |
hitonari | 0:2b57931c6ed4 | 408 | &g_rightloop_THR_rate, &g_leftloop_THR_rate, |
hitonari | 0:2b57931c6ed4 | 409 | &g_ascending_THR_rate, |
hitonari | 0:2b57931c6ed4 | 410 | &g_ascending_ROLL, &g_ascending_RUD, |
hitonari | 0:2b57931c6ed4 | 411 | &g_trim1, &g_trim2, &g_trim3, |
hitonari | 0:2b57931c6ed4 | 412 | &g_trim4, &g_trim5, &g_trim6, |
hitonari | 0:2b57931c6ed4 | 413 | &g_trim_check |
hitonari | 0:2b57931c6ed4 | 414 | ); |
hitonari | 0:2b57931c6ed4 | 415 | |
hitonari | 0:2b57931c6ed4 | 416 | Set_trim(); |
hitonari | 0:2b57931c6ed4 | 417 | |
hitonari | 0:2b57931c6ed4 | 418 | |
hitonari | 0:2b57931c6ed4 | 419 | |
hitonari | 0:2b57931c6ed4 | 420 | Init_PWM(); |
hitonari | 0:2b57931c6ed4 | 421 | Init_servo(); |
hitonari | 0:2b57931c6ed4 | 422 | Init_sbus(); |
hitonari | 0:2b57931c6ed4 | 423 | Init_sensors(); |
hitonari | 0:2b57931c6ed4 | 424 | //switch2.rise(ResetTrim); |
hitonari | 0:2b57931c6ed4 | 425 | |
hitonari | 0:2b57931c6ed4 | 426 | USsensor.attach(&UpdateDist, 0.05); |
hitonari | 0:2b57931c6ed4 | 427 | |
hitonari | 0:2b57931c6ed4 | 428 | NVIC_SetPriority(USART1_IRQn,0);//プロポ |
hitonari | 0:2b57931c6ed4 | 429 | NVIC_SetPriority(EXTI0_IRQn,1);//MPU割り込み禁止 |
hitonari | 0:2b57931c6ed4 | 430 | NVIC_SetPriority(TIM5_IRQn,2); |
hitonari | 0:2b57931c6ed4 | 431 | NVIC_SetPriority(EXTI9_5_IRQn,3); |
hitonari | 0:2b57931c6ed4 | 432 | NVIC_SetPriority(EXTI4_IRQn,5); |
hitonari | 0:2b57931c6ed4 | 433 | DisplayClock(); |
hitonari | 0:2b57931c6ed4 | 434 | t.start(); |
hitonari | 0:2b57931c6ed4 | 435 | t.start(); |
hitonari | 0:2b57931c6ed4 | 436 | |
hitonari | 0:2b57931c6ed4 | 437 | |
hitonari | 0:2b57931c6ed4 | 438 | pc.printf("MPU calibration start\r\n"); |
hitonari | 0:2b57931c6ed4 | 439 | |
hitonari | 0:2b57931c6ed4 | 440 | float offsetstart = t.read(); |
hitonari | 0:2b57931c6ed4 | 441 | while(t.read() - offsetstart < 26) { |
hitonari | 0:2b57931c6ed4 | 442 | SensingMPU(); |
hitonari | 0:2b57931c6ed4 | 443 | for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); |
hitonari | 0:2b57931c6ed4 | 444 | pc.printf("\r\n"); |
hitonari | 0:2b57931c6ed4 | 445 | led1 = !led1; |
hitonari | 0:2b57931c6ed4 | 446 | led2 = !led2; |
hitonari | 0:2b57931c6ed4 | 447 | led3 = !led3; |
hitonari | 0:2b57931c6ed4 | 448 | led4 = !led4; |
hitonari | 0:2b57931c6ed4 | 449 | } |
hitonari | 0:2b57931c6ed4 | 450 | //pc.attach(getSF_Serial, Serial::RxIrq); |
hitonari | 0:2b57931c6ed4 | 451 | NVIC_SetPriority(USART2_IRQn,4);//シリアル |
hitonari | 0:2b57931c6ed4 | 452 | |
hitonari | 0:2b57931c6ed4 | 453 | FirstROLL = nowAngle[ROLL]; |
hitonari | 0:2b57931c6ed4 | 454 | FirstPITCH = nowAngle[PITCH]; |
hitonari | 0:2b57931c6ed4 | 455 | nowAngle[ROLL] -=FirstROLL; |
hitonari | 0:2b57931c6ed4 | 456 | nowAngle[PITCH] -=FirstPITCH; |
hitonari | 0:2b57931c6ed4 | 457 | |
hitonari | 0:2b57931c6ed4 | 458 | SetupBMP280(); |
hitonari | 0:2b57931c6ed4 | 459 | |
hitonari | 0:2b57931c6ed4 | 460 | // NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 461 | // |
hitonari | 0:2b57931c6ed4 | 462 | // if(pc.readable()) { // 受信確認 |
hitonari | 0:2b57931c6ed4 | 463 | // |
hitonari | 0:2b57931c6ed4 | 464 | // static char SFbuf[16]= {'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; |
hitonari | 0:2b57931c6ed4 | 465 | // SFbuf[0] = pc.getc(); // 1文字取り出し |
hitonari | 0:2b57931c6ed4 | 466 | // if(SFbuf[0]=='S') { |
hitonari | 0:2b57931c6ed4 | 467 | // for(int i=0;i<3;i++){ |
hitonari | 0:2b57931c6ed4 | 468 | // led1 = 1; |
hitonari | 0:2b57931c6ed4 | 469 | // led2 = 1; |
hitonari | 0:2b57931c6ed4 | 470 | // led3 = 1; |
hitonari | 0:2b57931c6ed4 | 471 | // led4 = 1; |
hitonari | 0:2b57931c6ed4 | 472 | // wait(0.2); |
hitonari | 0:2b57931c6ed4 | 473 | // } |
hitonari | 0:2b57931c6ed4 | 474 | // } |
hitonari | 0:2b57931c6ed4 | 475 | // } |
hitonari | 0:2b57931c6ed4 | 476 | // NVIC_DisableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 477 | |
hitonari | 0:2b57931c6ed4 | 478 | |
hitonari | 0:2b57931c6ed4 | 479 | led1 = 0; |
hitonari | 0:2b57931c6ed4 | 480 | led2 = 0; |
hitonari | 0:2b57931c6ed4 | 481 | led3 = 0; |
hitonari | 0:2b57931c6ed4 | 482 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 483 | wait(0.2); |
hitonari | 0:2b57931c6ed4 | 484 | |
hitonari | 0:2b57931c6ed4 | 485 | |
hitonari | 0:2b57931c6ed4 | 486 | pc.printf("All initialized\r\n"); |
hitonari | 0:2b57931c6ed4 | 487 | } |
hitonari | 0:2b57931c6ed4 | 488 | |
hitonari | 0:2b57931c6ed4 | 489 | void Set_trim(){ |
hitonari | 0:2b57931c6ed4 | 490 | trimpwm[0] = g_trim1; |
hitonari | 0:2b57931c6ed4 | 491 | trimpwm[1] = g_trim2; |
hitonari | 0:2b57931c6ed4 | 492 | trimpwm[2] = g_trim3; |
hitonari | 0:2b57931c6ed4 | 493 | trimpwm[3] = g_trim4; |
hitonari | 0:2b57931c6ed4 | 494 | trimpwm[4] = g_trim5; |
hitonari | 0:2b57931c6ed4 | 495 | trimpwm[5] = g_trim6; |
hitonari | 0:2b57931c6ed4 | 496 | |
hitonari | 0:2b57931c6ed4 | 497 | maxpwm[0] = 1768;//trimpwm[0] + 330; |
hitonari | 0:2b57931c6ed4 | 498 | maxpwm[1] = 1964;//trimpwm[1] + 330; |
hitonari | 0:2b57931c6ed4 | 499 | maxpwm[2] = 1848;//trimpwm[2] + 660; |
hitonari | 0:2b57931c6ed4 | 500 | maxpwm[3] = 1825;//trimpwm[3] + 330; |
hitonari | 0:2b57931c6ed4 | 501 | maxpwm[4] = 1914;//trimpwm[4] + 660; |
hitonari | 0:2b57931c6ed4 | 502 | maxpwm[5] = 1816;//trimpwm[5] + 330; |
hitonari | 0:2b57931c6ed4 | 503 | |
hitonari | 0:2b57931c6ed4 | 504 | minpwm[0]= 1186;//trimpwm[0] - 330; |
hitonari | 0:2b57931c6ed4 | 505 | minpwm[1]= 1314;//trimpwm[1] - 330; |
hitonari | 0:2b57931c6ed4 | 506 | minpwm[2]= trimpwm[2] - 0; |
hitonari | 0:2b57931c6ed4 | 507 | minpwm[3]= 1154;//trimpwm[3] - 330; |
hitonari | 0:2b57931c6ed4 | 508 | minpwm[4]= 1344;//trimpwm[4] - 0; |
hitonari | 0:2b57931c6ed4 | 509 | minpwm[5]= 1144;//trimpwm[5] - 330; |
hitonari | 0:2b57931c6ed4 | 510 | } |
hitonari | 0:2b57931c6ed4 | 511 | |
hitonari | 0:2b57931c6ed4 | 512 | void loop() |
hitonari | 0:2b57931c6ed4 | 513 | { |
hitonari | 0:2b57931c6ed4 | 514 | static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; |
hitonari | 0:2b57931c6ed4 | 515 | SensingMPU(); |
hitonari | 0:2b57931c6ed4 | 516 | UpdateTargetAngle(targetAngle); |
hitonari | 0:2b57931c6ed4 | 517 | CalculateControlValue(targetAngle, controlValue); |
hitonari | 0:2b57931c6ed4 | 518 | UpdateAutoPWM(controlValue); |
hitonari | 0:2b57931c6ed4 | 519 | |
hitonari | 0:2b57931c6ed4 | 520 | |
hitonari | 0:2b57931c6ed4 | 521 | //NVIC_SetPriority(TIM5_IRQn,4); |
hitonari | 0:2b57931c6ed4 | 522 | //NVIC_SetPriority(USART2_IRQn,2); |
hitonari | 0:2b57931c6ed4 | 523 | |
hitonari | 0:2b57931c6ed4 | 524 | //wait_ms(23); |
hitonari | 0:2b57931c6ed4 | 525 | wait_ms(30); |
hitonari | 0:2b57931c6ed4 | 526 | |
hitonari | 0:2b57931c6ed4 | 527 | //NVIC_SetPriority(TIM5_IRQn,2); |
hitonari | 0:2b57931c6ed4 | 528 | //NVIC_SetPriority(USART2_IRQn,4); |
hitonari | 0:2b57931c6ed4 | 529 | |
hitonari | 0:2b57931c6ed4 | 530 | |
hitonari | 0:2b57931c6ed4 | 531 | // pc.printf("6\r\n"); |
hitonari | 0:2b57931c6ed4 | 532 | //NVIC_DisableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 533 | //pc.printf("%c",g_landingcommand); |
hitonari | 0:2b57931c6ed4 | 534 | //NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 535 | #if DEBUG_PRINT_INLOOP |
hitonari | 0:2b57931c6ed4 | 536 | //Sbusprintf(); |
hitonari | 0:2b57931c6ed4 | 537 | DebugPrint(); |
hitonari | 0:2b57931c6ed4 | 538 | #endif |
hitonari | 0:2b57931c6ed4 | 539 | } |
hitonari | 0:2b57931c6ed4 | 540 | |
hitonari | 0:2b57931c6ed4 | 541 | //サーボ初期化関数 |
hitonari | 0:2b57931c6ed4 | 542 | void Init_servo() |
hitonari | 0:2b57931c6ed4 | 543 | { |
hitonari | 0:2b57931c6ed4 | 544 | servo1.period_ms(16); |
hitonari | 0:2b57931c6ed4 | 545 | servo1.pulsewidth_us(trimpwm[AIL_R]); |
hitonari | 0:2b57931c6ed4 | 546 | |
hitonari | 0:2b57931c6ed4 | 547 | servo2.period_ms(16); |
hitonari | 0:2b57931c6ed4 | 548 | servo2.pulsewidth_us(trimpwm[ELE]); |
hitonari | 0:2b57931c6ed4 | 549 | |
hitonari | 0:2b57931c6ed4 | 550 | servo3.period_ms(16); |
hitonari | 0:2b57931c6ed4 | 551 | servo3.pulsewidth_us(trimpwm[THR]); |
hitonari | 0:2b57931c6ed4 | 552 | |
hitonari | 0:2b57931c6ed4 | 553 | servo4.period_ms(16); |
hitonari | 0:2b57931c6ed4 | 554 | servo4.pulsewidth_us(trimpwm[RUD]); |
hitonari | 0:2b57931c6ed4 | 555 | |
hitonari | 0:2b57931c6ed4 | 556 | servo5.period_ms(16); |
hitonari | 0:2b57931c6ed4 | 557 | servo5.pulsewidth_us(trimpwm[DROP]); |
hitonari | 0:2b57931c6ed4 | 558 | |
hitonari | 0:2b57931c6ed4 | 559 | servo6.period_ms(16); |
hitonari | 0:2b57931c6ed4 | 560 | servo6.pulsewidth_us(trimpwm[AIL_L]); |
hitonari | 0:2b57931c6ed4 | 561 | |
hitonari | 0:2b57931c6ed4 | 562 | int i = 0; |
hitonari | 0:2b57931c6ed4 | 563 | |
hitonari | 0:2b57931c6ed4 | 564 | for(i = 0;i<6;i++ ){ |
hitonari | 0:2b57931c6ed4 | 565 | autopwm[i]=trimpwm[i]; |
hitonari | 0:2b57931c6ed4 | 566 | } |
hitonari | 0:2b57931c6ed4 | 567 | |
hitonari | 0:2b57931c6ed4 | 568 | pc.printf("servo initialized\r\n"); |
hitonari | 0:2b57931c6ed4 | 569 | } |
hitonari | 0:2b57931c6ed4 | 570 | |
hitonari | 0:2b57931c6ed4 | 571 | //Sbus初期化 |
hitonari | 0:2b57931c6ed4 | 572 | void Init_sbus() |
hitonari | 0:2b57931c6ed4 | 573 | { |
hitonari | 0:2b57931c6ed4 | 574 | sbus.initialize(); |
hitonari | 0:2b57931c6ed4 | 575 | sbus.setLastfuncPoint(Update_PWM); |
hitonari | 0:2b57931c6ed4 | 576 | sbus.startInterrupt(); |
hitonari | 0:2b57931c6ed4 | 577 | } |
hitonari | 0:2b57931c6ed4 | 578 | |
hitonari | 0:2b57931c6ed4 | 579 | void Init_sensors() |
hitonari | 0:2b57931c6ed4 | 580 | { |
hitonari | 0:2b57931c6ed4 | 581 | if(mpu6050.setup() == -1) { |
hitonari | 0:2b57931c6ed4 | 582 | pc.printf("failed initialize\r\n"); |
hitonari | 0:2b57931c6ed4 | 583 | while(1) { |
hitonari | 0:2b57931c6ed4 | 584 | led1 = 1; |
hitonari | 0:2b57931c6ed4 | 585 | led2 = 0; |
hitonari | 0:2b57931c6ed4 | 586 | led3 = 1; |
hitonari | 0:2b57931c6ed4 | 587 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 588 | wait(1); |
hitonari | 0:2b57931c6ed4 | 589 | led1 = 0; |
hitonari | 0:2b57931c6ed4 | 590 | led2 = 1; |
hitonari | 0:2b57931c6ed4 | 591 | led3 = 0; |
hitonari | 0:2b57931c6ed4 | 592 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 593 | wait(1); |
hitonari | 0:2b57931c6ed4 | 594 | } |
hitonari | 0:2b57931c6ed4 | 595 | } |
hitonari | 0:2b57931c6ed4 | 596 | } |
hitonari | 0:2b57931c6ed4 | 597 | |
hitonari | 0:2b57931c6ed4 | 598 | void Init_PWM() |
hitonari | 0:2b57931c6ed4 | 599 | { |
hitonari | 0:2b57931c6ed4 | 600 | pc.printf("PWM initialized\r\n"); |
hitonari | 0:2b57931c6ed4 | 601 | } |
hitonari | 0:2b57931c6ed4 | 602 | |
hitonari | 0:2b57931c6ed4 | 603 | void DisplayClock() |
hitonari | 0:2b57931c6ed4 | 604 | { |
hitonari | 0:2b57931c6ed4 | 605 | pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); |
hitonari | 0:2b57931c6ed4 | 606 | pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); |
hitonari | 0:2b57931c6ed4 | 607 | pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); |
hitonari | 0:2b57931c6ed4 | 608 | pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); |
hitonari | 0:2b57931c6ed4 | 609 | pc.printf("\r\n"); |
hitonari | 0:2b57931c6ed4 | 610 | } |
hitonari | 0:2b57931c6ed4 | 611 | |
hitonari | 0:2b57931c6ed4 | 612 | void UpdateTargetAngle(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 613 | { |
hitonari | 0:2b57931c6ed4 | 614 | |
hitonari | 0:2b57931c6ed4 | 615 | |
hitonari | 0:2b57931c6ed4 | 616 | static int16_t count_op = 0; |
hitonari | 0:2b57931c6ed4 | 617 | #if DEBUG_SEMIAUTO |
hitonari | 0:2b57931c6ed4 | 618 | switch(operation_mode) { |
hitonari | 0:2b57931c6ed4 | 619 | case StartUp: |
hitonari | 0:2b57931c6ed4 | 620 | if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 621 | count_op++;B |
hitonari | 0:2b57931c6ed4 | 622 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 623 | operation_mode = SemiAuto; |
hitonari | 0:2b57931c6ed4 | 624 | pc.printf("Goto SemiAuto mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 625 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 626 | } |
hitonari | 0:2b57931c6ed4 | 627 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 628 | break; |
hitonari | 0:2b57931c6ed4 | 629 | |
hitonari | 0:2b57931c6ed4 | 630 | case SemiAuto: |
hitonari | 0:2b57931c6ed4 | 631 | /* 大会用では以下のif文を入れてoperation_modeを変える |
hitonari | 0:2b57931c6ed4 | 632 | if(CheckSW_Up(Ch6)){ |
hitonari | 0:2b57931c6ed4 | 633 | count_op++; |
hitonari | 0:2b57931c6ed4 | 634 | if(count_op>changeModeCount){ |
hitonari | 0:2b57931c6ed4 | 635 | output_status = XXX; |
hitonari | 0:2b57931c6ed4 | 636 | led2 = 0; |
hitonari | 0:2b57931c6ed4 | 637 | pc.printf("Goto XXX mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 638 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 639 | }else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 640 | ConvertPWMintoRAD(targetAngle); |
hitonari | 0:2b57931c6ed4 | 641 | } |
hitonari | 0:2b57931c6ed4 | 642 | */ |
hitonari | 0:2b57931c6ed4 | 643 | ConvertPWMintoRAD(targetAngle); |
hitonari | 0:2b57931c6ed4 | 644 | break; |
hitonari | 0:2b57931c6ed4 | 645 | |
hitonari | 0:2b57931c6ed4 | 646 | default: |
hitonari | 0:2b57931c6ed4 | 647 | operation_mode = SemiAuto; |
hitonari | 0:2b57931c6ed4 | 648 | break; |
hitonari | 0:2b57931c6ed4 | 649 | } |
hitonari | 0:2b57931c6ed4 | 650 | |
hitonari | 0:2b57931c6ed4 | 651 | #else |
hitonari | 0:2b57931c6ed4 | 652 | |
hitonari | 0:2b57931c6ed4 | 653 | switch(operation_mode) { |
hitonari | 0:2b57931c6ed4 | 654 | case StartUp: |
hitonari | 0:2b57931c6ed4 | 655 | if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) { //ch7;自動・手動切り替え ch8;自動操縦モード切替 |
hitonari | 0:2b57931c6ed4 | 656 | count_op++; |
hitonari | 0:2b57931c6ed4 | 657 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 658 | operation_mode = LeftLoop; |
hitonari | 0:2b57931c6ed4 | 659 | pc.printf("Goto LeftLoop mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 660 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 661 | } |
hitonari | 0:2b57931c6ed4 | 662 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 663 | break; |
hitonari | 0:2b57931c6ed4 | 664 | |
hitonari | 0:2b57931c6ed4 | 665 | case LeftLoop: |
hitonari | 0:2b57931c6ed4 | 666 | if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 667 | count_op++; |
hitonari | 0:2b57931c6ed4 | 668 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 669 | operation_mode = GoStraight; |
hitonari | 0:2b57931c6ed4 | 670 | pc.printf("Goto GoStraight mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 671 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 672 | } |
hitonari | 0:2b57931c6ed4 | 673 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 674 | UpdateTargetAngle_Leftloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 675 | //UpdateTargetAngle_GoStraight_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 676 | break; |
hitonari | 0:2b57931c6ed4 | 677 | |
hitonari | 0:2b57931c6ed4 | 678 | case GoStraight: |
hitonari | 0:2b57931c6ed4 | 679 | if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 680 | count_op++; |
hitonari | 0:2b57931c6ed4 | 681 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 682 | operation_mode = Moebius; |
hitonari | 0:2b57931c6ed4 | 683 | pc.printf("Goto Moebius mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 684 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 685 | } |
hitonari | 0:2b57931c6ed4 | 686 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 687 | //UpdateTargetAngle_GoStraight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 688 | UpdateTargetAngle_GoStraight_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 689 | //UpdateTargetAngle_Leftloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 690 | //UpdateTargetAngle_Leftloop_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 691 | |
hitonari | 0:2b57931c6ed4 | 692 | |
hitonari | 0:2b57931c6ed4 | 693 | break; |
hitonari | 0:2b57931c6ed4 | 694 | |
hitonari | 0:2b57931c6ed4 | 695 | case Moebius: |
hitonari | 0:2b57931c6ed4 | 696 | if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 697 | count_op++; |
hitonari | 0:2b57931c6ed4 | 698 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 699 | operation_mode = Glide; |
hitonari | 0:2b57931c6ed4 | 700 | pc.printf("Goto Glide mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 701 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 702 | } |
hitonari | 0:2b57931c6ed4 | 703 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 704 | UpdateTargetAngle_Moebius(targetAngle); |
hitonari | 0:2b57931c6ed4 | 705 | break; |
hitonari | 0:2b57931c6ed4 | 706 | |
hitonari | 0:2b57931c6ed4 | 707 | case Glide: |
hitonari | 0:2b57931c6ed4 | 708 | if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 709 | count_op++; |
hitonari | 0:2b57931c6ed4 | 710 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 711 | operation_mode = RightLoop; |
hitonari | 0:2b57931c6ed4 | 712 | pc.printf("Goto RightLoop mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 713 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 714 | } |
hitonari | 0:2b57931c6ed4 | 715 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 716 | //UpdateTargetAngle_Rightloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 717 | //UpdateTargetAngle_Rightloop_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 718 | UpdateTargetAngle_GoStraight_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 719 | |
hitonari | 0:2b57931c6ed4 | 720 | break; |
hitonari | 0:2b57931c6ed4 | 721 | |
hitonari | 0:2b57931c6ed4 | 722 | case RightLoop: |
hitonari | 0:2b57931c6ed4 | 723 | if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 724 | count_op++; |
hitonari | 0:2b57931c6ed4 | 725 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 726 | operation_mode = BombwithPC; |
hitonari | 0:2b57931c6ed4 | 727 | pc.attach(getSF_Serial, Serial::RxIrq); |
hitonari | 0:2b57931c6ed4 | 728 | autopwm[DROP]=trimpwm[DROP]; |
hitonari | 0:2b57931c6ed4 | 729 | pc.printf("Goto BombwithPC mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 730 | //g_AscendingFlag = false; |
hitonari | 0:2b57931c6ed4 | 731 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 732 | } |
hitonari | 0:2b57931c6ed4 | 733 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 734 | Ascending(targetAngle); |
hitonari | 0:2b57931c6ed4 | 735 | break; |
hitonari | 0:2b57931c6ed4 | 736 | |
hitonari | 0:2b57931c6ed4 | 737 | case BombwithPC: |
hitonari | 0:2b57931c6ed4 | 738 | if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)) { |
hitonari | 0:2b57931c6ed4 | 739 | count_op++; |
hitonari | 0:2b57931c6ed4 | 740 | if(count_op > changeModeCount) { |
hitonari | 0:2b57931c6ed4 | 741 | operation_mode = LeftLoop; |
hitonari | 0:2b57931c6ed4 | 742 | pc.printf("Goto Left mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 743 | pc.attach(NULL, Serial::RxIrq); |
hitonari | 0:2b57931c6ed4 | 744 | count_op = 0; |
hitonari | 0:2b57931c6ed4 | 745 | } |
hitonari | 0:2b57931c6ed4 | 746 | } else count_op = 0; |
hitonari | 0:2b57931c6ed4 | 747 | Take_off_and_landing(targetAngle); |
hitonari | 0:2b57931c6ed4 | 748 | break; |
hitonari | 0:2b57931c6ed4 | 749 | |
hitonari | 0:2b57931c6ed4 | 750 | default: |
hitonari | 0:2b57931c6ed4 | 751 | operation_mode = StartUp; |
hitonari | 0:2b57931c6ed4 | 752 | break; |
hitonari | 0:2b57931c6ed4 | 753 | } |
hitonari | 0:2b57931c6ed4 | 754 | #endif |
hitonari | 0:2b57931c6ed4 | 755 | |
hitonari | 0:2b57931c6ed4 | 756 | if(CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 757 | output_status = Auto; |
hitonari | 0:2b57931c6ed4 | 758 | led1 = 1; |
hitonari | 0:2b57931c6ed4 | 759 | } else { |
hitonari | 0:2b57931c6ed4 | 760 | output_status = Manual; |
hitonari | 0:2b57931c6ed4 | 761 | led1 = 0; |
hitonari | 0:2b57931c6ed4 | 762 | } |
hitonari | 0:2b57931c6ed4 | 763 | |
hitonari | 0:2b57931c6ed4 | 764 | |
hitonari | 0:2b57931c6ed4 | 765 | } |
hitonari | 0:2b57931c6ed4 | 766 | |
hitonari | 0:2b57931c6ed4 | 767 | int GetParameter(FILE *fp, const char *paramName,char parameter[]) |
hitonari | 0:2b57931c6ed4 | 768 | { |
hitonari | 0:2b57931c6ed4 | 769 | int i=0, j=0; |
hitonari | 0:2b57931c6ed4 | 770 | int strmax = 200; |
hitonari | 0:2b57931c6ed4 | 771 | char str[strmax]; |
hitonari | 0:2b57931c6ed4 | 772 | |
hitonari | 0:2b57931c6ed4 | 773 | rewind(fp); //ファイル位置を先頭に |
hitonari | 0:2b57931c6ed4 | 774 | while(1) { |
hitonari | 0:2b57931c6ed4 | 775 | if (fgets(str, strmax, fp) == NULL) { |
hitonari | 0:2b57931c6ed4 | 776 | return 0; |
hitonari | 0:2b57931c6ed4 | 777 | } |
hitonari | 0:2b57931c6ed4 | 778 | if (!strncmp(str, paramName, strlen(paramName))) { |
hitonari | 0:2b57931c6ed4 | 779 | while (str[i++] != '=') {} |
hitonari | 0:2b57931c6ed4 | 780 | while (str[i] != '\n') { |
hitonari | 0:2b57931c6ed4 | 781 | parameter[j++] = str[i++]; |
hitonari | 0:2b57931c6ed4 | 782 | } |
hitonari | 0:2b57931c6ed4 | 783 | parameter[j] = '\0'; |
hitonari | 0:2b57931c6ed4 | 784 | return 1; |
hitonari | 0:2b57931c6ed4 | 785 | } |
hitonari | 0:2b57931c6ed4 | 786 | } |
hitonari | 0:2b57931c6ed4 | 787 | } |
hitonari | 0:2b57931c6ed4 | 788 | |
hitonari | 0:2b57931c6ed4 | 789 | |
hitonari | 0:2b57931c6ed4 | 790 | //sdによる設定 |
hitonari | 0:2b57931c6ed4 | 791 | int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, |
hitonari | 0:2b57931c6ed4 | 792 | float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, |
hitonari | 0:2b57931c6ed4 | 793 | float *g_rightloopROLL, float *g_rightloopPITCH, |
hitonari | 0:2b57931c6ed4 | 794 | float *g_leftloopROLL, float *g_leftloopPITCH, |
hitonari | 0:2b57931c6ed4 | 795 | float *g_gostraightROLL, float *g_gostraightPITCH, |
hitonari | 0:2b57931c6ed4 | 796 | float *g_takeoffTHR, float *g_loopTHR, |
hitonari | 0:2b57931c6ed4 | 797 | float *g_rightloopROLLshort, float *g_rightloopPITCHshort, |
hitonari | 0:2b57931c6ed4 | 798 | float *g_leftloopROLLshort, float *g_leftloopPITCHshort, |
hitonari | 0:2b57931c6ed4 | 799 | float *g_glideloopROLL, float *g_glideloopPITCH, |
hitonari | 0:2b57931c6ed4 | 800 | float *g_kpAIL, float *g_kiAIL, float *g_kdAIL, |
hitonari | 0:2b57931c6ed4 | 801 | int *g_rightloopRUD, int *g_rightloopshortRUD, |
hitonari | 0:2b57931c6ed4 | 802 | int *g_leftloopRUD, int *g_leftloopshortRUD, |
hitonari | 0:2b57931c6ed4 | 803 | int *g_glideloopRUD, |
hitonari | 0:2b57931c6ed4 | 804 | float *g_rightloopROLL_approach,float *g_leftloopROLL_approach, |
hitonari | 0:2b57931c6ed4 | 805 | int *g_rightloopRUD_approach,int *g_leftloopRUD_approach, |
hitonari | 0:2b57931c6ed4 | 806 | float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach, |
hitonari | 0:2b57931c6ed4 | 807 | float *g_ascendingPITCH, |
hitonari | 0:2b57931c6ed4 | 808 | int *g_take_off_THROTTLE, float *g_take_off_PITCH, |
hitonari | 0:2b57931c6ed4 | 809 | float *g_rightloop_THR_rate, float *g_leftloop_THR_rate, |
hitonari | 0:2b57931c6ed4 | 810 | float *g_ascending_THR_rate, |
hitonari | 0:2b57931c6ed4 | 811 | float *g_ascending_ROLL, int *g_ascending_RUD, |
hitonari | 0:2b57931c6ed4 | 812 | int *g_trim1, int *g_trim2, int *g_trim3, |
hitonari | 0:2b57931c6ed4 | 813 | int *g_trim4, int *g_trim5, int *g_trim6, |
hitonari | 0:2b57931c6ed4 | 814 | bool *g_trim_check |
hitonari | 0:2b57931c6ed4 | 815 | ) |
hitonari | 0:2b57931c6ed4 | 816 | { |
hitonari | 0:2b57931c6ed4 | 817 | |
hitonari | 0:2b57931c6ed4 | 818 | pc.printf("SDsetup start.\r\n"); |
hitonari | 0:2b57931c6ed4 | 819 | |
hitonari | 0:2b57931c6ed4 | 820 | FILE *fp; |
hitonari | 0:2b57931c6ed4 | 821 | char parameter[50]; //文字列渡す用の配列 |
hitonari | 0:2b57931c6ed4 | 822 | int SDerrorcount = 0; //取得できなかった数を返す |
hitonari | 0:2b57931c6ed4 | 823 | const char *paramNames[] = { |
hitonari | 0:2b57931c6ed4 | 824 | "KP_ELEVATOR", |
hitonari | 0:2b57931c6ed4 | 825 | "KI_ELEVATOR", |
hitonari | 0:2b57931c6ed4 | 826 | "KD_ELEVATOR", |
hitonari | 0:2b57931c6ed4 | 827 | "KP_RUDDER", |
hitonari | 0:2b57931c6ed4 | 828 | "KI_RUDDER", |
hitonari | 0:2b57931c6ed4 | 829 | "KD_RUDDER", |
hitonari | 0:2b57931c6ed4 | 830 | "RIGHTLOOP_ROLL", |
hitonari | 0:2b57931c6ed4 | 831 | "RIGHTLOOP_PITCH", |
hitonari | 0:2b57931c6ed4 | 832 | "LEFTLOOP_ROLL", |
hitonari | 0:2b57931c6ed4 | 833 | "LEFTLOOP_PITCH", |
hitonari | 0:2b57931c6ed4 | 834 | "GOSTRAIGHT_ROLL", |
hitonari | 0:2b57931c6ed4 | 835 | "GOSTRAIGHT_PITCH", |
hitonari | 0:2b57931c6ed4 | 836 | "TAKEOFF_THR_RATE", |
hitonari | 0:2b57931c6ed4 | 837 | "LOOP_THR_RATE", |
hitonari | 0:2b57931c6ed4 | 838 | "RIGHTLOOP_ROLL_SHORT", |
hitonari | 0:2b57931c6ed4 | 839 | "RIGHTLOOP_PITCH_SHORT", |
hitonari | 0:2b57931c6ed4 | 840 | "LEFTLOOP_ROLL_SHORT", |
hitonari | 0:2b57931c6ed4 | 841 | "LEFTLOOP_PITCH_SHORT", |
hitonari | 0:2b57931c6ed4 | 842 | "AUTOGLIDE_ROLL", |
hitonari | 0:2b57931c6ed4 | 843 | "AUTOGLIDE PITCH", |
hitonari | 0:2b57931c6ed4 | 844 | "KP_AILERON", |
hitonari | 0:2b57931c6ed4 | 845 | "KI_AILERON", |
hitonari | 0:2b57931c6ed4 | 846 | "KD_AILERON", |
hitonari | 0:2b57931c6ed4 | 847 | "RIGHTLOOP_RUDDER", |
hitonari | 0:2b57931c6ed4 | 848 | "RIGHTLOOPSHORT_RUDDER", |
hitonari | 0:2b57931c6ed4 | 849 | "LEFTLOOP_RUDDER", |
hitonari | 0:2b57931c6ed4 | 850 | "LEFTLOOPSHORT_RUDDER", |
hitonari | 0:2b57931c6ed4 | 851 | "GLIDELOOP_RUDDER", |
hitonari | 0:2b57931c6ed4 | 852 | "RIGHTLOOP_ROLL_APPROACH", |
hitonari | 0:2b57931c6ed4 | 853 | "LEFTLOOP_ROLL_APPROACH", |
hitonari | 0:2b57931c6ed4 | 854 | "RIGHTLOOP_RUDDER_APPROACH", |
hitonari | 0:2b57931c6ed4 | 855 | "LEFTLOOP_RUDDER_APPROACH", |
hitonari | 0:2b57931c6ed4 | 856 | "RIGHTLOOP_PITCH_APPROACH", |
hitonari | 0:2b57931c6ed4 | 857 | "LEFTLOOP_PITCH_APPROACH", |
hitonari | 0:2b57931c6ed4 | 858 | "ASCENDING_PITCH", |
hitonari | 0:2b57931c6ed4 | 859 | "TAKE_OFF_THROTTLE", |
hitonari | 0:2b57931c6ed4 | 860 | "TAKE_OFF_PITCH", |
hitonari | 0:2b57931c6ed4 | 861 | "RIGHTLOOP_THR_RATE", |
hitonari | 0:2b57931c6ed4 | 862 | "LEFTLOOP_THR_RATE", |
hitonari | 0:2b57931c6ed4 | 863 | "ASCENDING_THR_RATE", |
hitonari | 0:2b57931c6ed4 | 864 | "ASCENDING_ROLL", |
hitonari | 0:2b57931c6ed4 | 865 | "ASCENDING_RUD", |
hitonari | 0:2b57931c6ed4 | 866 | "TRIM1", |
hitonari | 0:2b57931c6ed4 | 867 | "TRIM2", |
hitonari | 0:2b57931c6ed4 | 868 | "TRIM3", |
hitonari | 0:2b57931c6ed4 | 869 | "TRIM4", |
hitonari | 0:2b57931c6ed4 | 870 | "TRIM5", |
hitonari | 0:2b57931c6ed4 | 871 | "TRIM6", |
hitonari | 0:2b57931c6ed4 | 872 | "TRIM_CHECK" |
hitonari | 0:2b57931c6ed4 | 873 | }; |
hitonari | 0:2b57931c6ed4 | 874 | |
hitonari | 0:2b57931c6ed4 | 875 | fp = fopen("/sd/option.txt","r"); |
hitonari | 0:2b57931c6ed4 | 876 | |
hitonari | 0:2b57931c6ed4 | 877 | if(fp != NULL) { //開けたら |
hitonari | 0:2b57931c6ed4 | 878 | pc.printf("File was openned.\r\n"); |
hitonari | 0:2b57931c6ed4 | 879 | if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 880 | else { |
hitonari | 0:2b57931c6ed4 | 881 | *g_kpELE = KP_ELE; |
hitonari | 0:2b57931c6ed4 | 882 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 883 | } |
hitonari | 0:2b57931c6ed4 | 884 | if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 885 | else { |
hitonari | 0:2b57931c6ed4 | 886 | *g_kiELE = KI_ELE; |
hitonari | 0:2b57931c6ed4 | 887 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 888 | } |
hitonari | 0:2b57931c6ed4 | 889 | if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 890 | else { |
hitonari | 0:2b57931c6ed4 | 891 | *g_kdELE = KD_ELE; |
hitonari | 0:2b57931c6ed4 | 892 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 893 | } |
hitonari | 0:2b57931c6ed4 | 894 | if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 895 | else { |
hitonari | 0:2b57931c6ed4 | 896 | *g_kpRUD = KP_RUD; |
hitonari | 0:2b57931c6ed4 | 897 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 898 | } |
hitonari | 0:2b57931c6ed4 | 899 | if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 900 | else { |
hitonari | 0:2b57931c6ed4 | 901 | *g_kiRUD = KI_RUD; |
hitonari | 0:2b57931c6ed4 | 902 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 903 | } |
hitonari | 0:2b57931c6ed4 | 904 | if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 905 | else { |
hitonari | 0:2b57931c6ed4 | 906 | *g_kdRUD = KD_RUD; |
hitonari | 0:2b57931c6ed4 | 907 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 908 | } |
hitonari | 0:2b57931c6ed4 | 909 | if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 910 | else { |
hitonari | 0:2b57931c6ed4 | 911 | *g_rightloopROLL = RIGHT_ROLL; |
hitonari | 0:2b57931c6ed4 | 912 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 913 | } |
hitonari | 0:2b57931c6ed4 | 914 | if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 915 | else { |
hitonari | 0:2b57931c6ed4 | 916 | *g_rightloopPITCH = RIGHT_PITCH; |
hitonari | 0:2b57931c6ed4 | 917 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 918 | } |
hitonari | 0:2b57931c6ed4 | 919 | if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 920 | else { |
hitonari | 0:2b57931c6ed4 | 921 | *g_leftloopROLL = LEFT_ROLL; |
hitonari | 0:2b57931c6ed4 | 922 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 923 | } |
hitonari | 0:2b57931c6ed4 | 924 | if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 925 | else { |
hitonari | 0:2b57931c6ed4 | 926 | *g_leftloopPITCH = LEFT_PITCH; |
hitonari | 0:2b57931c6ed4 | 927 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 928 | } |
hitonari | 0:2b57931c6ed4 | 929 | if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 930 | else { |
hitonari | 0:2b57931c6ed4 | 931 | *g_gostraightROLL = STRAIGHT_ROLL; |
hitonari | 0:2b57931c6ed4 | 932 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 933 | } |
hitonari | 0:2b57931c6ed4 | 934 | if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 935 | else { |
hitonari | 0:2b57931c6ed4 | 936 | *g_gostraightPITCH = STRAIGHT_PITCH; |
hitonari | 0:2b57931c6ed4 | 937 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 938 | } |
hitonari | 0:2b57931c6ed4 | 939 | if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 940 | else { |
hitonari | 0:2b57931c6ed4 | 941 | *g_takeoffTHR = TAKEOFF_THR; |
hitonari | 0:2b57931c6ed4 | 942 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 943 | } |
hitonari | 0:2b57931c6ed4 | 944 | if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 945 | else { |
hitonari | 0:2b57931c6ed4 | 946 | *g_loopTHR = LOOP_THR; |
hitonari | 0:2b57931c6ed4 | 947 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 948 | } |
hitonari | 0:2b57931c6ed4 | 949 | if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 950 | else { |
hitonari | 0:2b57931c6ed4 | 951 | *g_rightloopROLLshort = RIGHT_ROLL_SHORT; |
hitonari | 0:2b57931c6ed4 | 952 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 953 | } |
hitonari | 0:2b57931c6ed4 | 954 | if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 955 | else { |
hitonari | 0:2b57931c6ed4 | 956 | *g_rightloopPITCHshort = RIGHT_PITCH_SHORT; |
hitonari | 0:2b57931c6ed4 | 957 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 958 | } |
hitonari | 0:2b57931c6ed4 | 959 | if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 960 | else { |
hitonari | 0:2b57931c6ed4 | 961 | *g_leftloopROLLshort = LEFT_ROLL_SHORT; |
hitonari | 0:2b57931c6ed4 | 962 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 963 | } |
hitonari | 0:2b57931c6ed4 | 964 | if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 965 | else { |
hitonari | 0:2b57931c6ed4 | 966 | *g_leftloopPITCHshort = LEFT_PITCH_SHORT; |
hitonari | 0:2b57931c6ed4 | 967 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 968 | } |
hitonari | 0:2b57931c6ed4 | 969 | if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 970 | else { |
hitonari | 0:2b57931c6ed4 | 971 | *g_glideloopROLL = GLIDE_ROLL; |
hitonari | 0:2b57931c6ed4 | 972 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 973 | } |
hitonari | 0:2b57931c6ed4 | 974 | if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 975 | else { |
hitonari | 0:2b57931c6ed4 | 976 | *g_glideloopPITCH = GLIDE_PITCH; |
hitonari | 0:2b57931c6ed4 | 977 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 978 | } |
hitonari | 0:2b57931c6ed4 | 979 | if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 980 | else { |
hitonari | 0:2b57931c6ed4 | 981 | *g_kpAIL = KP_AIL; |
hitonari | 0:2b57931c6ed4 | 982 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 983 | } |
hitonari | 0:2b57931c6ed4 | 984 | if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 985 | else { |
hitonari | 0:2b57931c6ed4 | 986 | *g_kiAIL = KI_AIL; |
hitonari | 0:2b57931c6ed4 | 987 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 988 | } |
hitonari | 0:2b57931c6ed4 | 989 | if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 990 | else { |
hitonari | 0:2b57931c6ed4 | 991 | *g_kdAIL = KP_AIL; |
hitonari | 0:2b57931c6ed4 | 992 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 993 | } |
hitonari | 0:2b57931c6ed4 | 994 | if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 995 | else { |
hitonari | 0:2b57931c6ed4 | 996 | *g_rightloopRUD = RIGHTLOOP_RUD; |
hitonari | 0:2b57931c6ed4 | 997 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 998 | } |
hitonari | 0:2b57931c6ed4 | 999 | if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1000 | else { |
hitonari | 0:2b57931c6ed4 | 1001 | *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD; |
hitonari | 0:2b57931c6ed4 | 1002 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1003 | } |
hitonari | 0:2b57931c6ed4 | 1004 | if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1005 | else { |
hitonari | 0:2b57931c6ed4 | 1006 | *g_leftloopshortRUD = LEFTLOOP_RUD; |
hitonari | 0:2b57931c6ed4 | 1007 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1008 | } |
hitonari | 0:2b57931c6ed4 | 1009 | if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1010 | else { |
hitonari | 0:2b57931c6ed4 | 1011 | *g_leftloopshortRUD = LEFTLOOPSHORT_RUD; |
hitonari | 0:2b57931c6ed4 | 1012 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1013 | } |
hitonari | 0:2b57931c6ed4 | 1014 | if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1015 | else { |
hitonari | 0:2b57931c6ed4 | 1016 | *g_glideloopRUD = GLIDELOOP_RUD; |
hitonari | 0:2b57931c6ed4 | 1017 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1018 | } |
hitonari | 0:2b57931c6ed4 | 1019 | if(GetParameter(fp,paramNames[28],parameter)) *g_rightloopROLL_approach = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1020 | else { |
hitonari | 0:2b57931c6ed4 | 1021 | *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1022 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1023 | } |
hitonari | 0:2b57931c6ed4 | 1024 | if(GetParameter(fp,paramNames[29],parameter)) *g_leftloopROLL_approach= atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1025 | else { |
hitonari | 0:2b57931c6ed4 | 1026 | *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1027 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1028 | } |
hitonari | 0:2b57931c6ed4 | 1029 | if(GetParameter(fp,paramNames[30],parameter)) *g_rightloopRUD_approach = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1030 | else { |
hitonari | 0:2b57931c6ed4 | 1031 | *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1032 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1033 | } |
hitonari | 0:2b57931c6ed4 | 1034 | if(GetParameter(fp,paramNames[31],parameter)) *g_leftloopRUD_approach= atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1035 | else { |
hitonari | 0:2b57931c6ed4 | 1036 | *g_leftloopRUD_approach= LEFTLOOPRUD_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1037 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1038 | } |
hitonari | 0:2b57931c6ed4 | 1039 | if(GetParameter(fp,paramNames[32],parameter)) *g_rightloopPITCH_approach = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1040 | else { |
hitonari | 0:2b57931c6ed4 | 1041 | *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1042 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1043 | } |
hitonari | 0:2b57931c6ed4 | 1044 | if(GetParameter(fp,paramNames[33],parameter)) *g_leftloopPITCH_approach = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1045 | else { |
hitonari | 0:2b57931c6ed4 | 1046 | *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1047 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1048 | } |
hitonari | 0:2b57931c6ed4 | 1049 | if(GetParameter(fp,paramNames[34],parameter)) *g_ascendingPITCH = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1050 | else { |
hitonari | 0:2b57931c6ed4 | 1051 | *g_ascendingPITCH = ASCENDING_PITCH; |
hitonari | 0:2b57931c6ed4 | 1052 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1053 | } |
hitonari | 0:2b57931c6ed4 | 1054 | if(GetParameter(fp,paramNames[35],parameter)) *g_take_off_THROTTLE = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1055 | else { |
hitonari | 0:2b57931c6ed4 | 1056 | *g_take_off_THROTTLE = TAKE_OFF_THROTTLE; |
hitonari | 0:2b57931c6ed4 | 1057 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1058 | } |
hitonari | 0:2b57931c6ed4 | 1059 | if(GetParameter(fp,paramNames[36],parameter)) *g_take_off_PITCH = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1060 | else { |
hitonari | 0:2b57931c6ed4 | 1061 | *g_take_off_PITCH = TAKE_OFF_PITCH; |
hitonari | 0:2b57931c6ed4 | 1062 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1063 | } |
hitonari | 0:2b57931c6ed4 | 1064 | if(GetParameter(fp,paramNames[37],parameter)) *g_rightloop_THR_rate = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1065 | else { |
hitonari | 0:2b57931c6ed4 | 1066 | *g_rightloop_THR_rate = RIGHTLOOP_THR_RATE; |
hitonari | 0:2b57931c6ed4 | 1067 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1068 | } |
hitonari | 0:2b57931c6ed4 | 1069 | if(GetParameter(fp,paramNames[38],parameter)) *g_leftloop_THR_rate = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1070 | else { |
hitonari | 0:2b57931c6ed4 | 1071 | *g_leftloop_THR_rate = LEFTLOOP_THR_RATE; |
hitonari | 0:2b57931c6ed4 | 1072 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1073 | } |
hitonari | 0:2b57931c6ed4 | 1074 | if(GetParameter(fp,paramNames[39],parameter)) *g_ascending_THR_rate = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1075 | else { |
hitonari | 0:2b57931c6ed4 | 1076 | *g_ascending_THR_rate = ASCENDING_THR_RATE; |
hitonari | 0:2b57931c6ed4 | 1077 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1078 | } |
hitonari | 0:2b57931c6ed4 | 1079 | if(GetParameter(fp,paramNames[40],parameter)) *g_ascending_ROLL = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1080 | else { |
hitonari | 0:2b57931c6ed4 | 1081 | *g_ascending_ROLL = ASCENDING_ROLL; |
hitonari | 0:2b57931c6ed4 | 1082 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1083 | } |
hitonari | 0:2b57931c6ed4 | 1084 | if(GetParameter(fp,paramNames[41],parameter)) *g_ascending_RUD= atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1085 | else { |
hitonari | 0:2b57931c6ed4 | 1086 | *g_ascending_RUD = ASCENDING_RUD; |
hitonari | 0:2b57931c6ed4 | 1087 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1088 | } |
hitonari | 0:2b57931c6ed4 | 1089 | if(GetParameter(fp,paramNames[42],parameter)) *g_trim1 = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1090 | else { |
hitonari | 0:2b57931c6ed4 | 1091 | *g_trim1 = TRIM1; |
hitonari | 0:2b57931c6ed4 | 1092 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1093 | } |
hitonari | 0:2b57931c6ed4 | 1094 | if(GetParameter(fp,paramNames[43],parameter)) *g_trim2 = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1095 | else { |
hitonari | 0:2b57931c6ed4 | 1096 | *g_trim2 = TRIM2; |
hitonari | 0:2b57931c6ed4 | 1097 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1098 | } |
hitonari | 0:2b57931c6ed4 | 1099 | if(GetParameter(fp,paramNames[44],parameter)) *g_trim3 = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1100 | else { |
hitonari | 0:2b57931c6ed4 | 1101 | *g_trim3 = TRIM3; |
hitonari | 0:2b57931c6ed4 | 1102 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1103 | } |
hitonari | 0:2b57931c6ed4 | 1104 | if(GetParameter(fp,paramNames[45],parameter)) *g_trim4 = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1105 | else { |
hitonari | 0:2b57931c6ed4 | 1106 | *g_trim4 = TRIM4; |
hitonari | 0:2b57931c6ed4 | 1107 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1108 | } |
hitonari | 0:2b57931c6ed4 | 1109 | if(GetParameter(fp,paramNames[46],parameter)) *g_trim5 = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1110 | else { |
hitonari | 0:2b57931c6ed4 | 1111 | *g_trim5 = TRIM5; |
hitonari | 0:2b57931c6ed4 | 1112 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1113 | } |
hitonari | 0:2b57931c6ed4 | 1114 | if(GetParameter(fp,paramNames[47],parameter)) *g_trim6 = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1115 | else { |
hitonari | 0:2b57931c6ed4 | 1116 | *g_trim6 = TRIM6; |
hitonari | 0:2b57931c6ed4 | 1117 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1118 | } |
hitonari | 0:2b57931c6ed4 | 1119 | if(GetParameter(fp,paramNames[48],parameter)) *g_trim_check = atof(parameter); |
hitonari | 0:2b57931c6ed4 | 1120 | else { |
hitonari | 0:2b57931c6ed4 | 1121 | *g_trim_check = TRIM_CHECK; |
hitonari | 0:2b57931c6ed4 | 1122 | SDerrorcount++; |
hitonari | 0:2b57931c6ed4 | 1123 | } |
hitonari | 0:2b57931c6ed4 | 1124 | |
hitonari | 0:2b57931c6ed4 | 1125 | |
hitonari | 0:2b57931c6ed4 | 1126 | fclose(fp); |
hitonari | 0:2b57931c6ed4 | 1127 | |
hitonari | 0:2b57931c6ed4 | 1128 | } else { //ファイルがなかったら |
hitonari | 0:2b57931c6ed4 | 1129 | pc.printf("fp was null.\r\n"); |
hitonari | 0:2b57931c6ed4 | 1130 | *g_kpELE = KP_ELE; |
hitonari | 0:2b57931c6ed4 | 1131 | *g_kiELE = KI_ELE; |
hitonari | 0:2b57931c6ed4 | 1132 | *g_kdELE = KD_ELE; |
hitonari | 0:2b57931c6ed4 | 1133 | *g_kpRUD = KP_RUD; |
hitonari | 0:2b57931c6ed4 | 1134 | *g_kiRUD = KI_RUD; |
hitonari | 0:2b57931c6ed4 | 1135 | *g_kdRUD = KD_RUD; |
hitonari | 0:2b57931c6ed4 | 1136 | *g_rightloopROLL = RIGHT_ROLL; |
hitonari | 0:2b57931c6ed4 | 1137 | *g_rightloopPITCH = RIGHT_PITCH; |
hitonari | 0:2b57931c6ed4 | 1138 | *g_leftloopROLL = LEFT_ROLL; |
hitonari | 0:2b57931c6ed4 | 1139 | *g_leftloopPITCH = LEFT_PITCH; |
hitonari | 0:2b57931c6ed4 | 1140 | *g_gostraightROLL = STRAIGHT_ROLL; |
hitonari | 0:2b57931c6ed4 | 1141 | *g_gostraightPITCH = STRAIGHT_PITCH; |
hitonari | 0:2b57931c6ed4 | 1142 | *g_takeoffTHR = TAKEOFF_THR; |
hitonari | 0:2b57931c6ed4 | 1143 | *g_loopTHR = LOOP_THR; |
hitonari | 0:2b57931c6ed4 | 1144 | *g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく!! |
hitonari | 0:2b57931c6ed4 | 1145 | *g_kiAIL = KI_AIL; |
hitonari | 0:2b57931c6ed4 | 1146 | *g_kdAIL = KD_AIL; |
hitonari | 0:2b57931c6ed4 | 1147 | *g_rightloopRUD = RIGHTLOOP_RUD; |
hitonari | 0:2b57931c6ed4 | 1148 | *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD; |
hitonari | 0:2b57931c6ed4 | 1149 | *g_leftloopRUD = LEFTLOOP_RUD; |
hitonari | 0:2b57931c6ed4 | 1150 | *g_leftloopshortRUD = LEFTLOOPSHORT_RUD; |
hitonari | 0:2b57931c6ed4 | 1151 | *g_glideloopRUD = GLIDELOOP_RUD; |
hitonari | 0:2b57931c6ed4 | 1152 | *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1153 | *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1154 | *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1155 | *g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1156 | *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1157 | *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH; |
hitonari | 0:2b57931c6ed4 | 1158 | *g_ascendingPITCH = ASCENDING_PITCH; |
hitonari | 0:2b57931c6ed4 | 1159 | *g_take_off_THROTTLE = TAKE_OFF_THROTTLE; |
hitonari | 0:2b57931c6ed4 | 1160 | *g_take_off_PITCH = TAKE_OFF_PITCH; |
hitonari | 0:2b57931c6ed4 | 1161 | *g_rightloop_THR_rate = RIGHTLOOP_THR_RATE; |
hitonari | 0:2b57931c6ed4 | 1162 | *g_leftloop_THR_rate = LEFTLOOP_THR_RATE; |
hitonari | 0:2b57931c6ed4 | 1163 | *g_ascending_THR_rate = ASCENDING_THR_RATE; |
hitonari | 0:2b57931c6ed4 | 1164 | *g_ascending_ROLL = ASCENDING_ROLL; |
hitonari | 0:2b57931c6ed4 | 1165 | *g_ascending_RUD = ASCENDING_RUD; |
hitonari | 0:2b57931c6ed4 | 1166 | *g_trim1 = TRIM1; |
hitonari | 0:2b57931c6ed4 | 1167 | *g_trim2 = TRIM2; |
hitonari | 0:2b57931c6ed4 | 1168 | *g_trim3 = TRIM3; |
hitonari | 0:2b57931c6ed4 | 1169 | *g_trim4 = TRIM4; |
hitonari | 0:2b57931c6ed4 | 1170 | *g_trim5 = TRIM5; |
hitonari | 0:2b57931c6ed4 | 1171 | *g_trim6 = TRIM6; |
hitonari | 0:2b57931c6ed4 | 1172 | *g_trim_check = TRIM_CHECK; |
hitonari | 0:2b57931c6ed4 | 1173 | |
hitonari | 0:2b57931c6ed4 | 1174 | SDerrorcount = -1; |
hitonari | 0:2b57931c6ed4 | 1175 | } |
hitonari | 0:2b57931c6ed4 | 1176 | pc.printf("SDsetup finished.\r\n"); |
hitonari | 0:2b57931c6ed4 | 1177 | if(SDerrorcount == 0) pc.printf("setting option is success\r\n"); |
hitonari | 0:2b57931c6ed4 | 1178 | else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n"); |
hitonari | 0:2b57931c6ed4 | 1179 | else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); |
hitonari | 0:2b57931c6ed4 | 1180 | |
hitonari | 0:2b57931c6ed4 | 1181 | pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE); |
hitonari | 0:2b57931c6ed4 | 1182 | pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD); |
hitonari | 0:2b57931c6ed4 | 1183 | pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH); |
hitonari | 0:2b57931c6ed4 | 1184 | pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", *g_leftloopROLL, *g_leftloopPITCH); |
hitonari | 0:2b57931c6ed4 | 1185 | pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH); |
hitonari | 0:2b57931c6ed4 | 1186 | pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR); |
hitonari | 0:2b57931c6ed4 | 1187 | pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort); |
hitonari | 0:2b57931c6ed4 | 1188 | pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort); |
hitonari | 0:2b57931c6ed4 | 1189 | pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH); |
hitonari | 0:2b57931c6ed4 | 1190 | pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",*g_kpAIL,*g_kiAIL,*g_kdAIL); |
hitonari | 0:2b57931c6ed4 | 1191 | pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",*g_rightloopRUD,*g_rightloopshortRUD); |
hitonari | 0:2b57931c6ed4 | 1192 | pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",*g_leftloopRUD,*g_leftloopshortRUD); |
hitonari | 0:2b57931c6ed4 | 1193 | pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD); |
hitonari | 0:2b57931c6ed4 | 1194 | pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",*g_rightloopROLL_approach,*g_leftloopROLL_approach); |
hitonari | 0:2b57931c6ed4 | 1195 | pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",*g_rightloopRUD_approach,*g_leftloopRUD_approach); |
hitonari | 0:2b57931c6ed4 | 1196 | pc.printf("RIGHTLOOP_PITCH_APPROACH = %f, LEFTLOOP_PITCH_APPROACH = %f\r\n",*g_rightloopPITCH_approach,*g_leftloopPITCH_approach); |
hitonari | 0:2b57931c6ed4 | 1197 | pc.printf("ASCENDING_PITCH = %f\r\n",*g_ascendingPITCH); |
hitonari | 0:2b57931c6ed4 | 1198 | pc.printf("TAKE_OFF_THROTTLE = %d, TAKE_OFF_PITCH = %f\r\n",*g_take_off_THROTTLE,*g_take_off_PITCH); |
hitonari | 0:2b57931c6ed4 | 1199 | pc.printf("RIGHTLOOP_THR_RATE = %f, LEFTLOOP_THR_RATE = %f, ASCENDING_THR_RATE = %f\r\n", *g_rightloop_THR_rate, *g_leftloop_THR_rate, *g_ascending_THR_rate); |
hitonari | 0:2b57931c6ed4 | 1200 | pc.printf("ASCENDING_ROLL = %f,ASCENDING_RUD = %d\r\n",*g_ascending_ROLL,*g_ascending_RUD); |
hitonari | 0:2b57931c6ed4 | 1201 | pc.printf("TRIM1 = %d, TRIM2 = %d, TRIM3 = %d\r\n",*g_trim1,*g_trim2,*g_trim3); |
hitonari | 0:2b57931c6ed4 | 1202 | pc.printf("TRIM4 = %d, TRIM5 = %d, TRIM6 = %d\r\n",*g_trim4,*g_trim5,*g_trim6); |
hitonari | 0:2b57931c6ed4 | 1203 | pc.printf("TRIM_CHECK = %d\r\n",*g_trim_check); |
hitonari | 0:2b57931c6ed4 | 1204 | |
hitonari | 0:2b57931c6ed4 | 1205 | return SDerrorcount; |
hitonari | 0:2b57931c6ed4 | 1206 | } |
hitonari | 0:2b57931c6ed4 | 1207 | |
hitonari | 0:2b57931c6ed4 | 1208 | void CalculateControlValue(float targetAngle[3], float controlValue[3]) |
hitonari | 0:2b57931c6ed4 | 1209 | { |
hitonari | 0:2b57931c6ed4 | 1210 | |
hitonari | 0:2b57931c6ed4 | 1211 | static int t_last; |
hitonari | 0:2b57931c6ed4 | 1212 | int t_now; |
hitonari | 0:2b57931c6ed4 | 1213 | float dt; |
hitonari | 0:2b57931c6ed4 | 1214 | |
hitonari | 0:2b57931c6ed4 | 1215 | t_now = t.read_us(); |
hitonari | 0:2b57931c6ed4 | 1216 | dt = (float)((t_now - t_last)/1000000.0f) ; |
hitonari | 0:2b57931c6ed4 | 1217 | t_last = t_now; |
hitonari | 0:2b57931c6ed4 | 1218 | |
hitonari | 0:2b57931c6ed4 | 1219 | |
hitonari | 0:2b57931c6ed4 | 1220 | //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); |
hitonari | 0:2b57931c6ed4 | 1221 | controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御 |
hitonari | 0:2b57931c6ed4 | 1222 | controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); |
hitonari | 0:2b57931c6ed4 | 1223 | |
hitonari | 0:2b57931c6ed4 | 1224 | } |
hitonari | 0:2b57931c6ed4 | 1225 | |
hitonari | 0:2b57931c6ed4 | 1226 | void UpdateAutoPWM(float controlValue[3]) |
hitonari | 0:2b57931c6ed4 | 1227 | { |
hitonari | 0:2b57931c6ed4 | 1228 | NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1229 | int16_t addpwm[2]; //-500~500 |
hitonari | 0:2b57931c6ed4 | 1230 | addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ:機首下げ正 レバー:機首上げ正 |
hitonari | 0:2b57931c6ed4 | 1231 | addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ:右回転正(8月13日時点;左回転が正!) レバー:右回転正 |
hitonari | 0:2b57931c6ed4 | 1232 | |
hitonari | 0:2b57931c6ed4 | 1233 | autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite |
hitonari | 0:2b57931c6ed4 | 1234 | autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL]; |
hitonari | 0:2b57931c6ed4 | 1235 | //autopwm[THR] = oldTHR; |
hitonari | 0:2b57931c6ed4 | 1236 | |
hitonari | 0:2b57931c6ed4 | 1237 | autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); |
hitonari | 0:2b57931c6ed4 | 1238 | autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]); |
hitonari | 0:2b57931c6ed4 | 1239 | |
hitonari | 0:2b57931c6ed4 | 1240 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1241 | } |
hitonari | 0:2b57931c6ed4 | 1242 | |
hitonari | 0:2b57931c6ed4 | 1243 | inline float CalcRatio(float value, float trim, float limit) |
hitonari | 0:2b57931c6ed4 | 1244 | { |
hitonari | 0:2b57931c6ed4 | 1245 | return (value - trim) / (limit - trim); |
hitonari | 0:2b57931c6ed4 | 1246 | } |
hitonari | 0:2b57931c6ed4 | 1247 | |
hitonari | 0:2b57931c6ed4 | 1248 | bool CheckSW_Up(Channel ch) |
hitonari | 0:2b57931c6ed4 | 1249 | { |
hitonari | 0:2b57931c6ed4 | 1250 | |
hitonari | 0:2b57931c6ed4 | 1251 | if(SWITCH_CHECK < sbus.manualpwm[ch]) { |
hitonari | 0:2b57931c6ed4 | 1252 | return true; |
hitonari | 0:2b57931c6ed4 | 1253 | } else { |
hitonari | 0:2b57931c6ed4 | 1254 | return false; |
hitonari | 0:2b57931c6ed4 | 1255 | } |
hitonari | 0:2b57931c6ed4 | 1256 | |
hitonari | 0:2b57931c6ed4 | 1257 | } |
hitonari | 0:2b57931c6ed4 | 1258 | |
hitonari | 0:2b57931c6ed4 | 1259 | int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min) |
hitonari | 0:2b57931c6ed4 | 1260 | { |
hitonari | 0:2b57931c6ed4 | 1261 | if(value > max) return max; |
hitonari | 0:2b57931c6ed4 | 1262 | if(value < min) return min; |
hitonari | 0:2b57931c6ed4 | 1263 | return value; |
hitonari | 0:2b57931c6ed4 | 1264 | } |
hitonari | 0:2b57931c6ed4 | 1265 | |
hitonari | 0:2b57931c6ed4 | 1266 | inline int16_t SetTHRinRatio(float ratio) |
hitonari | 0:2b57931c6ed4 | 1267 | { |
hitonari | 0:2b57931c6ed4 | 1268 | return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); |
hitonari | 0:2b57931c6ed4 | 1269 | } |
hitonari | 0:2b57931c6ed4 | 1270 | |
hitonari | 0:2b57931c6ed4 | 1271 | |
hitonari | 0:2b57931c6ed4 | 1272 | |
hitonari | 0:2b57931c6ed4 | 1273 | /*---SBUS割り込み処理---*/ |
hitonari | 0:2b57931c6ed4 | 1274 | |
hitonari | 0:2b57931c6ed4 | 1275 | //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) |
hitonari | 0:2b57931c6ed4 | 1276 | //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) |
hitonari | 0:2b57931c6ed4 | 1277 | void Update_PWM() |
hitonari | 0:2b57931c6ed4 | 1278 | { |
hitonari | 0:2b57931c6ed4 | 1279 | NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1280 | static int16_t pwm[6]; |
hitonari | 0:2b57931c6ed4 | 1281 | static int16_t temppwm[6]= {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; |
hitonari | 0:2b57931c6ed4 | 1282 | static int16_t FailsafeCounter=0; |
hitonari | 0:2b57931c6ed4 | 1283 | static int16_t ResetCounter=0; |
hitonari | 0:2b57931c6ed4 | 1284 | static int16_t OKCounter=0; |
hitonari | 0:2b57931c6ed4 | 1285 | |
hitonari | 0:2b57931c6ed4 | 1286 | if(sbus.flg_ch_update == true) { |
hitonari | 0:2b57931c6ed4 | 1287 | |
hitonari | 0:2b57931c6ed4 | 1288 | switch(output_status) { //マニュアルモード,自動モード,自動着陸もモードを切替 |
hitonari | 0:2b57931c6ed4 | 1289 | case Manual: |
hitonari | 0:2b57931c6ed4 | 1290 | if(OKCounter!=0) break; |
hitonari | 0:2b57931c6ed4 | 1291 | for(uint8_t i=0; i<6; i++) { |
hitonari | 0:2b57931c6ed4 | 1292 | pwm[i] = sbus.manualpwm[i]; |
hitonari | 0:2b57931c6ed4 | 1293 | } |
hitonari | 0:2b57931c6ed4 | 1294 | oldTHR = sbus.manualpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1295 | //pc.printf("update_manual\r\n"); |
hitonari | 0:2b57931c6ed4 | 1296 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1297 | break; |
hitonari | 0:2b57931c6ed4 | 1298 | |
hitonari | 0:2b57931c6ed4 | 1299 | case Auto: |
hitonari | 0:2b57931c6ed4 | 1300 | if(OKCounter!=0) break; |
hitonari | 0:2b57931c6ed4 | 1301 | pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL]; |
hitonari | 0:2b57931c6ed4 | 1302 | pwm[ELE] = autopwm[ELE]; |
hitonari | 0:2b57931c6ed4 | 1303 | pwm[THR] = autopwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1304 | pwm[RUD] = autopwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 1305 | pwm[DROP] = autopwm[DROP]; |
hitonari | 0:2b57931c6ed4 | 1306 | pwm[AIL_L] = autopwm[AIL_L]; |
hitonari | 0:2b57931c6ed4 | 1307 | |
hitonari | 0:2b57931c6ed4 | 1308 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1309 | break; |
hitonari | 0:2b57931c6ed4 | 1310 | |
hitonari | 0:2b57931c6ed4 | 1311 | default: |
hitonari | 0:2b57931c6ed4 | 1312 | if(OKCounter!=0) break; |
hitonari | 0:2b57931c6ed4 | 1313 | for(uint8_t i=0; i<6; i++) { |
hitonari | 0:2b57931c6ed4 | 1314 | pwm[i] = sbus.manualpwm[i]; |
hitonari | 0:2b57931c6ed4 | 1315 | } //pc.printf("update_manual\r\n"); |
hitonari | 0:2b57931c6ed4 | 1316 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1317 | break; |
hitonari | 0:2b57931c6ed4 | 1318 | } |
hitonari | 0:2b57931c6ed4 | 1319 | |
hitonari | 0:2b57931c6ed4 | 1320 | for(uint8_t i=0; i<6; i++) { |
hitonari | 0:2b57931c6ed4 | 1321 | if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; |
hitonari | 0:2b57931c6ed4 | 1322 | temppwm[i]=pwm[i]; |
hitonari | 0:2b57931c6ed4 | 1323 | } |
hitonari | 0:2b57931c6ed4 | 1324 | |
hitonari | 0:2b57931c6ed4 | 1325 | } |
hitonari | 0:2b57931c6ed4 | 1326 | //else(sbus.flg_ch_update == false) pc.printf("0\r\n"); |
hitonari | 0:2b57931c6ed4 | 1327 | /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){ |
hitonari | 0:2b57931c6ed4 | 1328 | pc.printf("OK\r\n"); |
hitonari | 0:2b57931c6ed4 | 1329 | } |
hitonari | 0:2b57931c6ed4 | 1330 | */ |
hitonari | 0:2b57931c6ed4 | 1331 | //pc.printf("%d\r\n",sbus.failsafe_status); |
hitonari | 0:2b57931c6ed4 | 1332 | |
hitonari | 0:2b57931c6ed4 | 1333 | if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++; |
hitonari | 0:2b57931c6ed4 | 1334 | else ResetCounter++; |
hitonari | 0:2b57931c6ed4 | 1335 | |
hitonari | 0:2b57931c6ed4 | 1336 | if(ResetCounter>7) { |
hitonari | 0:2b57931c6ed4 | 1337 | ResetCounter=0; |
hitonari | 0:2b57931c6ed4 | 1338 | FailsafeCounter=0; |
hitonari | 0:2b57931c6ed4 | 1339 | } |
hitonari | 0:2b57931c6ed4 | 1340 | |
hitonari | 0:2b57931c6ed4 | 1341 | if(FailsafeCounter>10) { |
hitonari | 0:2b57931c6ed4 | 1342 | ResetCounter=0; |
hitonari | 0:2b57931c6ed4 | 1343 | for(uint8_t i=0; i<6; i++) pwm[i] = trimpwm[i]; |
hitonari | 0:2b57931c6ed4 | 1344 | |
hitonari | 0:2b57931c6ed4 | 1345 | if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++; |
hitonari | 0:2b57931c6ed4 | 1346 | else OKCounter=0; |
hitonari | 0:2b57931c6ed4 | 1347 | |
hitonari | 0:2b57931c6ed4 | 1348 | if(OKCounter>10) { |
hitonari | 0:2b57931c6ed4 | 1349 | OKCounter=0; |
hitonari | 0:2b57931c6ed4 | 1350 | FailsafeCounter=0; |
hitonari | 0:2b57931c6ed4 | 1351 | } |
hitonari | 0:2b57931c6ed4 | 1352 | //pc.printf("OKCounter=%d, FailsafeCounter=%d, sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status); |
hitonari | 0:2b57931c6ed4 | 1353 | } |
hitonari | 0:2b57931c6ed4 | 1354 | //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;} |
hitonari | 0:2b57931c6ed4 | 1355 | |
hitonari | 0:2b57931c6ed4 | 1356 | |
hitonari | 0:2b57931c6ed4 | 1357 | sbus.flg_ch_update = false; |
hitonari | 0:2b57931c6ed4 | 1358 | Output_PWM(pwm); |
hitonari | 0:2b57931c6ed4 | 1359 | } |
hitonari | 0:2b57931c6ed4 | 1360 | |
hitonari | 0:2b57931c6ed4 | 1361 | |
hitonari | 0:2b57931c6ed4 | 1362 | |
hitonari | 0:2b57931c6ed4 | 1363 | |
hitonari | 0:2b57931c6ed4 | 1364 | //pwmをサーボに出力。 |
hitonari | 0:2b57931c6ed4 | 1365 | void Output_PWM(int16_t pwm[5]) |
hitonari | 0:2b57931c6ed4 | 1366 | { |
hitonari | 0:2b57931c6ed4 | 1367 | NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1368 | servo1.pulsewidth_us(pwm[0]); |
hitonari | 0:2b57931c6ed4 | 1369 | servo2.pulsewidth_us(pwm[1]); |
hitonari | 0:2b57931c6ed4 | 1370 | servo3.pulsewidth_us(pwm[2]); |
hitonari | 0:2b57931c6ed4 | 1371 | servo4.pulsewidth_us(pwm[3]); |
hitonari | 0:2b57931c6ed4 | 1372 | servo5.pulsewidth_us(pwm[4]); |
hitonari | 0:2b57931c6ed4 | 1373 | servo6.pulsewidth_us(pwm[5]); |
hitonari | 0:2b57931c6ed4 | 1374 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1375 | |
hitonari | 0:2b57931c6ed4 | 1376 | } |
hitonari | 0:2b57931c6ed4 | 1377 | |
hitonari | 0:2b57931c6ed4 | 1378 | void ResetTrim() |
hitonari | 0:2b57931c6ed4 | 1379 | { |
hitonari | 0:2b57931c6ed4 | 1380 | for(uint8_t i=0; i<6; i++) { //i=4から書き換え_投下サーボは入ってない模様 |
hitonari | 0:2b57931c6ed4 | 1381 | trimpwm[i] = sbus.manualpwm[i]; |
hitonari | 0:2b57931c6ed4 | 1382 | } |
hitonari | 0:2b57931c6ed4 | 1383 | pc.printf("reset PWM trim\r\n"); |
hitonari | 0:2b57931c6ed4 | 1384 | } |
hitonari | 0:2b57931c6ed4 | 1385 | |
hitonari | 0:2b57931c6ed4 | 1386 | |
hitonari | 0:2b57931c6ed4 | 1387 | void SensingMPU() |
hitonari | 0:2b57931c6ed4 | 1388 | { |
hitonari | 0:2b57931c6ed4 | 1389 | //static int16_t deltaT = 0, t_start = 0; |
hitonari | 0:2b57931c6ed4 | 1390 | //t_start = t.read_us(); |
hitonari | 0:2b57931c6ed4 | 1391 | |
hitonari | 0:2b57931c6ed4 | 1392 | float rpy[3] = {0}, oldrpy[3] = {0}; |
hitonari | 0:2b57931c6ed4 | 1393 | static uint16_t count_changeRPY = 0; |
hitonari | 0:2b57931c6ed4 | 1394 | static bool flg_checkoutlier = false; |
hitonari | 0:2b57931c6ed4 | 1395 | NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1396 | NVIC_DisableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1397 | NVIC_DisableIRQ(TIM5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1398 | NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止 |
hitonari | 0:2b57931c6ed4 | 1399 | NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止 |
hitonari | 0:2b57931c6ed4 | 1400 | |
hitonari | 0:2b57931c6ed4 | 1401 | mpu6050.getRollPitchYaw_Skipper(rpy); |
hitonari | 0:2b57931c6ed4 | 1402 | |
hitonari | 0:2b57931c6ed4 | 1403 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1404 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1405 | NVIC_EnableIRQ(TIM5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1406 | NVIC_EnableIRQ(EXTI0_IRQn); |
hitonari | 0:2b57931c6ed4 | 1407 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1408 | |
hitonari | 0:2b57931c6ed4 | 1409 | |
hitonari | 0:2b57931c6ed4 | 1410 | //外れ値対策 |
hitonari | 0:2b57931c6ed4 | 1411 | for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; |
hitonari | 0:2b57931c6ed4 | 1412 | rpy[ROLL] -= FirstROLL; |
hitonari | 0:2b57931c6ed4 | 1413 | rpy[PITCH] -= FirstPITCH; |
hitonari | 0:2b57931c6ed4 | 1414 | rpy[YAW] -= FirstYAW; |
hitonari | 0:2b57931c6ed4 | 1415 | |
hitonari | 0:2b57931c6ed4 | 1416 | for(uint8_t i=0; i<3; i++) { |
hitonari | 0:2b57931c6ed4 | 1417 | if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) { |
hitonari | 0:2b57931c6ed4 | 1418 | flg_checkoutlier = true; |
hitonari | 0:2b57931c6ed4 | 1419 | } |
hitonari | 0:2b57931c6ed4 | 1420 | } |
hitonari | 0:2b57931c6ed4 | 1421 | if(!flg_checkoutlier || count_changeRPY >= 2) { |
hitonari | 0:2b57931c6ed4 | 1422 | for(uint8_t i=0; i<3; i++) { |
hitonari | 0:2b57931c6ed4 | 1423 | nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 |
hitonari | 0:2b57931c6ed4 | 1424 | } |
hitonari | 0:2b57931c6ed4 | 1425 | count_changeRPY = 0; |
hitonari | 0:2b57931c6ed4 | 1426 | } else count_changeRPY++; |
hitonari | 0:2b57931c6ed4 | 1427 | flg_checkoutlier = false; |
hitonari | 0:2b57931c6ed4 | 1428 | |
hitonari | 0:2b57931c6ed4 | 1429 | } |
hitonari | 0:2b57931c6ed4 | 1430 | |
hitonari | 0:2b57931c6ed4 | 1431 | float TranslateNewYaw(float beforeYaw, float newzeroYaw) |
hitonari | 0:2b57931c6ed4 | 1432 | { |
hitonari | 0:2b57931c6ed4 | 1433 | float newYaw = beforeYaw - newzeroYaw; |
hitonari | 0:2b57931c6ed4 | 1434 | |
hitonari | 0:2b57931c6ed4 | 1435 | if(newYaw<-180.0f) newYaw += 360.0f; |
hitonari | 0:2b57931c6ed4 | 1436 | else if(newYaw>180.0f) newYaw -= 360.0f; |
hitonari | 0:2b57931c6ed4 | 1437 | return newYaw; |
hitonari | 0:2b57931c6ed4 | 1438 | } |
hitonari | 0:2b57931c6ed4 | 1439 | |
hitonari | 0:2b57931c6ed4 | 1440 | |
hitonari | 0:2b57931c6ed4 | 1441 | void getSF_Serial() |
hitonari | 0:2b57931c6ed4 | 1442 | { |
hitonari | 0:2b57931c6ed4 | 1443 | //NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1444 | //NVIC_DisableIRQ(EXTI0_IRQn); |
hitonari | 0:2b57931c6ed4 | 1445 | //NVIC_DisableIRQ(TIM5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1446 | |
hitonari | 0:2b57931c6ed4 | 1447 | |
hitonari | 0:2b57931c6ed4 | 1448 | static char SFbuf[16]= {'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'}; |
hitonari | 0:2b57931c6ed4 | 1449 | |
hitonari | 0:2b57931c6ed4 | 1450 | static int bufcounter=0; |
hitonari | 0:2b57931c6ed4 | 1451 | |
hitonari | 0:2b57931c6ed4 | 1452 | float targetyaw = 0; |
hitonari | 0:2b57931c6ed4 | 1453 | |
hitonari | 0:2b57931c6ed4 | 1454 | |
hitonari | 0:2b57931c6ed4 | 1455 | |
hitonari | 0:2b57931c6ed4 | 1456 | if(pc.readable()) { // 受信確認 |
hitonari | 0:2b57931c6ed4 | 1457 | |
hitonari | 0:2b57931c6ed4 | 1458 | SFbuf[bufcounter] = pc.getc(); // 1文字取り出し |
hitonari | 0:2b57931c6ed4 | 1459 | if(SFbuf[0]!='S') { |
hitonari | 0:2b57931c6ed4 | 1460 | //pc.printf("x"); |
hitonari | 0:2b57931c6ed4 | 1461 | return; |
hitonari | 0:2b57931c6ed4 | 1462 | } |
hitonari | 0:2b57931c6ed4 | 1463 | |
hitonari | 0:2b57931c6ed4 | 1464 | |
hitonari | 0:2b57931c6ed4 | 1465 | |
hitonari | 0:2b57931c6ed4 | 1466 | pc.printf("%c",SFbuf[bufcounter]); |
hitonari | 0:2b57931c6ed4 | 1467 | |
hitonari | 0:2b57931c6ed4 | 1468 | if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++; |
hitonari | 0:2b57931c6ed4 | 1469 | |
hitonari | 0:2b57931c6ed4 | 1470 | if(bufcounter==5 && SFbuf[4]=='F') { |
hitonari | 0:2b57931c6ed4 | 1471 | |
hitonari | 0:2b57931c6ed4 | 1472 | g_landingcommand = SFbuf[1]; |
hitonari | 0:2b57931c6ed4 | 1473 | //pc.printf("%c",g_landingcommand); |
hitonari | 0:2b57931c6ed4 | 1474 | wait_ms(10); |
hitonari | 0:2b57931c6ed4 | 1475 | if(g_landingcommand=='Y'){ |
hitonari | 0:2b57931c6ed4 | 1476 | targetyaw = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); |
hitonari | 0:2b57931c6ed4 | 1477 | if(targetyaw>=0 && targetyaw<=360){ |
hitonari | 0:2b57931c6ed4 | 1478 | g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); |
hitonari | 0:2b57931c6ed4 | 1479 | } |
hitonari | 0:2b57931c6ed4 | 1480 | } |
hitonari | 0:2b57931c6ed4 | 1481 | //if(g_landingcommand=='Y')g_SerialTargetYAW =0.0f; |
hitonari | 0:2b57931c6ed4 | 1482 | bufcounter = 0; |
hitonari | 0:2b57931c6ed4 | 1483 | memset(SFbuf, 0, sizeof(SFbuf)); |
hitonari | 0:2b57931c6ed4 | 1484 | NVIC_ClearPendingIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1485 | //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); |
hitonari | 0:2b57931c6ed4 | 1486 | } |
hitonari | 0:2b57931c6ed4 | 1487 | |
hitonari | 0:2b57931c6ed4 | 1488 | else if(bufcounter>=5) { |
hitonari | 0:2b57931c6ed4 | 1489 | //pc.printf("Communication Falsed.\r\n"); |
hitonari | 0:2b57931c6ed4 | 1490 | memset(SFbuf, 0, sizeof(SFbuf)); |
hitonari | 0:2b57931c6ed4 | 1491 | bufcounter = 0; |
hitonari | 0:2b57931c6ed4 | 1492 | NVIC_ClearPendingIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1493 | } |
hitonari | 0:2b57931c6ed4 | 1494 | } |
hitonari | 0:2b57931c6ed4 | 1495 | |
hitonari | 0:2b57931c6ed4 | 1496 | //NVIC_EnableIRQ(TIM5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1497 | //NVIC_EnableIRQ(EXTI0_IRQn); |
hitonari | 0:2b57931c6ed4 | 1498 | //NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 1499 | } |
hitonari | 0:2b57931c6ed4 | 1500 | |
hitonari | 0:2b57931c6ed4 | 1501 | float ConvertByteintoFloat(char high, char low) |
hitonari | 0:2b57931c6ed4 | 1502 | { |
hitonari | 0:2b57931c6ed4 | 1503 | |
hitonari | 0:2b57931c6ed4 | 1504 | //int16_t intvalue = (int16_t)high*256 + (int16_t)low; |
hitonari | 0:2b57931c6ed4 | 1505 | int16_t intvalue = (int16_t)(((int16_t)high << 8) | low); // Turn the MSB and LSB into a signed 16-bit value |
hitonari | 0:2b57931c6ed4 | 1506 | float floatvalue = (float)intvalue-256; |
hitonari | 0:2b57931c6ed4 | 1507 | return floatvalue; |
hitonari | 0:2b57931c6ed4 | 1508 | } |
hitonari | 0:2b57931c6ed4 | 1509 | |
hitonari | 0:2b57931c6ed4 | 1510 | |
hitonari | 0:2b57931c6ed4 | 1511 | //超音波割り込み |
hitonari | 0:2b57931c6ed4 | 1512 | |
hitonari | 0:2b57931c6ed4 | 1513 | void UpdateDist() |
hitonari | 0:2b57931c6ed4 | 1514 | { |
hitonari | 0:2b57931c6ed4 | 1515 | g_distance = usensor.get_dist_cm(); |
hitonari | 0:2b57931c6ed4 | 1516 | usensor.start(); |
hitonari | 0:2b57931c6ed4 | 1517 | } |
hitonari | 0:2b57931c6ed4 | 1518 | |
hitonari | 0:2b57931c6ed4 | 1519 | //8の字旋回 |
hitonari | 0:2b57931c6ed4 | 1520 | void UpdateTargetAngle_Moebius(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1521 | { |
hitonari | 0:2b57931c6ed4 | 1522 | static uint8_t RotateCounter=0; |
hitonari | 0:2b57931c6ed4 | 1523 | static bool flg_setInStartAuto = false; |
hitonari | 0:2b57931c6ed4 | 1524 | static float FirstYAW_Moebius = 0.0; |
hitonari | 0:2b57931c6ed4 | 1525 | float newYaw_Moebius; |
hitonari | 0:2b57931c6ed4 | 1526 | |
hitonari | 0:2b57931c6ed4 | 1527 | if(!flg_setInStartAuto && CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1528 | FirstYAW_Moebius = nowAngle[YAW]; |
hitonari | 0:2b57931c6ed4 | 1529 | RotateCounter = 0; |
hitonari | 0:2b57931c6ed4 | 1530 | flg_setInStartAuto = true; |
hitonari | 0:2b57931c6ed4 | 1531 | } else if(!CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1532 | flg_setInStartAuto = false; |
hitonari | 0:2b57931c6ed4 | 1533 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 1534 | } |
hitonari | 0:2b57931c6ed4 | 1535 | autopwm[THR]=oldTHR; |
hitonari | 0:2b57931c6ed4 | 1536 | |
hitonari | 0:2b57931c6ed4 | 1537 | newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); |
hitonari | 0:2b57931c6ed4 | 1538 | |
hitonari | 0:2b57931c6ed4 | 1539 | if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) { |
hitonari | 0:2b57931c6ed4 | 1540 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 1541 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 1542 | pc.printf("Rotate 90\r\n"); |
hitonari | 0:2b57931c6ed4 | 1543 | } |
hitonari | 0:2b57931c6ed4 | 1544 | if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) { |
hitonari | 0:2b57931c6ed4 | 1545 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 1546 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 1547 | pc.printf("Rotate 180\r\n"); |
hitonari | 0:2b57931c6ed4 | 1548 | } |
hitonari | 0:2b57931c6ed4 | 1549 | if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-20.0) { |
hitonari | 0:2b57931c6ed4 | 1550 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 1551 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 1552 | pc.printf("Rotate 270\r\n"); |
hitonari | 0:2b57931c6ed4 | 1553 | } |
hitonari | 0:2b57931c6ed4 | 1554 | if(RotateCounter == 3 && newYaw_Moebius >-10.0 && newYaw_Moebius < 90.0) { |
hitonari | 0:2b57931c6ed4 | 1555 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 1556 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 1557 | pc.printf("Change Rotate direction\r\n"); |
hitonari | 0:2b57931c6ed4 | 1558 | } |
hitonari | 0:2b57931c6ed4 | 1559 | |
hitonari | 0:2b57931c6ed4 | 1560 | |
hitonari | 0:2b57931c6ed4 | 1561 | if(RotateCounter <= 3) UpdateTargetAngle_Rightloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1562 | else UpdateTargetAngle_Leftloop(targetAngle); //左旋回 |
hitonari | 0:2b57931c6ed4 | 1563 | |
hitonari | 0:2b57931c6ed4 | 1564 | } |
hitonari | 0:2b57931c6ed4 | 1565 | |
hitonari | 0:2b57931c6ed4 | 1566 | |
hitonari | 0:2b57931c6ed4 | 1567 | |
hitonari | 0:2b57931c6ed4 | 1568 | //離陸-投下-着陸一連 |
hitonari | 0:2b57931c6ed4 | 1569 | void Take_off_and_landing(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1570 | { |
hitonari | 0:2b57931c6ed4 | 1571 | |
hitonari | 0:2b57931c6ed4 | 1572 | if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; |
hitonari | 0:2b57931c6ed4 | 1573 | |
hitonari | 0:2b57931c6ed4 | 1574 | switch(bombing_mode) { |
hitonari | 0:2b57931c6ed4 | 1575 | case Takeoff: |
hitonari | 0:2b57931c6ed4 | 1576 | static bool flg_setFirstYaw = false; |
hitonari | 0:2b57931c6ed4 | 1577 | static int TakeoffCount = 0; |
hitonari | 0:2b57931c6ed4 | 1578 | |
hitonari | 0:2b57931c6ed4 | 1579 | if(!flg_setFirstYaw && CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1580 | FirstYAW = nowAngle[YAW]; |
hitonari | 0:2b57931c6ed4 | 1581 | flg_setFirstYaw = true; |
hitonari | 0:2b57931c6ed4 | 1582 | } else if(flg_setFirstYaw && !CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1583 | flg_setFirstYaw = false; |
hitonari | 0:2b57931c6ed4 | 1584 | } |
hitonari | 0:2b57931c6ed4 | 1585 | |
hitonari | 0:2b57931c6ed4 | 1586 | UpdateTargetAngle_Takeoff(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1587 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1588 | |
hitonari | 0:2b57931c6ed4 | 1589 | |
hitonari | 0:2b57931c6ed4 | 1590 | //if(g_distance>120) TakeoffCount++; |
hitonari | 0:2b57931c6ed4 | 1591 | // else TakeoffCount = 0; |
hitonari | 0:2b57931c6ed4 | 1592 | // if(TakeoffCount>3) Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1593 | |
hitonari | 0:2b57931c6ed4 | 1594 | //if(g_distance>180)TakeoffCount++; |
hitonari | 0:2b57931c6ed4 | 1595 | //else TakeoffCount = 0; |
hitonari | 0:2b57931c6ed4 | 1596 | |
hitonari | 0:2b57931c6ed4 | 1597 | //if(TakeoffCount>2)Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1598 | |
hitonari | 0:2b57931c6ed4 | 1599 | |
hitonari | 0:2b57931c6ed4 | 1600 | //if(g_distance>210)Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1601 | |
hitonari | 0:2b57931c6ed4 | 1602 | |
hitonari | 0:2b57931c6ed4 | 1603 | |
hitonari | 0:2b57931c6ed4 | 1604 | if(g_distance>120) TakeoffCount++; |
hitonari | 0:2b57931c6ed4 | 1605 | else TakeoffCount = 0; |
hitonari | 0:2b57931c6ed4 | 1606 | //Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1607 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1608 | if(g_landingcommand == 'B'){ |
hitonari | 0:2b57931c6ed4 | 1609 | Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1610 | } |
hitonari | 0:2b57931c6ed4 | 1611 | if(TakeoffCount>5) { |
hitonari | 0:2b57931c6ed4 | 1612 | |
hitonari | 0:2b57931c6ed4 | 1613 | //autopwm[THR] = SetTHRinRatio(0.3); |
hitonari | 0:2b57931c6ed4 | 1614 | autopwm[THR] = 1700;//g_take_off_THROTTLE;//1180+320*2*0.3; |
hitonari | 0:2b57931c6ed4 | 1615 | targetAngle[PITCH]=g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 1616 | autopwm[RUD]=1370;//trimpwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 1617 | targetAngle[ROLL]=g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 1618 | Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1619 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 1620 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 1621 | } |
hitonari | 0:2b57931c6ed4 | 1622 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 1623 | |
hitonari | 0:2b57931c6ed4 | 1624 | //pc.printf("Now go to Approach mode!!"); |
hitonari | 0:2b57931c6ed4 | 1625 | bombing_mode = Approach; |
hitonari | 0:2b57931c6ed4 | 1626 | g_landingcommand = 'T'; |
hitonari | 0:2b57931c6ed4 | 1627 | } |
hitonari | 0:2b57931c6ed4 | 1628 | |
hitonari | 0:2b57931c6ed4 | 1629 | |
hitonari | 0:2b57931c6ed4 | 1630 | |
hitonari | 0:2b57931c6ed4 | 1631 | break; |
hitonari | 0:2b57931c6ed4 | 1632 | |
hitonari | 0:2b57931c6ed4 | 1633 | //case Chicken: |
hitonari | 0:2b57931c6ed4 | 1634 | //break; |
hitonari | 0:2b57931c6ed4 | 1635 | /* |
hitonari | 0:2b57931c6ed4 | 1636 | case Transition: |
hitonari | 0:2b57931c6ed4 | 1637 | static int ApproachCount = 0; |
hitonari | 0:2b57931c6ed4 | 1638 | targetAngle[YAW]=180.0; |
hitonari | 0:2b57931c6ed4 | 1639 | int Judge = Rotate(targetAngle, g_SerialTargetYAW); |
hitonari | 0:2b57931c6ed4 | 1640 | |
hitonari | 0:2b57931c6ed4 | 1641 | if(Judge==0) ApproachCount++; |
hitonari | 0:2b57931c6ed4 | 1642 | if(ApproachCount>5) bombing_mode = Approach; |
hitonari | 0:2b57931c6ed4 | 1643 | break; |
hitonari | 0:2b57931c6ed4 | 1644 | */ |
hitonari | 0:2b57931c6ed4 | 1645 | case Approach: |
hitonari | 0:2b57931c6ed4 | 1646 | |
hitonari | 0:2b57931c6ed4 | 1647 | //autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合 |
hitonari | 0:2b57931c6ed4 | 1648 | autopwm[THR] = minpwm[THR];//1180+320*2*0.3; |
hitonari | 0:2b57931c6ed4 | 1649 | if(g_Finflag == true){ |
hitonari | 0:2b57931c6ed4 | 1650 | autopwm[THR] = minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1651 | autopwm[ELE] = minpwm[ELE]; |
hitonari | 0:2b57931c6ed4 | 1652 | autopwm[RUD]=trimpwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 1653 | autopwm[AIL_R] = minpwm[AIL_R]; |
hitonari | 0:2b57931c6ed4 | 1654 | autopwm[AIL_L]= maxpwm[AIL_L]; |
hitonari | 0:2b57931c6ed4 | 1655 | targetAngle[PITCH] = -80; |
hitonari | 0:2b57931c6ed4 | 1656 | targetAngle[ROLL] = -80; |
hitonari | 0:2b57931c6ed4 | 1657 | }else{ |
hitonari | 0:2b57931c6ed4 | 1658 | UpdateTargetAngle_Approach(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1659 | } |
hitonari | 0:2b57931c6ed4 | 1660 | |
hitonari | 0:2b57931c6ed4 | 1661 | break; |
hitonari | 0:2b57931c6ed4 | 1662 | |
hitonari | 0:2b57931c6ed4 | 1663 | default: |
hitonari | 0:2b57931c6ed4 | 1664 | bombing_mode = Takeoff; |
hitonari | 0:2b57931c6ed4 | 1665 | break; |
hitonari | 0:2b57931c6ed4 | 1666 | } |
hitonari | 0:2b57931c6ed4 | 1667 | |
hitonari | 0:2b57931c6ed4 | 1668 | } |
hitonari | 0:2b57931c6ed4 | 1669 | |
hitonari | 0:2b57931c6ed4 | 1670 | //離陸モード |
hitonari | 0:2b57931c6ed4 | 1671 | void UpdateTargetAngle_Takeoff(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1672 | { |
hitonari | 0:2b57931c6ed4 | 1673 | //pc.printf("%d \r\n",g_distance); |
hitonari | 0:2b57931c6ed4 | 1674 | static int tELE_start = 0; |
hitonari | 0:2b57931c6ed4 | 1675 | static bool flg_ELEup = false; |
hitonari | 0:2b57931c6ed4 | 1676 | int t_def = 0; |
hitonari | 0:2b57931c6ed4 | 1677 | |
hitonari | 0:2b57931c6ed4 | 1678 | autopwm[RUD] = 1390;//trimpwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 1679 | targetAngle[ROLL]=g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 1680 | |
hitonari | 0:2b57931c6ed4 | 1681 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 1682 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 1683 | } |
hitonari | 0:2b57931c6ed4 | 1684 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 1685 | |
hitonari | 0:2b57931c6ed4 | 1686 | |
hitonari | 0:2b57931c6ed4 | 1687 | if(!flg_ELEup && CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1688 | tELE_start = t.read_ms(); |
hitonari | 0:2b57931c6ed4 | 1689 | flg_ELEup = true; |
hitonari | 0:2b57931c6ed4 | 1690 | pc.printf("timer start!!!!\r\n"); |
hitonari | 0:2b57931c6ed4 | 1691 | } else if(!CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1692 | tELE_start = 0; |
hitonari | 0:2b57931c6ed4 | 1693 | flg_ELEup = false; |
hitonari | 0:2b57931c6ed4 | 1694 | //pc.printf("lost!!!!\r\n"); |
hitonari | 0:2b57931c6ed4 | 1695 | } |
hitonari | 0:2b57931c6ed4 | 1696 | if(flg_ELEup) { |
hitonari | 0:2b57931c6ed4 | 1697 | t_def = t.read_ms() - tELE_start; |
hitonari | 0:2b57931c6ed4 | 1698 | |
hitonari | 0:2b57931c6ed4 | 1699 | //1.5秒経過すればELE上げ舵へ |
hitonari | 0:2b57931c6ed4 | 1700 | if(t_def>500) targetAngle[PITCH]= g_take_off_PITCH; |
hitonari | 0:2b57931c6ed4 | 1701 | else { |
hitonari | 0:2b57931c6ed4 | 1702 | t_def = 0; |
hitonari | 0:2b57931c6ed4 | 1703 | targetAngle[PITCH]=g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 1704 | } |
hitonari | 0:2b57931c6ed4 | 1705 | } |
hitonari | 0:2b57931c6ed4 | 1706 | targetAngle[ROLL] = g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 1707 | //targetAngle[PITCH] = g_loopTHR; |
hitonari | 0:2b57931c6ed4 | 1708 | autopwm[THR] = g_take_off_THROTTLE;//SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 |
hitonari | 0:2b57931c6ed4 | 1709 | } |
hitonari | 0:2b57931c6ed4 | 1710 | |
hitonari | 0:2b57931c6ed4 | 1711 | |
hitonari | 0:2b57931c6ed4 | 1712 | //ヨーを目標値にして許容角度になるまで水平旋回 |
hitonari | 0:2b57931c6ed4 | 1713 | int Rotate(float targetAngle[3], float TargetYAW) |
hitonari | 0:2b57931c6ed4 | 1714 | { |
hitonari | 0:2b57931c6ed4 | 1715 | float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); |
hitonari | 0:2b57931c6ed4 | 1716 | |
hitonari | 0:2b57931c6ed4 | 1717 | if(diffYaw > LIMIT_STRAIGHT_YAW) { |
hitonari | 0:2b57931c6ed4 | 1718 | |
hitonari | 0:2b57931c6ed4 | 1719 | if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1720 | else UpdateTargetAngle_Rightloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1721 | |
hitonari | 0:2b57931c6ed4 | 1722 | //UpdateTargetAngle_Rightloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1723 | return 1; |
hitonari | 0:2b57931c6ed4 | 1724 | } else if(diffYaw < -LIMIT_STRAIGHT_YAW) { |
hitonari | 0:2b57931c6ed4 | 1725 | |
hitonari | 0:2b57931c6ed4 | 1726 | if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1727 | else UpdateTargetAngle_Leftloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1728 | |
hitonari | 0:2b57931c6ed4 | 1729 | //UpdateTargetAngle_Leftloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1730 | |
hitonari | 0:2b57931c6ed4 | 1731 | return 1; |
hitonari | 0:2b57931c6ed4 | 1732 | } else { |
hitonari | 0:2b57931c6ed4 | 1733 | UpdateTargetAngle_GoStraight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1734 | return 0; |
hitonari | 0:2b57931c6ed4 | 1735 | } |
hitonari | 0:2b57931c6ed4 | 1736 | } |
hitonari | 0:2b57931c6ed4 | 1737 | |
hitonari | 0:2b57931c6ed4 | 1738 | int Rotate_zero(float targetAngle[3], float TargetYAW){ |
hitonari | 0:2b57931c6ed4 | 1739 | float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); |
hitonari | 0:2b57931c6ed4 | 1740 | |
hitonari | 0:2b57931c6ed4 | 1741 | if(diffYaw > LIMIT_STRAIGHT_YAW) { |
hitonari | 0:2b57931c6ed4 | 1742 | |
hitonari | 0:2b57931c6ed4 | 1743 | /*if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1744 | else UpdateTargetAngle_Rightloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1745 | */ |
hitonari | 0:2b57931c6ed4 | 1746 | UpdateTargetAngle_Rightloop_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1747 | return 1; |
hitonari | 0:2b57931c6ed4 | 1748 | } else if(diffYaw < -LIMIT_STRAIGHT_YAW) { |
hitonari | 0:2b57931c6ed4 | 1749 | |
hitonari | 0:2b57931c6ed4 | 1750 | /*if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1751 | else UpdateTargetAngle_Leftloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1752 | */ |
hitonari | 0:2b57931c6ed4 | 1753 | UpdateTargetAngle_Leftloop_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1754 | |
hitonari | 0:2b57931c6ed4 | 1755 | return 1; |
hitonari | 0:2b57931c6ed4 | 1756 | } else { |
hitonari | 0:2b57931c6ed4 | 1757 | UpdateTargetAngle_GoStraight_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1758 | return 0; |
hitonari | 0:2b57931c6ed4 | 1759 | } |
hitonari | 0:2b57931c6ed4 | 1760 | } |
hitonari | 0:2b57931c6ed4 | 1761 | |
hitonari | 0:2b57931c6ed4 | 1762 | //チキラー投下 |
hitonari | 0:2b57931c6ed4 | 1763 | void Chicken_Drop() |
hitonari | 0:2b57931c6ed4 | 1764 | { |
hitonari | 0:2b57931c6ed4 | 1765 | //if(CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1766 | autopwm[DROP] = maxpwm[DROP]; |
hitonari | 0:2b57931c6ed4 | 1767 | //pc.printf("Bombed!\r\n"); |
hitonari | 0:2b57931c6ed4 | 1768 | //RerurnChickenServo1.attach(&ReturnChickenServo1, 3); |
hitonari | 0:2b57931c6ed4 | 1769 | //operation_mode = Approach; |
hitonari | 0:2b57931c6ed4 | 1770 | //buzzer = 0; |
hitonari | 0:2b57931c6ed4 | 1771 | //pc.printf("Goto LeftLoop mode\r\n"); |
hitonari | 0:2b57931c6ed4 | 1772 | //} |
hitonari | 0:2b57931c6ed4 | 1773 | } |
hitonari | 0:2b57931c6ed4 | 1774 | |
hitonari | 0:2b57931c6ed4 | 1775 | void ReturnChickenServo1() |
hitonari | 0:2b57931c6ed4 | 1776 | { |
hitonari | 0:2b57931c6ed4 | 1777 | autopwm[DROP] = 1344; |
hitonari | 0:2b57931c6ed4 | 1778 | pc.printf("first reverse\r\n"); |
hitonari | 0:2b57931c6ed4 | 1779 | RerurnChickenServo2.attach(&ReturnChickenServo2, 1); |
hitonari | 0:2b57931c6ed4 | 1780 | } |
hitonari | 0:2b57931c6ed4 | 1781 | |
hitonari | 0:2b57931c6ed4 | 1782 | void ReturnChickenServo2() |
hitonari | 0:2b57931c6ed4 | 1783 | { |
hitonari | 0:2b57931c6ed4 | 1784 | autopwm[DROP] = 1392; |
hitonari | 0:2b57931c6ed4 | 1785 | pc.printf("second reverse\r\n"); |
hitonari | 0:2b57931c6ed4 | 1786 | } |
hitonari | 0:2b57931c6ed4 | 1787 | |
hitonari | 0:2b57931c6ed4 | 1788 | //着陸モード(PCからの指令に従う) |
hitonari | 0:2b57931c6ed4 | 1789 | void UpdateTargetAngle_Approach(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1790 | { |
hitonari | 0:2b57931c6ed4 | 1791 | |
hitonari | 0:2b57931c6ed4 | 1792 | static bool Zeroflag=false;//着陸時のスロットル動作確認用 |
hitonari | 0:2b57931c6ed4 | 1793 | |
hitonari | 0:2b57931c6ed4 | 1794 | NVIC_DisableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1795 | |
hitonari | 0:2b57931c6ed4 | 1796 | if(CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 1797 | output_status = Auto; |
hitonari | 0:2b57931c6ed4 | 1798 | led1 = 1; |
hitonari | 0:2b57931c6ed4 | 1799 | } else { |
hitonari | 0:2b57931c6ed4 | 1800 | output_status = Manual; |
hitonari | 0:2b57931c6ed4 | 1801 | led1 = 0; |
hitonari | 0:2b57931c6ed4 | 1802 | Zeroflag=true; |
hitonari | 0:2b57931c6ed4 | 1803 | //g_landingcommand='G'; |
hitonari | 0:2b57931c6ed4 | 1804 | } |
hitonari | 0:2b57931c6ed4 | 1805 | |
hitonari | 0:2b57931c6ed4 | 1806 | //pc.printf("Zeroflag = %d\r\n",Zeroflag); |
hitonari | 0:2b57931c6ed4 | 1807 | |
hitonari | 0:2b57931c6ed4 | 1808 | switch(g_landingcommand) { |
hitonari | 0:2b57931c6ed4 | 1809 | case 'R': //右旋回セヨ |
hitonari | 0:2b57931c6ed4 | 1810 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1811 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1812 | UpdateTargetAngle_Rightloop_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1813 | } |
hitonari | 0:2b57931c6ed4 | 1814 | else{ |
hitonari | 0:2b57931c6ed4 | 1815 | UpdateTargetAngle_Rightloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1816 | /* |
hitonari | 0:2b57931c6ed4 | 1817 | targetAngle[ROLL] = g_rightloopROLL_approach; |
hitonari | 0:2b57931c6ed4 | 1818 | targetAngle[PITCH] = g_rightloopPITCH_approach ; |
hitonari | 0:2b57931c6ed4 | 1819 | autopwm[RUD]=g_rightloopRUD_approach; //RUD固定 |
hitonari | 0:2b57931c6ed4 | 1820 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 1821 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; |
hitonari | 0:2b57931c6ed4 | 1822 | } |
hitonari | 0:2b57931c6ed4 | 1823 | else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 1824 | autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; |
hitonari | 0:2b57931c6ed4 | 1825 | */ |
hitonari | 0:2b57931c6ed4 | 1826 | } |
hitonari | 0:2b57931c6ed4 | 1827 | |
hitonari | 0:2b57931c6ed4 | 1828 | break; |
hitonari | 0:2b57931c6ed4 | 1829 | |
hitonari | 0:2b57931c6ed4 | 1830 | case 'L': //左旋回セヨ |
hitonari | 0:2b57931c6ed4 | 1831 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1832 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1833 | UpdateTargetAngle_Leftloop_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1834 | }else{ |
hitonari | 0:2b57931c6ed4 | 1835 | UpdateTargetAngle_Leftloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1836 | /*targetAngle[ROLL] = g_leftloopROLL_approach; |
hitonari | 0:2b57931c6ed4 | 1837 | targetAngle[PITCH] = g_leftloopPITCH_approach; |
hitonari | 0:2b57931c6ed4 | 1838 | autopwm[RUD]=g_leftloopRUD_approach; |
hitonari | 0:2b57931c6ed4 | 1839 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ |
hitonari | 0:2b57931c6ed4 | 1840 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; |
hitonari | 0:2b57931c6ed4 | 1841 | } |
hitonari | 0:2b57931c6ed4 | 1842 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 1843 | */ |
hitonari | 0:2b57931c6ed4 | 1844 | } |
hitonari | 0:2b57931c6ed4 | 1845 | |
hitonari | 0:2b57931c6ed4 | 1846 | break; |
hitonari | 0:2b57931c6ed4 | 1847 | |
hitonari | 0:2b57931c6ed4 | 1848 | case 'G': //直進セヨ |
hitonari | 0:2b57931c6ed4 | 1849 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1850 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1851 | UpdateTargetAngle_GoStraight_zero(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1852 | } |
hitonari | 0:2b57931c6ed4 | 1853 | else{ |
hitonari | 0:2b57931c6ed4 | 1854 | UpdateTargetAngle_GoStraight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1855 | /* |
hitonari | 0:2b57931c6ed4 | 1856 | targetAngle[ROLL] = g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 1857 | targetAngle[PITCH] = g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 1858 | */ |
hitonari | 0:2b57931c6ed4 | 1859 | } |
hitonari | 0:2b57931c6ed4 | 1860 | |
hitonari | 0:2b57931c6ed4 | 1861 | break; |
hitonari | 0:2b57931c6ed4 | 1862 | |
hitonari | 0:2b57931c6ed4 | 1863 | case 'Y': //指定ノヨー方向ニ移動セヨ |
hitonari | 0:2b57931c6ed4 | 1864 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1865 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1866 | Rotate_zero(targetAngle, g_SerialTargetYAW); |
hitonari | 0:2b57931c6ed4 | 1867 | //autopwm[THR]=minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1868 | }else{ |
hitonari | 0:2b57931c6ed4 | 1869 | Rotate(targetAngle, g_SerialTargetYAW); |
hitonari | 0:2b57931c6ed4 | 1870 | //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05); |
hitonari | 0:2b57931c6ed4 | 1871 | } |
hitonari | 0:2b57931c6ed4 | 1872 | break; |
hitonari | 0:2b57931c6ed4 | 1873 | |
hitonari | 0:2b57931c6ed4 | 1874 | case 'T': //0度ノヨー方向ニ移動セヨ |
hitonari | 0:2b57931c6ed4 | 1875 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1876 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1877 | Rotate_zero(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1878 | //autopwm[THR]=minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1879 | }else{ |
hitonari | 0:2b57931c6ed4 | 1880 | Rotate(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1881 | //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05); |
hitonari | 0:2b57931c6ed4 | 1882 | } |
hitonari | 0:2b57931c6ed4 | 1883 | break; |
hitonari | 0:2b57931c6ed4 | 1884 | |
hitonari | 0:2b57931c6ed4 | 1885 | case 'U': //機首ヲ上ゲヨ |
hitonari | 0:2b57931c6ed4 | 1886 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1887 | static int UpCounter=0; |
hitonari | 0:2b57931c6ed4 | 1888 | UpCounter++; |
hitonari | 0:2b57931c6ed4 | 1889 | if(UpCounter==3) { |
hitonari | 0:2b57931c6ed4 | 1890 | g_Finflag = true; |
hitonari | 0:2b57931c6ed4 | 1891 | } |
hitonari | 0:2b57931c6ed4 | 1892 | |
hitonari | 0:2b57931c6ed4 | 1893 | break; |
hitonari | 0:2b57931c6ed4 | 1894 | |
hitonari | 0:2b57931c6ed4 | 1895 | case 'Z': //ヤレバワカル |
hitonari | 0:2b57931c6ed4 | 1896 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1897 | autopwm[THR] = minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1898 | autopwm[ELE] = minpwm[ELE]; |
hitonari | 0:2b57931c6ed4 | 1899 | autopwm[RUD]=trimpwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 1900 | autopwm[AIL_R] = minpwm[AIL_R]; |
hitonari | 0:2b57931c6ed4 | 1901 | autopwm[AIL_L]= maxpwm[AIL_L]; |
hitonari | 0:2b57931c6ed4 | 1902 | targetAngle[ROLL] = -80; |
hitonari | 0:2b57931c6ed4 | 1903 | |
hitonari | 0:2b57931c6ed4 | 1904 | break; |
hitonari | 0:2b57931c6ed4 | 1905 | |
hitonari | 0:2b57931c6ed4 | 1906 | /*case 'B': //ブザーヲ鳴ラセ |
hitonari | 0:2b57931c6ed4 | 1907 | //buzzer = 1; |
hitonari | 0:2b57931c6ed4 | 1908 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1909 | break;*/ |
hitonari | 0:2b57931c6ed4 | 1910 | |
hitonari | 0:2b57931c6ed4 | 1911 | case 'B': //物資ヲ落トセ |
hitonari | 0:2b57931c6ed4 | 1912 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1913 | Chicken_Drop(); |
hitonari | 0:2b57931c6ed4 | 1914 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1915 | Rotate_zero(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1916 | //autopwm[THR]=minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1917 | }else{ |
hitonari | 0:2b57931c6ed4 | 1918 | Rotate(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1919 | //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05); |
hitonari | 0:2b57931c6ed4 | 1920 | } |
hitonari | 0:2b57931c6ed4 | 1921 | |
hitonari | 0:2b57931c6ed4 | 1922 | break; |
hitonari | 0:2b57931c6ed4 | 1923 | |
hitonari | 0:2b57931c6ed4 | 1924 | case 'C': //停止セヨ |
hitonari | 0:2b57931c6ed4 | 1925 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1926 | /*targetAngle[ROLL] = g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 1927 | targetAngle[PITCH] = g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 1928 | autopwm[THR] = minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1929 | */ |
hitonari | 0:2b57931c6ed4 | 1930 | Zeroflag = true; |
hitonari | 0:2b57931c6ed4 | 1931 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1932 | Rotate_zero(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1933 | //autopwm[THR]=minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1934 | }else{ |
hitonari | 0:2b57931c6ed4 | 1935 | Rotate(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1936 | //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05); |
hitonari | 0:2b57931c6ed4 | 1937 | } |
hitonari | 0:2b57931c6ed4 | 1938 | |
hitonari | 0:2b57931c6ed4 | 1939 | break; |
hitonari | 0:2b57931c6ed4 | 1940 | |
hitonari | 0:2b57931c6ed4 | 1941 | case 'X': //再起動セヨ |
hitonari | 0:2b57931c6ed4 | 1942 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1943 | /* |
hitonari | 0:2b57931c6ed4 | 1944 | targetAngle[ROLL] = g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 1945 | targetAngle[PITCH] = g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 1946 | autopwm[THR] = minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1947 | */ |
hitonari | 0:2b57931c6ed4 | 1948 | Zeroflag = false; |
hitonari | 0:2b57931c6ed4 | 1949 | if(Zeroflag==true){ |
hitonari | 0:2b57931c6ed4 | 1950 | Rotate_zero(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1951 | //autopwm[THR]=minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 1952 | }else{ |
hitonari | 0:2b57931c6ed4 | 1953 | Rotate(targetAngle, 0); |
hitonari | 0:2b57931c6ed4 | 1954 | //autopwm[THR]=SetTHRinRatio(g_loopTHR+0.05); |
hitonari | 0:2b57931c6ed4 | 1955 | } |
hitonari | 0:2b57931c6ed4 | 1956 | |
hitonari | 0:2b57931c6ed4 | 1957 | break; |
hitonari | 0:2b57931c6ed4 | 1958 | |
hitonari | 0:2b57931c6ed4 | 1959 | default : |
hitonari | 0:2b57931c6ed4 | 1960 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 1961 | break; |
hitonari | 0:2b57931c6ed4 | 1962 | |
hitonari | 0:2b57931c6ed4 | 1963 | |
hitonari | 0:2b57931c6ed4 | 1964 | } |
hitonari | 0:2b57931c6ed4 | 1965 | |
hitonari | 0:2b57931c6ed4 | 1966 | } |
hitonari | 0:2b57931c6ed4 | 1967 | |
hitonari | 0:2b57931c6ed4 | 1968 | void checkHeight(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1969 | { |
hitonari | 0:2b57931c6ed4 | 1970 | |
hitonari | 0:2b57931c6ed4 | 1971 | static int targetHeight = 200; |
hitonari | 0:2b57931c6ed4 | 1972 | |
hitonari | 0:2b57931c6ed4 | 1973 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1974 | if(g_distance < targetHeight + ALLOWHEIGHT) { |
hitonari | 0:2b57931c6ed4 | 1975 | UpdateTargetAngle_NoseUP(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1976 | if(CheckSW_Up(Ch7)) led2 = 1; |
hitonari | 0:2b57931c6ed4 | 1977 | } else if(g_distance > targetHeight - ALLOWHEIGHT) { |
hitonari | 0:2b57931c6ed4 | 1978 | UpdateTargetAngle_NoseDOWN(targetAngle); |
hitonari | 0:2b57931c6ed4 | 1979 | if(CheckSW_Up(Ch7)) led2 = 1; |
hitonari | 0:2b57931c6ed4 | 1980 | } else led2=0; |
hitonari | 0:2b57931c6ed4 | 1981 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 1982 | } |
hitonari | 0:2b57931c6ed4 | 1983 | |
hitonari | 0:2b57931c6ed4 | 1984 | void UpdateTargetAngle_NoseUP(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1985 | { |
hitonari | 0:2b57931c6ed4 | 1986 | |
hitonari | 0:2b57931c6ed4 | 1987 | //targetAngle[PITCH] += 2.0f; |
hitonari | 0:2b57931c6ed4 | 1988 | //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6); |
hitonari | 0:2b57931c6ed4 | 1989 | autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05); |
hitonari | 0:2b57931c6ed4 | 1990 | //pc.printf("nose UP"); |
hitonari | 0:2b57931c6ed4 | 1991 | } |
hitonari | 0:2b57931c6ed4 | 1992 | |
hitonari | 0:2b57931c6ed4 | 1993 | void UpdateTargetAngle_NoseDOWN(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 1994 | { |
hitonari | 0:2b57931c6ed4 | 1995 | |
hitonari | 0:2b57931c6ed4 | 1996 | //targetAngle[PITCH] -= 2.0f; |
hitonari | 0:2b57931c6ed4 | 1997 | autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05); |
hitonari | 0:2b57931c6ed4 | 1998 | //pc.printf("nose DOWN"); |
hitonari | 0:2b57931c6ed4 | 1999 | } |
hitonari | 0:2b57931c6ed4 | 2000 | |
hitonari | 0:2b57931c6ed4 | 2001 | //直進 |
hitonari | 0:2b57931c6ed4 | 2002 | void UpdateTargetAngle_GoStraight(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 2003 | { |
hitonari | 0:2b57931c6ed4 | 2004 | |
hitonari | 0:2b57931c6ed4 | 2005 | autopwm[RUD] = trimpwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 2006 | targetAngle[ROLL] = g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 2007 | targetAngle[PITCH] = g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 2008 | autopwm[THR] = SetTHRinRatio(g_loopTHR); |
hitonari | 0:2b57931c6ed4 | 2009 | |
hitonari | 0:2b57931c6ed4 | 2010 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 2011 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2012 | } |
hitonari | 0:2b57931c6ed4 | 2013 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 2014 | |
hitonari | 0:2b57931c6ed4 | 2015 | autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; |
hitonari | 0:2b57931c6ed4 | 2016 | |
hitonari | 0:2b57931c6ed4 | 2017 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2018 | } |
hitonari | 0:2b57931c6ed4 | 2019 | |
hitonari | 0:2b57931c6ed4 | 2020 | //直進(着陸時スロットル0のとき) |
hitonari | 0:2b57931c6ed4 | 2021 | void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 2022 | { |
hitonari | 0:2b57931c6ed4 | 2023 | |
hitonari | 0:2b57931c6ed4 | 2024 | autopwm[RUD] = trimpwm[RUD]; |
hitonari | 0:2b57931c6ed4 | 2025 | targetAngle[ROLL] = g_gostraightROLL; |
hitonari | 0:2b57931c6ed4 | 2026 | targetAngle[PITCH] = -7.0;//g_gostraightPITCH; |
hitonari | 0:2b57931c6ed4 | 2027 | autopwm[THR] = minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 2028 | |
hitonari | 0:2b57931c6ed4 | 2029 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 2030 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2031 | } |
hitonari | 0:2b57931c6ed4 | 2032 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 2033 | |
hitonari | 0:2b57931c6ed4 | 2034 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2035 | } |
hitonari | 0:2b57931c6ed4 | 2036 | |
hitonari | 0:2b57931c6ed4 | 2037 | //右旋回 |
hitonari | 0:2b57931c6ed4 | 2038 | void UpdateTargetAngle_Rightloop(float targetAngle[3]) //右旋回 |
hitonari | 0:2b57931c6ed4 | 2039 | { |
hitonari | 0:2b57931c6ed4 | 2040 | targetAngle[ROLL] = g_rightloopROLL; |
hitonari | 0:2b57931c6ed4 | 2041 | targetAngle[PITCH] = g_rightloopPITCH; |
hitonari | 0:2b57931c6ed4 | 2042 | autopwm[RUD]=g_rightloopRUD; //RUD固定 |
hitonari | 0:2b57931c6ed4 | 2043 | autopwm[THR] = SetTHRinRatio(g_rightloop_THR_rate); //手動スロットル記憶 |
hitonari | 0:2b57931c6ed4 | 2044 | |
hitonari | 0:2b57931c6ed4 | 2045 | /* |
hitonari | 0:2b57931c6ed4 | 2046 | if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){ |
hitonari | 0:2b57931c6ed4 | 2047 | t2.start(); |
hitonari | 0:2b57931c6ed4 | 2048 | pc.printf("Timer start."); |
hitonari | 0:2b57931c6ed4 | 2049 | } |
hitonari | 0:2b57931c6ed4 | 2050 | if(0.0<t2.read()<5.0){ |
hitonari | 0:2b57931c6ed4 | 2051 | //pc.printf("tagetAngle is changed."); |
hitonari | 0:2b57931c6ed4 | 2052 | targetAngle[ROLL] = rightloopROLL2; |
hitonari | 0:2b57931c6ed4 | 2053 | } |
hitonari | 0:2b57931c6ed4 | 2054 | else { |
hitonari | 0:2b57931c6ed4 | 2055 | t2.stop(); |
hitonari | 0:2b57931c6ed4 | 2056 | t2.reset(); |
hitonari | 0:2b57931c6ed4 | 2057 | pc.printf("Timer stopped."); |
hitonari | 0:2b57931c6ed4 | 2058 | targetAngle[ROLL] = g_rightloopROLL; |
hitonari | 0:2b57931c6ed4 | 2059 | } |
hitonari | 0:2b57931c6ed4 | 2060 | */ |
hitonari | 0:2b57931c6ed4 | 2061 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 2062 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2063 | } |
hitonari | 0:2b57931c6ed4 | 2064 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 2065 | |
hitonari | 0:2b57931c6ed4 | 2066 | autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; |
hitonari | 0:2b57931c6ed4 | 2067 | |
hitonari | 0:2b57931c6ed4 | 2068 | |
hitonari | 0:2b57931c6ed4 | 2069 | //checkHeight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2070 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2071 | } |
hitonari | 0:2b57931c6ed4 | 2072 | |
hitonari | 0:2b57931c6ed4 | 2073 | void UpdateTargetAngle_Rightloop_UP(float targetAngle[3]) //右上昇旋回 |
hitonari | 0:2b57931c6ed4 | 2074 | { |
hitonari | 0:2b57931c6ed4 | 2075 | //autopwm[ELE]=maxpwm[ELE]; |
hitonari | 0:2b57931c6ed4 | 2076 | targetAngle[ROLL] = g_ascending_ROLL; |
hitonari | 0:2b57931c6ed4 | 2077 | targetAngle[PITCH] = g_ascendingPITCH; |
hitonari | 0:2b57931c6ed4 | 2078 | autopwm[RUD]=g_ascending_RUD; //RUD固定 |
hitonari | 0:2b57931c6ed4 | 2079 | autopwm[THR] = SetTHRinRatio(g_ascending_THR_rate); //手動スロットル記憶 |
hitonari | 0:2b57931c6ed4 | 2080 | |
hitonari | 0:2b57931c6ed4 | 2081 | /* |
hitonari | 0:2b57931c6ed4 | 2082 | if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){ |
hitonari | 0:2b57931c6ed4 | 2083 | t2.start(); |
hitonari | 0:2b57931c6ed4 | 2084 | pc.printf("Timer start."); |
hitonari | 0:2b57931c6ed4 | 2085 | } |
hitonari | 0:2b57931c6ed4 | 2086 | if(0.0<t2.read()<5.0){ |
hitonari | 0:2b57931c6ed4 | 2087 | //pc.printf("tagetAngle is changed."); |
hitonari | 0:2b57931c6ed4 | 2088 | targetAngle[ROLL] = rightloopROLL2; |
hitonari | 0:2b57931c6ed4 | 2089 | } |
hitonari | 0:2b57931c6ed4 | 2090 | else { |
hitonari | 0:2b57931c6ed4 | 2091 | t2.stop(); |
hitonari | 0:2b57931c6ed4 | 2092 | t2.reset(); |
hitonari | 0:2b57931c6ed4 | 2093 | pc.printf("Timer stopped."); |
hitonari | 0:2b57931c6ed4 | 2094 | targetAngle[ROLL] = g_rightloopROLL; |
hitonari | 0:2b57931c6ed4 | 2095 | } |
hitonari | 0:2b57931c6ed4 | 2096 | */ |
hitonari | 0:2b57931c6ed4 | 2097 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 2098 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2099 | } |
hitonari | 0:2b57931c6ed4 | 2100 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 2101 | |
hitonari | 0:2b57931c6ed4 | 2102 | autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; |
hitonari | 0:2b57931c6ed4 | 2103 | |
hitonari | 0:2b57931c6ed4 | 2104 | |
hitonari | 0:2b57931c6ed4 | 2105 | //checkHeight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2106 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2107 | } |
hitonari | 0:2b57931c6ed4 | 2108 | |
hitonari | 0:2b57931c6ed4 | 2109 | //右旋回(着陸時スロットル0の時) |
hitonari | 0:2b57931c6ed4 | 2110 | void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]) //右旋回 |
hitonari | 0:2b57931c6ed4 | 2111 | { |
hitonari | 0:2b57931c6ed4 | 2112 | autopwm[THR]=minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 2113 | targetAngle[ROLL] = g_rightloopROLL_approach; |
hitonari | 0:2b57931c6ed4 | 2114 | targetAngle[PITCH] = g_rightloopPITCH_approach ; |
hitonari | 0:2b57931c6ed4 | 2115 | autopwm[RUD]=g_rightloopRUD_approach; //RUD固定 |
hitonari | 0:2b57931c6ed4 | 2116 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ //エルロン上がりやすさ調節 |
hitonari | 0:2b57931c6ed4 | 2117 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2118 | } |
hitonari | 0:2b57931c6ed4 | 2119 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; |
hitonari | 0:2b57931c6ed4 | 2120 | |
hitonari | 0:2b57931c6ed4 | 2121 | autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; |
hitonari | 0:2b57931c6ed4 | 2122 | |
hitonari | 0:2b57931c6ed4 | 2123 | //checkHeight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2124 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2125 | } |
hitonari | 0:2b57931c6ed4 | 2126 | |
hitonari | 0:2b57931c6ed4 | 2127 | void UpdateTargetAngle_Rightloop_short(float targetAngle[3]) //右旋回 |
hitonari | 0:2b57931c6ed4 | 2128 | { |
hitonari | 0:2b57931c6ed4 | 2129 | |
hitonari | 0:2b57931c6ed4 | 2130 | targetAngle[ROLL] = g_rightloopROLLshort; |
hitonari | 0:2b57931c6ed4 | 2131 | targetAngle[PITCH] = g_rightloopPITCHshort; |
hitonari | 0:2b57931c6ed4 | 2132 | autopwm[RUD]=g_rightloopshortRUD; |
hitonari | 0:2b57931c6ed4 | 2133 | autopwm[THR] = SetTHRinRatio(g_rightloop_THR_rate); |
hitonari | 0:2b57931c6ed4 | 2134 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ |
hitonari | 0:2b57931c6ed4 | 2135 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; |
hitonari | 0:2b57931c6ed4 | 2136 | } |
hitonari | 0:2b57931c6ed4 | 2137 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2138 | |
hitonari | 0:2b57931c6ed4 | 2139 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2140 | } |
hitonari | 0:2b57931c6ed4 | 2141 | |
hitonari | 0:2b57931c6ed4 | 2142 | |
hitonari | 0:2b57931c6ed4 | 2143 | //左旋回 |
hitonari | 0:2b57931c6ed4 | 2144 | void UpdateTargetAngle_Leftloop(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 2145 | { |
hitonari | 0:2b57931c6ed4 | 2146 | |
hitonari | 0:2b57931c6ed4 | 2147 | targetAngle[ROLL] = g_leftloopROLL; |
hitonari | 0:2b57931c6ed4 | 2148 | targetAngle[PITCH] = g_leftloopPITCH; |
hitonari | 0:2b57931c6ed4 | 2149 | //autopwm[ELE] = 1700; |
hitonari | 0:2b57931c6ed4 | 2150 | autopwm[RUD]=g_leftloopRUD; |
hitonari | 0:2b57931c6ed4 | 2151 | autopwm[THR] = SetTHRinRatio(g_leftloop_THR_rate); |
hitonari | 0:2b57931c6ed4 | 2152 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ |
hitonari | 0:2b57931c6ed4 | 2153 | autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; |
hitonari | 0:2b57931c6ed4 | 2154 | } |
hitonari | 0:2b57931c6ed4 | 2155 | else autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2156 | |
hitonari | 0:2b57931c6ed4 | 2157 | //checkHeight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2158 | |
hitonari | 0:2b57931c6ed4 | 2159 | //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2160 | } |
hitonari | 0:2b57931c6ed4 | 2161 | |
hitonari | 0:2b57931c6ed4 | 2162 | //左旋回(着陸時スロットル0のとき) |
hitonari | 0:2b57931c6ed4 | 2163 | void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 2164 | { |
hitonari | 0:2b57931c6ed4 | 2165 | |
hitonari | 0:2b57931c6ed4 | 2166 | targetAngle[ROLL] = g_leftloopROLL_approach; |
hitonari | 0:2b57931c6ed4 | 2167 | targetAngle[PITCH] = g_leftloopPITCH_approach; |
hitonari | 0:2b57931c6ed4 | 2168 | autopwm[RUD]=g_leftloopRUD_approach; |
hitonari | 0:2b57931c6ed4 | 2169 | autopwm[THR] = minpwm[THR]; |
hitonari | 0:2b57931c6ed4 | 2170 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ |
hitonari | 0:2b57931c6ed4 | 2171 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; |
hitonari | 0:2b57931c6ed4 | 2172 | } |
hitonari | 0:2b57931c6ed4 | 2173 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2174 | |
hitonari | 0:2b57931c6ed4 | 2175 | //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop; |
hitonari | 0:2b57931c6ed4 | 2176 | //checkHeight(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2177 | |
hitonari | 0:2b57931c6ed4 | 2178 | //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2179 | } |
hitonari | 0:2b57931c6ed4 | 2180 | |
hitonari | 0:2b57931c6ed4 | 2181 | void UpdateTargetAngle_Leftloop_short(float targetAngle[3]) |
hitonari | 0:2b57931c6ed4 | 2182 | { |
hitonari | 0:2b57931c6ed4 | 2183 | |
hitonari | 0:2b57931c6ed4 | 2184 | targetAngle[ROLL] = g_leftloopROLLshort; |
hitonari | 0:2b57931c6ed4 | 2185 | targetAngle[PITCH] = g_leftloopPITCHshort; |
hitonari | 0:2b57931c6ed4 | 2186 | autopwm[RUD]=g_leftloopRUD; |
hitonari | 0:2b57931c6ed4 | 2187 | autopwm[THR] = SetTHRinRatio(g_leftloop_THR_rate); |
hitonari | 0:2b57931c6ed4 | 2188 | if(autopwm[AIL_R]<trimpwm[AIL_R]){ |
hitonari | 0:2b57931c6ed4 | 2189 | autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; |
hitonari | 0:2b57931c6ed4 | 2190 | } |
hitonari | 0:2b57931c6ed4 | 2191 | else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //エルロンサーボ二つ使用時 |
hitonari | 0:2b57931c6ed4 | 2192 | |
hitonari | 0:2b57931c6ed4 | 2193 | //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); |
hitonari | 0:2b57931c6ed4 | 2194 | } |
hitonari | 0:2b57931c6ed4 | 2195 | |
hitonari | 0:2b57931c6ed4 | 2196 | void Ascending(float targetAngle[3]){ |
hitonari | 0:2b57931c6ed4 | 2197 | static uint8_t RotateCounter=0; |
hitonari | 0:2b57931c6ed4 | 2198 | static bool LapCounter = false; |
hitonari | 0:2b57931c6ed4 | 2199 | static bool UpFlag = false; |
hitonari | 0:2b57931c6ed4 | 2200 | static bool PressFlag = false; |
hitonari | 0:2b57931c6ed4 | 2201 | static float FirstHeight = 0; |
hitonari | 0:2b57931c6ed4 | 2202 | static bool flg_setInStartAuto = false; |
hitonari | 0:2b57931c6ed4 | 2203 | //static float Time = 0.0; |
hitonari | 0:2b57931c6ed4 | 2204 | static float FirstYaw_Ascending = 0; |
hitonari | 0:2b57931c6ed4 | 2205 | float newYaw_Ascending; |
hitonari | 0:2b57931c6ed4 | 2206 | |
hitonari | 0:2b57931c6ed4 | 2207 | //pc.printf("FirstHeight = %f g_z_comp = %f g_AscendingFlag = %d\r\n",FirstHeight,g_z_comp,g_AscendingFlag); |
hitonari | 0:2b57931c6ed4 | 2208 | |
hitonari | 0:2b57931c6ed4 | 2209 | getBMP280(); |
hitonari | 0:2b57931c6ed4 | 2210 | |
hitonari | 0:2b57931c6ed4 | 2211 | if(!flg_setInStartAuto && CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 2212 | FirstYaw_Ascending = nowAngle[YAW]; |
hitonari | 0:2b57931c6ed4 | 2213 | RotateCounter = 0; |
hitonari | 0:2b57931c6ed4 | 2214 | flg_setInStartAuto = true; |
hitonari | 0:2b57931c6ed4 | 2215 | LapCounter = false; |
hitonari | 0:2b57931c6ed4 | 2216 | UpFlag = false; |
hitonari | 0:2b57931c6ed4 | 2217 | PressFlag = false; |
hitonari | 0:2b57931c6ed4 | 2218 | //tBMP.reset(); |
hitonari | 0:2b57931c6ed4 | 2219 | } else if(!CheckSW_Up(Ch7)) { |
hitonari | 0:2b57931c6ed4 | 2220 | flg_setInStartAuto = false; |
hitonari | 0:2b57931c6ed4 | 2221 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 2222 | } |
hitonari | 0:2b57931c6ed4 | 2223 | |
hitonari | 0:2b57931c6ed4 | 2224 | newYaw_Ascending = TranslateNewYaw(nowAngle[YAW], FirstYaw_Ascending); |
hitonari | 0:2b57931c6ed4 | 2225 | |
hitonari | 0:2b57931c6ed4 | 2226 | if(RotateCounter == 0 && newYaw_Ascending >90.0 && newYaw_Ascending < 180.0) { |
hitonari | 0:2b57931c6ed4 | 2227 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 2228 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 2229 | pc.printf("Rotate 90\r\n"); |
hitonari | 0:2b57931c6ed4 | 2230 | } |
hitonari | 0:2b57931c6ed4 | 2231 | if(RotateCounter == 1 && newYaw_Ascending >-180.0 && newYaw_Ascending < -90.0) { |
hitonari | 0:2b57931c6ed4 | 2232 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 2233 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 2234 | pc.printf("Rotate 180\r\n"); |
hitonari | 0:2b57931c6ed4 | 2235 | } |
hitonari | 0:2b57931c6ed4 | 2236 | if(RotateCounter == 2 && newYaw_Ascending >-90.0 && newYaw_Ascending <-20.0) { |
hitonari | 0:2b57931c6ed4 | 2237 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 2238 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 2239 | pc.printf("Rotate 270\r\n"); |
hitonari | 0:2b57931c6ed4 | 2240 | } |
hitonari | 0:2b57931c6ed4 | 2241 | if(RotateCounter == 3 && newYaw_Ascending >-10.0 && newYaw_Ascending < 90.0) { |
hitonari | 0:2b57931c6ed4 | 2242 | RotateCounter++; |
hitonari | 0:2b57931c6ed4 | 2243 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 2244 | if(LapCounter == false){ |
hitonari | 0:2b57931c6ed4 | 2245 | RotateCounter = 0; |
hitonari | 0:2b57931c6ed4 | 2246 | LapCounter = true; |
hitonari | 0:2b57931c6ed4 | 2247 | pc.printf("Lap2\r\n"); |
hitonari | 0:2b57931c6ed4 | 2248 | } |
hitonari | 0:2b57931c6ed4 | 2249 | } |
hitonari | 0:2b57931c6ed4 | 2250 | |
hitonari | 0:2b57931c6ed4 | 2251 | if(RotateCounter <= 3){ |
hitonari | 0:2b57931c6ed4 | 2252 | UpdateTargetAngle_Rightloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2253 | }else if(3 < RotateCounter && UpFlag == false){ |
hitonari | 0:2b57931c6ed4 | 2254 | if(PressFlag == false){ |
hitonari | 0:2b57931c6ed4 | 2255 | FirstHeight = g_z_comp; |
hitonari | 0:2b57931c6ed4 | 2256 | //tBMP.start(); |
hitonari | 0:2b57931c6ed4 | 2257 | PressFlag = true; |
hitonari | 0:2b57931c6ed4 | 2258 | led4 = 1; |
hitonari | 0:2b57931c6ed4 | 2259 | } |
hitonari | 0:2b57931c6ed4 | 2260 | //Time = tBMP.read(); |
hitonari | 0:2b57931c6ed4 | 2261 | if(g_z_comp > FirstHeight+3000){ |
hitonari | 0:2b57931c6ed4 | 2262 | UpFlag = true; |
hitonari | 0:2b57931c6ed4 | 2263 | led4 = 0; |
hitonari | 0:2b57931c6ed4 | 2264 | } |
hitonari | 0:2b57931c6ed4 | 2265 | UpdateTargetAngle_Rightloop_UP(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2266 | }else{ |
hitonari | 0:2b57931c6ed4 | 2267 | UpdateTargetAngle_Rightloop(targetAngle); |
hitonari | 0:2b57931c6ed4 | 2268 | } |
hitonari | 0:2b57931c6ed4 | 2269 | |
hitonari | 0:2b57931c6ed4 | 2270 | |
hitonari | 0:2b57931c6ed4 | 2271 | } |
hitonari | 0:2b57931c6ed4 | 2272 | |
hitonari | 0:2b57931c6ed4 | 2273 | void Sbusprintf() |
hitonari | 0:2b57931c6ed4 | 2274 | { |
hitonari | 0:2b57931c6ed4 | 2275 | |
hitonari | 0:2b57931c6ed4 | 2276 | for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]); |
hitonari | 0:2b57931c6ed4 | 2277 | pc.printf("\r\n"); |
hitonari | 0:2b57931c6ed4 | 2278 | |
hitonari | 0:2b57931c6ed4 | 2279 | } |
hitonari | 0:2b57931c6ed4 | 2280 | |
hitonari | 0:2b57931c6ed4 | 2281 | |
hitonari | 0:2b57931c6ed4 | 2282 | //---sensing--- |
hitonari | 0:2b57931c6ed4 | 2283 | //センサーの値を読み込み、各種フィルタをかける(認知) |
hitonari | 0:2b57931c6ed4 | 2284 | void sensing() |
hitonari | 0:2b57931c6ed4 | 2285 | { |
hitonari | 0:2b57931c6ed4 | 2286 | // If intPin goes high, all data registers have new data |
hitonari | 0:2b57931c6ed4 | 2287 | if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt |
hitonari | 0:2b57931c6ed4 | 2288 | |
hitonari | 0:2b57931c6ed4 | 2289 | mpu9250.readAccelData(accelCount); // Read the x/y/z adc values |
hitonari | 0:2b57931c6ed4 | 2290 | // Now we'll calculate the accleration value into actual g's |
hitonari | 0:2b57931c6ed4 | 2291 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
hitonari | 0:2b57931c6ed4 | 2292 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
hitonari | 0:2b57931c6ed4 | 2293 | az = (float)accelCount[2]*aRes - accelBias[2]; |
hitonari | 0:2b57931c6ed4 | 2294 | |
hitonari | 0:2b57931c6ed4 | 2295 | mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values |
hitonari | 0:2b57931c6ed4 | 2296 | // Calculate the gyro value into actual degrees per second |
hitonari | 0:2b57931c6ed4 | 2297 | gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
hitonari | 0:2b57931c6ed4 | 2298 | gy = (float)gyroCount[1]*gRes - gyroBias[1]; |
hitonari | 0:2b57931c6ed4 | 2299 | gz = (float)gyroCount[2]*gRes - gyroBias[2]; |
hitonari | 0:2b57931c6ed4 | 2300 | |
hitonari | 0:2b57931c6ed4 | 2301 | mpu9250.readMagData(magCount); // Read the x/y/z adc values |
hitonari | 0:2b57931c6ed4 | 2302 | // Calculate the magnetometer values in milliGauss |
hitonari | 0:2b57931c6ed4 | 2303 | // Include factory calibration per data sheet and user environmental corrections |
hitonari | 0:2b57931c6ed4 | 2304 | mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set |
hitonari | 0:2b57931c6ed4 | 2305 | my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1]; |
hitonari | 0:2b57931c6ed4 | 2306 | mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2]; |
hitonari | 0:2b57931c6ed4 | 2307 | } |
hitonari | 0:2b57931c6ed4 | 2308 | |
hitonari | 0:2b57931c6ed4 | 2309 | Now = t.read_us(); |
hitonari | 0:2b57931c6ed4 | 2310 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
hitonari | 0:2b57931c6ed4 | 2311 | lastUpdate = Now; |
hitonari | 0:2b57931c6ed4 | 2312 | |
hitonari | 0:2b57931c6ed4 | 2313 | g_sum += deltat; |
hitonari | 0:2b57931c6ed4 | 2314 | g_sumCount++; |
hitonari | 0:2b57931c6ed4 | 2315 | |
hitonari | 0:2b57931c6ed4 | 2316 | // if(lastUpdate - firstUpdate > 10000000.0f) { |
hitonari | 0:2b57931c6ed4 | 2317 | // beta = 0.04; // decrease filter gain after stabilized |
hitonari | 0:2b57931c6ed4 | 2318 | // zeta = 0.015; // increasey bias drift gain after stabilized |
hitonari | 0:2b57931c6ed4 | 2319 | // } |
hitonari | 0:2b57931c6ed4 | 2320 | |
hitonari | 0:2b57931c6ed4 | 2321 | // Pass gyro rate as rad/s |
hitonari | 0:2b57931c6ed4 | 2322 | mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); |
hitonari | 0:2b57931c6ed4 | 2323 | // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
hitonari | 0:2b57931c6ed4 | 2324 | |
hitonari | 0:2b57931c6ed4 | 2325 | mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); |
hitonari | 0:2b57931c6ed4 | 2326 | // mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); |
hitonari | 0:2b57931c6ed4 | 2327 | |
hitonari | 0:2b57931c6ed4 | 2328 | /* Auto_Loop()で50ms待ってるから要らないと思う |
hitonari | 0:2b57931c6ed4 | 2329 | // Serial print and/or display at 0.5 s rate independent of data rates |
hitonari | 0:2b57931c6ed4 | 2330 | delt_t = t.read_ms() - count; |
hitonari | 0:2b57931c6ed4 | 2331 | if (delt_t > 500) { // update LCD once per half-second independent of read rate |
hitonari | 0:2b57931c6ed4 | 2332 | */ |
hitonari | 0:2b57931c6ed4 | 2333 | |
hitonari | 0:2b57931c6ed4 | 2334 | //pc.printf("ax = %f", 1000*ax); |
hitonari | 0:2b57931c6ed4 | 2335 | //pc.printf(" ay = %f", 1000*ay); |
hitonari | 0:2b57931c6ed4 | 2336 | //pc.printf(" az = %f mg\n\r", 1000*az); |
hitonari | 0:2b57931c6ed4 | 2337 | |
hitonari | 0:2b57931c6ed4 | 2338 | //pc.printf("gx = %f", gx); |
hitonari | 0:2b57931c6ed4 | 2339 | //pc.printf(" gy = %f", gy); |
hitonari | 0:2b57931c6ed4 | 2340 | //pc.printf(" gz = %f deg/s\n\r", gz); |
hitonari | 0:2b57931c6ed4 | 2341 | |
hitonari | 0:2b57931c6ed4 | 2342 | //pc.printf("mx = %f,", mx); |
hitonari | 0:2b57931c6ed4 | 2343 | //pc.printf(" my = %f,", my); |
hitonari | 0:2b57931c6ed4 | 2344 | //pc.printf(" mz = %f mG\n\r", mz); |
hitonari | 0:2b57931c6ed4 | 2345 | |
hitonari | 0:2b57931c6ed4 | 2346 | tempCount = mpu9250.readTempData(); // Read the adc values |
hitonari | 0:2b57931c6ed4 | 2347 | temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade |
hitonari | 0:2b57931c6ed4 | 2348 | //pc.printf(" temperature = %f C\n\r", temperature); |
hitonari | 0:2b57931c6ed4 | 2349 | |
hitonari | 0:2b57931c6ed4 | 2350 | //pc.printf("q0 = %f\n\r", q[0]); |
hitonari | 0:2b57931c6ed4 | 2351 | //pc.printf("q1 = %f\n\r", q[1]); |
hitonari | 0:2b57931c6ed4 | 2352 | //pc.printf("q2 = %f\n\r", q[2]); |
hitonari | 0:2b57931c6ed4 | 2353 | //pc.printf("q3 = %f\n\r", q[3]); |
hitonari | 0:2b57931c6ed4 | 2354 | |
hitonari | 0:2b57931c6ed4 | 2355 | |
hitonari | 0:2b57931c6ed4 | 2356 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
hitonari | 0:2b57931c6ed4 | 2357 | // In this coordinate system, the positive z-axis is down toward Earth. |
hitonari | 0:2b57931c6ed4 | 2358 | // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. |
hitonari | 0:2b57931c6ed4 | 2359 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
hitonari | 0:2b57931c6ed4 | 2360 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
hitonari | 0:2b57931c6ed4 | 2361 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
hitonari | 0:2b57931c6ed4 | 2362 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
hitonari | 0:2b57931c6ed4 | 2363 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
hitonari | 0:2b57931c6ed4 | 2364 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
hitonari | 0:2b57931c6ed4 | 2365 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
hitonari | 0:2b57931c6ed4 | 2366 | roll = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
hitonari | 0:2b57931c6ed4 | 2367 | pitch = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
hitonari | 0:2b57931c6ed4 | 2368 | pitch *= 180.0f / PI; |
hitonari | 0:2b57931c6ed4 | 2369 | yaw *= 180.0f / PI; |
hitonari | 0:2b57931c6ed4 | 2370 | yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 |
hitonari | 0:2b57931c6ed4 | 2371 | roll *= 180.0f / PI; |
hitonari | 0:2b57931c6ed4 | 2372 | |
hitonari | 0:2b57931c6ed4 | 2373 | //pc.printf("$%d %d %d;",(int)(yaw*100),(int)(pitch*100),(int)(roll*100)); |
hitonari | 0:2b57931c6ed4 | 2374 | //pc.printf("Yaw: %f Pitch:%f Roll:%f\n\r", yaw, pitch, roll); |
hitonari | 0:2b57931c6ed4 | 2375 | //pc.printf("average rate = %f\n\r", (float) sumCount/sum); |
hitonari | 0:2b57931c6ed4 | 2376 | |
hitonari | 0:2b57931c6ed4 | 2377 | count = t.read_ms(); |
hitonari | 0:2b57931c6ed4 | 2378 | g_sum = 0; |
hitonari | 0:2b57931c6ed4 | 2379 | g_sumCount = 0; |
hitonari | 0:2b57931c6ed4 | 2380 | //} |
hitonari | 0:2b57931c6ed4 | 2381 | } |
hitonari | 0:2b57931c6ed4 | 2382 | |
hitonari | 0:2b57931c6ed4 | 2383 | void SetupBMP280(){ |
hitonari | 0:2b57931c6ed4 | 2384 | bmp.initialize(BMP280::INDOOR_NAVIGATION); |
hitonari | 0:2b57931c6ed4 | 2385 | |
hitonari | 0:2b57931c6ed4 | 2386 | for(int sn = 0; sn < 100; sn++){ |
hitonari | 0:2b57931c6ed4 | 2387 | g_press = bmp.getPressure(); |
hitonari | 0:2b57931c6ed4 | 2388 | if(sn > 0){ |
hitonari | 0:2b57931c6ed4 | 2389 | g_InitPress[sn] = g_press; |
hitonari | 0:2b57931c6ed4 | 2390 | g_InitPressAve += g_InitPress[sn]; |
hitonari | 0:2b57931c6ed4 | 2391 | float press_ratio = g_st_press / g_press; |
hitonari | 0:2b57931c6ed4 | 2392 | g_InitBMPHeight[sn] = ((powf(press_ratio, 0.19) - 1) * 298.15 * 1000) / 0.0065; |
hitonari | 0:2b57931c6ed4 | 2393 | g_InitBMPHeightAve += g_InitBMPHeight[sn]; |
hitonari | 0:2b57931c6ed4 | 2394 | |
hitonari | 0:2b57931c6ed4 | 2395 | } |
hitonari | 0:2b57931c6ed4 | 2396 | pc.printf("%f,%f\r\n", g_InitBMPHeight[sn],g_InitPress[sn]); |
hitonari | 0:2b57931c6ed4 | 2397 | wait(0.05); |
hitonari | 0:2b57931c6ed4 | 2398 | } |
hitonari | 0:2b57931c6ed4 | 2399 | |
hitonari | 0:2b57931c6ed4 | 2400 | g_InitPressAve /= 100.0; |
hitonari | 0:2b57931c6ed4 | 2401 | |
hitonari | 0:2b57931c6ed4 | 2402 | g_InitBMPHeightAve /= 100.0; |
hitonari | 0:2b57931c6ed4 | 2403 | |
hitonari | 0:2b57931c6ed4 | 2404 | pc.printf("\r\n%f,%f\r\n\r\n", g_InitBMPHeightAve,g_InitPressAve); |
hitonari | 0:2b57931c6ed4 | 2405 | |
hitonari | 0:2b57931c6ed4 | 2406 | pc.printf("bmp280 initialized\r\n"); |
hitonari | 0:2b57931c6ed4 | 2407 | |
hitonari | 0:2b57931c6ed4 | 2408 | g_BMPheight = g_InitBMPHeightAve; |
hitonari | 0:2b57931c6ed4 | 2409 | |
hitonari | 0:2b57931c6ed4 | 2410 | tBMP.start();//dtを計測 |
hitonari | 0:2b57931c6ed4 | 2411 | |
hitonari | 0:2b57931c6ed4 | 2412 | sensing();//最初だけ空回し |
hitonari | 0:2b57931c6ed4 | 2413 | |
hitonari | 0:2b57931c6ed4 | 2414 | g_tempaz[0] = az * g_grav * 1000; |
hitonari | 0:2b57931c6ed4 | 2415 | |
hitonari | 0:2b57931c6ed4 | 2416 | } |
hitonari | 0:2b57931c6ed4 | 2417 | |
hitonari | 0:2b57931c6ed4 | 2418 | void getBMP280(){ |
hitonari | 0:2b57931c6ed4 | 2419 | float z_angle, gyro_ver, z_angle_prev; |
hitonari | 0:2b57931c6ed4 | 2420 | static float press_rel_coef = 0.03; |
hitonari | 0:2b57931c6ed4 | 2421 | float press_ratio = g_st_press / g_press; |
hitonari | 0:2b57931c6ed4 | 2422 | |
hitonari | 0:2b57931c6ed4 | 2423 | g_z_PressHeight = g_BMPheight; |
hitonari | 0:2b57931c6ed4 | 2424 | |
hitonari | 0:2b57931c6ed4 | 2425 | z_angle = atan2(sqrt(ax * ax + ay * ay), az); |
hitonari | 0:2b57931c6ed4 | 2426 | gyro_ver = cos(z_angle); //垂直方向の加速度 |
hitonari | 0:2b57931c6ed4 | 2427 | |
hitonari | 0:2b57931c6ed4 | 2428 | g_dt = tBMP.read_us(); |
hitonari | 0:2b57931c6ed4 | 2429 | g_dt /= 1000000; |
hitonari | 0:2b57931c6ed4 | 2430 | tBMP.reset(); |
hitonari | 0:2b57931c6ed4 | 2431 | tBMP.start(); |
hitonari | 0:2b57931c6ed4 | 2432 | |
hitonari | 0:2b57931c6ed4 | 2433 | |
hitonari | 0:2b57931c6ed4 | 2434 | g_z_AccelHeight = g_z_prev - g_v_prev * g_dt - (az - gyro_ver) * g_grav *1000* g_dt * g_dt / 2; |
hitonari | 0:2b57931c6ed4 | 2435 | |
hitonari | 0:2b57931c6ed4 | 2436 | g_z_comp = g_z_PressHeight* press_rel_coef + g_z_AccelHeight* (1 - press_rel_coef);//係数は足して1になるように適当に設定,単位はmm |
hitonari | 0:2b57931c6ed4 | 2437 | |
hitonari | 0:2b57931c6ed4 | 2438 | //pc.printf("%f\r\n",g_z_comp/1000); |
hitonari | 0:2b57931c6ed4 | 2439 | |
hitonari | 0:2b57931c6ed4 | 2440 | g_BMPheight_int = g_z_comp; |
hitonari | 0:2b57931c6ed4 | 2441 | |
hitonari | 0:2b57931c6ed4 | 2442 | g_press = bmp.getPressure(); |
hitonari | 0:2b57931c6ed4 | 2443 | |
hitonari | 0:2b57931c6ed4 | 2444 | g_BMPheight = ((powf(press_ratio, 0.19) - 1) * 298.15 *1000) / 0.0065 - g_InitBMPHeightAve; |
hitonari | 0:2b57931c6ed4 | 2445 | |
hitonari | 0:2b57931c6ed4 | 2446 | NVIC_DisableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 2447 | NVIC_DisableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 2448 | NVIC_DisableIRQ(TIM5_IRQn); |
hitonari | 0:2b57931c6ed4 | 2449 | NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止 |
hitonari | 0:2b57931c6ed4 | 2450 | NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止 |
hitonari | 0:2b57931c6ed4 | 2451 | sensing(); |
hitonari | 0:2b57931c6ed4 | 2452 | NVIC_EnableIRQ(USART1_IRQn); |
hitonari | 0:2b57931c6ed4 | 2453 | NVIC_EnableIRQ(USART2_IRQn); |
hitonari | 0:2b57931c6ed4 | 2454 | NVIC_EnableIRQ(TIM5_IRQn); |
hitonari | 0:2b57931c6ed4 | 2455 | NVIC_EnableIRQ(EXTI0_IRQn); |
hitonari | 0:2b57931c6ed4 | 2456 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 2457 | |
hitonari | 0:2b57931c6ed4 | 2458 | g_v_prev = (g_z_comp - g_z_prev) / g_dt / 1000000; |
hitonari | 0:2b57931c6ed4 | 2459 | |
hitonari | 0:2b57931c6ed4 | 2460 | g_z_prev = g_z_comp; |
hitonari | 0:2b57931c6ed4 | 2461 | |
hitonari | 0:2b57931c6ed4 | 2462 | |
hitonari | 0:2b57931c6ed4 | 2463 | //wait_ms(1.0); |
hitonari | 0:2b57931c6ed4 | 2464 | |
hitonari | 0:2b57931c6ed4 | 2465 | } |
hitonari | 0:2b57931c6ed4 | 2466 | |
hitonari | 0:2b57931c6ed4 | 2467 | |
hitonari | 0:2b57931c6ed4 | 2468 | |
hitonari | 0:2b57931c6ed4 | 2469 | //デバッグ用= |
hitonari | 0:2b57931c6ed4 | 2470 | void DebugPrint() |
hitonari | 0:2b57931c6ed4 | 2471 | { |
hitonari | 0:2b57931c6ed4 | 2472 | /* |
hitonari | 0:2b57931c6ed4 | 2473 | static int16_t deltaT = 0, t_start = 0; |
hitonari | 0:2b57931c6ed4 | 2474 | //deltaT = t.read_u2s() - t_start; |
hitonari | 0:2b57931c6ed4 | 2475 | pc.printf("t:%d us, ",deltaT); |
hitonari | 0:2b57931c6ed4 | 2476 | pc.printf("\r\n"); |
hitonari | 0:2b57931c6ed4 | 2477 | t_start = t.read_us(); |
hitonari | 0:2b57931c6ed4 | 2478 | |
hitonari | 0:2b57931c6ed4 | 2479 | for(uint8_t i=0; i<6; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]); |
hitonari | 0:2b57931c6ed4 | 2480 | if(g_trim_check){ |
hitonari | 0:2b57931c6ed4 | 2481 | for(uint8_t i=0; i<6; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]); |
hitonari | 0:2b57931c6ed4 | 2482 | pc.printf("\r\n"); |
hitonari | 0:2b57931c6ed4 | 2483 | } |
hitonari | 0:2b57931c6ed4 | 2484 | |
hitonari | 0:2b57931c6ed4 | 2485 | pc.printf("trimpwm[AIL_R]:%d ELE:%d THR:%d RUD:%d 4:%d AIL_L:%d",trimpwm[AIL_R],trimpwm[ELE],trimpwm[THR],trimpwm[RUD],trimpwm[4],trimpwm[AIL_L]); |
hitonari | 0:2b57931c6ed4 | 2486 | for(uint8_t i=0; i<8; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]); |
hitonari | 0:2b57931c6ed4 | 2487 | for(uint8_t i=1; i<4; i++) pc.printf("sbus.manualpwm[%d]:%d ",i,sbus.manualpwm[i]); |
hitonari | 0:2b57931c6ed4 | 2488 | pc.printf("\r\n"); |
hitonari | 0:2b57931c6ed4 | 2489 | for(uint8_t i=0; i<3; i++) pc.printf("nowangle[%d]:%3.2f\t",i,nowAngle[i]); |
hitonari | 0:2b57931c6ed4 | 2490 | for(uint8_t i=0; i<2; i++) pc.printf("nowangle[%d]:%3.2f\t",i,nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ |
hitonari | 0:2b57931c6ed4 | 2491 | pc.printf("nowangle[pitch]:%3.2f\t\r\n",nowAngle[2]); |
hitonari | 0:2b57931c6ed4 | 2492 | pc.printf("autopwm[ELE]:%d\t\r\n",autopwm[ELE]); |
hitonari | 0:2b57931c6ed4 | 2493 | pc.printf("autpwm[RUD]:%d\t",autopwm[RUD]);*/ |
hitonari | 0:2b57931c6ed4 | 2494 | NVIC_DisableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 2495 | pc.printf("g_distance:%d\t",g_distance);/* |
hitonari | 0:2b57931c6ed4 | 2496 | NVIC_EnableIRQ(EXTI9_5_IRQn); |
hitonari | 0:2b57931c6ed4 | 2497 | pc.printf("Mode: %c: ",g_buf[0]); |
hitonari | 0:2b57931c6ed4 | 2498 | if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); |
hitonari | 0:2b57931c6ed4 | 2499 | pc.printf("%f",g_z_comp); |
hitonari | 0:2b57931c6ed4 | 2500 | pc.printf("\r\n%f,%f\r\n", g_InitBMPHeightAve,g_InitPressAve); |
hitonari | 0:2b57931c6ed4 | 2501 | pc.printf("\r\n"); */ |
hitonari | 0:2b57931c6ed4 | 2502 | } |