航空研究会 / Mbed 2 deprecated AutoFlight2017_now_copy

s

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2017_now_copy by Bot Furukawa

main.cpp@0:92024886c0be, 2017-08-01 (annotated)

Committer:
TUATBM
Date:
Tue Aug 01 12:27:13 2017 +0000
Revision:
0:92024886c0be
Child:
1:f31e17341659
??????????????????????; ????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TUATBM 0:92024886c0be 1 #include "mbed.h"
TUATBM 0:92024886c0be 2 //#include "rtos.h"
TUATBM 0:92024886c0be 3
TUATBM 0:92024886c0be 4 #include "math.h"
TUATBM 0:92024886c0be 5 #include "MPU9250.h"
TUATBM 0:92024886c0be 6 #include "BMP280.h"
TUATBM 0:92024886c0be 7 #include "SkipperSv2.h"
TUATBM 0:92024886c0be 8 #include "Estrela.h"
TUATBM 0:92024886c0be 9 #include "sbus.h"
TUATBM 0:92024886c0be 10 #include "pid.h"
TUATBM 0:92024886c0be 11
TUATBM 0:92024886c0be 12 #define DEBUG_SEMIAUTO 1
TUATBM 0:92024886c0be 13 #define DEBUG_PRINT_INLOOP 1
TUATBM 0:92024886c0be 14
TUATBM 0:92024886c0be 15 #define KP_ELE 2
TUATBM 0:92024886c0be 16 #define KI_ELE 0
TUATBM 0:92024886c0be 17 #define KD_ELE 0
TUATBM 0:92024886c0be 18 #define KP_RUD 2
TUATBM 0:92024886c0be 19 #define KI_RUD 0
TUATBM 0:92024886c0be 20 #define KD_RUD 0
TUATBM 0:92024886c0be 21
TUATBM 0:92024886c0be 22 #define GAIN_CONTROLVALUE_TO_PWM 3
TUATBM 0:92024886c0be 23
TUATBM 0:92024886c0be 24 #define MAGBIAS_X 185.0
TUATBM 0:92024886c0be 25 #define MAGBIAS_Y 220.0
TUATBM 0:92024886c0be 26 #define MAGBIAS_Z -350.0
TUATBM 0:92024886c0be 27
TUATBM 0:92024886c0be 28 const uint16_t changeModeCount = 50;
TUATBM 0:92024886c0be 29 const int16_t lengthdivpwm = 320;
TUATBM 0:92024886c0be 30
TUATBM 0:92024886c0be 31 const int16_t trim[4] = {0,14,-100,-10};
TUATBM 0:92024886c0be 32 const float expMax[4] = {100,115,100,89};
TUATBM 0:92024886c0be 33 const float expMin[4] = {100,47,100,110};
TUATBM 0:92024886c0be 34
TUATBM 0:92024886c0be 35
TUATBM 0:92024886c0be 36 SBUS sbus(PA_9, PA_10); //SBUS
TUATBM 0:92024886c0be 37
TUATBM 0:92024886c0be 38 PwmOut servo1(PC_6); // TIM3_CH1
TUATBM 0:92024886c0be 39 PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
TUATBM 0:92024886c0be 40 PwmOut servo3(PB_0); // TIM3_CH3
TUATBM 0:92024886c0be 41 PwmOut servo4(PB_1); // TIM3_CH4
TUATBM 0:92024886c0be 42 PwmOut servo5(PB_6); // TIM4_CH1
TUATBM 0:92024886c0be 43 PwmOut servo6(PB_7); // TIM4_CH2
TUATBM 0:92024886c0be 44 PwmOut servo7(PB_8); // TIM4_CH3 //PB_8
TUATBM 0:92024886c0be 45 PwmOut servo8(PB_9); // TIM4_CH4
TUATBM 0:92024886c0be 46
TUATBM 0:92024886c0be 47 Serial pc(PA_2, PA_3);
TUATBM 0:92024886c0be 48
TUATBM 0:92024886c0be 49 DigitalOut led1(PA_0); //緑
TUATBM 0:92024886c0be 50 DigitalOut led2(PA_1); //赤
TUATBM 0:92024886c0be 51 DigitalOut led3(PB_4);
TUATBM 0:92024886c0be 52 DigitalOut led4(PB_5);
TUATBM 0:92024886c0be 53
TUATBM 0:92024886c0be 54 InterruptIn switch2(PC_14);
TUATBM 0:92024886c0be 55
TUATBM 0:92024886c0be 56 MPU9250 mpu9250(PC_9,PA_8,&pc);
TUATBM 0:92024886c0be 57
TUATBM 0:92024886c0be 58 PID pid_ELE(KP_ELE,KI_ELE,KD_ELE);
TUATBM 0:92024886c0be 59 PID pid_RUD(KP_RUD,KI_RUD,KD_RUD);
TUATBM 0:92024886c0be 60
TUATBM 0:92024886c0be 61 enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8};
TUATBM 0:92024886c0be 62 enum Angle{ROLL, PITCH, YAW};
TUATBM 0:92024886c0be 63 enum OperationMode{StartUp, SemiAuto};
TUATBM 0:92024886c0be 64 enum OutputStatus{Manual, Auto};
TUATBM 0:92024886c0be 65
TUATBM 0:92024886c0be 66 static OutputStatus output_status = Manual;
TUATBM 0:92024886c0be 67 OperationMode operation_mode = StartUp;
TUATBM 0:92024886c0be 68 static int16_t autopwm[8] = {1500,1500,1000,1500};
TUATBM 0:92024886c0be 69
TUATBM 0:92024886c0be 70 static int16_t trimpwm[4] = {1500,1500,1000,1500};
TUATBM 0:92024886c0be 71 int16_t maxpwm[4] = {1820,1820,1820,1820};
TUATBM 0:92024886c0be 72 int16_t minpwm[4] = {1180,1180,1180,1180};
TUATBM 0:92024886c0be 73 int16_t oldTHR = 1000;
TUATBM 0:92024886c0be 74
TUATBM 0:92024886c0be 75 static float nowAngle[3] = {0,0,0};
TUATBM 0:92024886c0be 76 const float trimAngle[3] = {0,0,0};
TUATBM 0:92024886c0be 77 const float maxAngle[2] = {90, 90};
TUATBM 0:92024886c0be 78 const float minAngle[2] = {-90, -90};
TUATBM 0:92024886c0be 79
TUATBM 0:92024886c0be 80 Timer t;
TUATBM 0:92024886c0be 81
TUATBM 0:92024886c0be 82 //IWDG_HandleTypeDef hiwdg;
TUATBM 0:92024886c0be 83
TUATBM 0:92024886c0be 84 /*-----関数のプロトタイプ宣言-----*/
TUATBM 0:92024886c0be 85 void setup();
TUATBM 0:92024886c0be 86 void loop();
TUATBM 0:92024886c0be 87
TUATBM 0:92024886c0be 88 void Init_PWM();
TUATBM 0:92024886c0be 89 void Init_servo(); //サーボ初期化
TUATBM 0:92024886c0be 90 void Init_sbus(); //SBUS初期化
TUATBM 0:92024886c0be 91 void Init_sensors();
TUATBM 0:92024886c0be 92 void DisplayClock(); //クロック状態確認
TUATBM 0:92024886c0be 93
TUATBM 0:92024886c0be 94 void SensingMPU();
TUATBM 0:92024886c0be 95 void UpdateTargetAngle(float targetAngle[3]);
TUATBM 0:92024886c0be 96 void CalculateControlValue(float targetAngle[3], float controlValue[3]);
TUATBM 0:92024886c0be 97 void UpdateAutoPWM(float controlValue[3]);
TUATBM 0:92024886c0be 98 void ConvertPWMintoRAD(float targetAngle[3]);
TUATBM 0:92024886c0be 99 inline float CalcRatio(float value, float trim, float limit);
TUATBM 0:92024886c0be 100 bool CheckSW_Up(Channel ch);
TUATBM 0:92024886c0be 101 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min);
TUATBM 0:92024886c0be 102
TUATBM 0:92024886c0be 103 //sbus割り込み
TUATBM 0:92024886c0be 104 void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力
TUATBM 0:92024886c0be 105 void Output_PWM(int16_t pwm[8]); //pwmをサーボへ出力
TUATBM 0:92024886c0be 106
TUATBM 0:92024886c0be 107 //switch2割り込み
TUATBM 0:92024886c0be 108 void ResetTrim();
TUATBM 0:92024886c0be 109
TUATBM 0:92024886c0be 110 //デバッグ用
TUATBM 0:92024886c0be 111 void DebugPrint();
TUATBM 0:92024886c0be 112
TUATBM 0:92024886c0be 113 /*---関数のプロトタイプ宣言終わり---*/
TUATBM 0:92024886c0be 114
TUATBM 0:92024886c0be 115 int main()
TUATBM 0:92024886c0be 116 {
TUATBM 0:92024886c0be 117 setup();
TUATBM 0:92024886c0be 118 while(1){
TUATBM 0:92024886c0be 119 loop();
TUATBM 0:92024886c0be 120 }
TUATBM 0:92024886c0be 121 }
TUATBM 0:92024886c0be 122
TUATBM 0:92024886c0be 123 void setup(){
TUATBM 0:92024886c0be 124 led1 = 1;
TUATBM 0:92024886c0be 125 led2 = 1;
TUATBM 0:92024886c0be 126 led3 = 0;
TUATBM 0:92024886c0be 127 led4 = 0;
TUATBM 0:92024886c0be 128
TUATBM 0:92024886c0be 129 pc.baud(115200);
TUATBM 0:92024886c0be 130 Init_PWM();
TUATBM 0:92024886c0be 131 Init_servo();
TUATBM 0:92024886c0be 132 Init_sbus();
TUATBM 0:92024886c0be 133 Init_sensors();
TUATBM 0:92024886c0be 134 switch2.rise(ResetTrim);
TUATBM 0:92024886c0be 135 t.start();
TUATBM 0:92024886c0be 136 DisplayClock();
TUATBM 0:92024886c0be 137
TUATBM 0:92024886c0be 138 led1 = 0;
TUATBM 0:92024886c0be 139 led2 = 0;
TUATBM 0:92024886c0be 140 led3 = 0;
TUATBM 0:92024886c0be 141 led4 = 0;
TUATBM 0:92024886c0be 142
TUATBM 0:92024886c0be 143 pc.printf("All initialized\r\n");
TUATBM 0:92024886c0be 144
TUATBM 0:92024886c0be 145 mpu9250.sensingAcGyMg();
TUATBM 0:92024886c0be 146 mpu9250.calculatePostureAngle(nowAngle);
TUATBM 0:92024886c0be 147 }
TUATBM 0:92024886c0be 148
TUATBM 0:92024886c0be 149 void loop(){
TUATBM 0:92024886c0be 150 static float targetAngle[3], controlValue[2];
TUATBM 0:92024886c0be 151
TUATBM 0:92024886c0be 152 SensingMPU();
TUATBM 0:92024886c0be 153 UpdateTargetAngle(targetAngle);
TUATBM 0:92024886c0be 154 CalculateControlValue(targetAngle, controlValue);
TUATBM 0:92024886c0be 155 UpdateAutoPWM(controlValue);
TUATBM 0:92024886c0be 156
TUATBM 0:92024886c0be 157 #if DEBUG_PRINT_INLOOP
TUATBM 0:92024886c0be 158 DebugPrint();
TUATBM 0:92024886c0be 159 #endif
TUATBM 0:92024886c0be 160 }
TUATBM 0:92024886c0be 161
TUATBM 0:92024886c0be 162
TUATBM 0:92024886c0be 163
TUATBM 0:92024886c0be 164 //サーボ初期化関数
TUATBM 0:92024886c0be 165 void Init_servo()
TUATBM 0:92024886c0be 166 {
TUATBM 0:92024886c0be 167
TUATBM 0:92024886c0be 168 servo1.period_ms(14);
TUATBM 0:92024886c0be 169 servo1.pulsewidth_us(trimpwm[AIL]);
TUATBM 0:92024886c0be 170
TUATBM 0:92024886c0be 171 servo2.period_ms(14);
TUATBM 0:92024886c0be 172 servo2.pulsewidth_us(trimpwm[ELE]);
TUATBM 0:92024886c0be 173
TUATBM 0:92024886c0be 174 servo3.period_ms(14);
TUATBM 0:92024886c0be 175 servo3.pulsewidth_us(trimpwm[THR]);
TUATBM 0:92024886c0be 176
TUATBM 0:92024886c0be 177 servo4.period_ms(14);
TUATBM 0:92024886c0be 178 servo4.pulsewidth_us(trimpwm[RUD]);
TUATBM 0:92024886c0be 179
TUATBM 0:92024886c0be 180 servo5.period_ms(14);
TUATBM 0:92024886c0be 181 servo5.pulsewidth_us(1500);
TUATBM 0:92024886c0be 182
TUATBM 0:92024886c0be 183 servo6.period_ms(14);
TUATBM 0:92024886c0be 184 servo6.pulsewidth_us(1500);
TUATBM 0:92024886c0be 185
TUATBM 0:92024886c0be 186 servo7.period_ms(14);
TUATBM 0:92024886c0be 187 servo7.pulsewidth_us(1500);
TUATBM 0:92024886c0be 188
TUATBM 0:92024886c0be 189 servo8.period_ms(14);
TUATBM 0:92024886c0be 190 servo8.pulsewidth_us(1500);
TUATBM 0:92024886c0be 191
TUATBM 0:92024886c0be 192 pc.printf("servo initialized\r\n");
TUATBM 0:92024886c0be 193 }
TUATBM 0:92024886c0be 194
TUATBM 0:92024886c0be 195 //Sbusを初期化する関数
TUATBM 0:92024886c0be 196 void Init_sbus(){
TUATBM 0:92024886c0be 197 sbus.initialize();
TUATBM 0:92024886c0be 198 sbus.setLastfuncPoint(Update_PWM);
TUATBM 0:92024886c0be 199 sbus.startInterrupt();
TUATBM 0:92024886c0be 200 }
TUATBM 0:92024886c0be 201
TUATBM 0:92024886c0be 202 void Init_sensors(){
TUATBM 0:92024886c0be 203 if(!mpu9250.Initialize()){
TUATBM 0:92024886c0be 204 pc.printf("failed initialize\r\n");
TUATBM 0:92024886c0be 205 while(1);
TUATBM 0:92024886c0be 206 }
TUATBM 0:92024886c0be 207 mpu9250.setMagBias(MAGBIAS_X,MAGBIAS_Y,MAGBIAS_Z);
TUATBM 0:92024886c0be 208 }
TUATBM 0:92024886c0be 209
TUATBM 0:92024886c0be 210 void Init_PWM(){
TUATBM 0:92024886c0be 211 for (uint8_t i = 0; i < 4; ++i){
TUATBM 0:92024886c0be 212 trimpwm[i] = 1500 + trim[i];
TUATBM 0:92024886c0be 213 maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100));
TUATBM 0:92024886c0be 214 minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100));
TUATBM 0:92024886c0be 215 }
TUATBM 0:92024886c0be 216 pc.printf("PWM initialized\r\n");
TUATBM 0:92024886c0be 217 }
TUATBM 0:92024886c0be 218
TUATBM 0:92024886c0be 219 void DisplayClock(){
TUATBM 0:92024886c0be 220 pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
TUATBM 0:92024886c0be 221 pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
TUATBM 0:92024886c0be 222 pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
TUATBM 0:92024886c0be 223 pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
TUATBM 0:92024886c0be 224 pc.printf("\r\n");
TUATBM 0:92024886c0be 225 }
TUATBM 0:92024886c0be 226
TUATBM 0:92024886c0be 227 void UpdateTargetAngle(float targetAngle[3]){
TUATBM 0:92024886c0be 228 static uint16_t count_op = 0, count_out = 0;
TUATBM 0:92024886c0be 229
TUATBM 0:92024886c0be 230 #if DEBUG_SEMIAUTO
TUATBM 0:92024886c0be 231 switch(operation_mode){
TUATBM 0:92024886c0be 232 case StartUp:
TUATBM 0:92024886c0be 233 if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){
TUATBM 0:92024886c0be 234 count_op++;
TUATBM 0:92024886c0be 235 if(count_op > changeModeCount){
TUATBM 0:92024886c0be 236 operation_mode = SemiAuto;
TUATBM 0:92024886c0be 237 pc.printf("Goto SemiAuto mode\r\n");
TUATBM 0:92024886c0be 238 count_op = 0;
TUATBM 0:92024886c0be 239 }
TUATBM 0:92024886c0be 240 }else count_op = 0;
TUATBM 0:92024886c0be 241 break;
TUATBM 0:92024886c0be 242
TUATBM 0:92024886c0be 243 case SemiAuto:
TUATBM 0:92024886c0be 244 /* 大会用では以下のif文を入れてoperation_modeを変える
TUATBM 0:92024886c0be 245 if(CheckSW_Up(Ch6)){
TUATBM 0:92024886c0be 246 count_op++;
TUATBM 0:92024886c0be 247 if(count_op>changeModeCount){
TUATBM 0:92024886c0be 248 output_status = XXX;
TUATBM 0:92024886c0be 249 led2 = 0;
TUATBM 0:92024886c0be 250 pc.printf("Goto XXX mode\r\n");
TUATBM 0:92024886c0be 251 count_op = 0;
TUATBM 0:92024886c0be 252 }else count_op = 0;
TUATBM 0:92024886c0be 253 ConvertPWMintoRAD(targetAngle);
TUATBM 0:92024886c0be 254 }
TUATBM 0:92024886c0be 255 */
TUATBM 0:92024886c0be 256 ConvertPWMintoRAD(targetAngle);
TUATBM 0:92024886c0be 257 break;
TUATBM 0:92024886c0be 258
TUATBM 0:92024886c0be 259 default:
TUATBM 0:92024886c0be 260 operation_mode = SemiAuto;
TUATBM 0:92024886c0be 261 break;
TUATBM 0:92024886c0be 262 }
TUATBM 0:92024886c0be 263
TUATBM 0:92024886c0be 264 if(CheckSW_Up(Ch5)){
TUATBM 0:92024886c0be 265 output_status = Auto;
TUATBM 0:92024886c0be 266 led1 = 1;
TUATBM 0:92024886c0be 267 }else{
TUATBM 0:92024886c0be 268 output_status = Manual;
TUATBM 0:92024886c0be 269 led1 = 0;
TUATBM 0:92024886c0be 270 }
TUATBM 0:92024886c0be 271 #endif
TUATBM 0:92024886c0be 272 }
TUATBM 0:92024886c0be 273
TUATBM 0:92024886c0be 274 void CalculateControlValue(float targetAngle[3], float controlValue[3]){
TUATBM 0:92024886c0be 275 static int t_last;
TUATBM 0:92024886c0be 276 int t_now;
TUATBM 0:92024886c0be 277 float dt;
TUATBM 0:92024886c0be 278
TUATBM 0:92024886c0be 279 t_now = t.read_us();
TUATBM 0:92024886c0be 280 dt = (float)((t_now - t_last)/1000000.0f) ;
TUATBM 0:92024886c0be 281 t_last = t_now;
TUATBM 0:92024886c0be 282
TUATBM 0:92024886c0be 283 controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt);
TUATBM 0:92024886c0be 284 controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt);
TUATBM 0:92024886c0be 285 }
TUATBM 0:92024886c0be 286
TUATBM 0:92024886c0be 287 void UpdateAutoPWM(float controlValue[3]){
TUATBM 0:92024886c0be 288 int16_t addpwm[2]; //-500~500
TUATBM 0:92024886c0be 289 addpwm[ROLL] = GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ：右回転正 レバー:右回転正
TUATBM 0:92024886c0be 290 addpwm[PITCH] = -1 * GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ：機首下げ正 レバー:機首上げ正
TUATBM 0:92024886c0be 291
TUATBM 0:92024886c0be 292 autopwm[ELE] = trimpwm[ELE] + addpwm[PITCH];
TUATBM 0:92024886c0be 293 autopwm[RUD] = trimpwm[RUD] + addpwm[ROLL];
TUATBM 0:92024886c0be 294 autopwm[THR] = oldTHR;
TUATBM 0:92024886c0be 295
TUATBM 0:92024886c0be 296 autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
TUATBM 0:92024886c0be 297 autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]);
TUATBM 0:92024886c0be 298 }
TUATBM 0:92024886c0be 299
TUATBM 0:92024886c0be 300 void ConvertPWMintoRAD(float targetAngle[3]){
TUATBM 0:92024886c0be 301 float ratio[2];
TUATBM 0:92024886c0be 302
TUATBM 0:92024886c0be 303 if(sbus.manualpwm[ELE] > trimpwm[ELE]){
TUATBM 0:92024886c0be 304 ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)maxpwm[ELE]);
TUATBM 0:92024886c0be 305 targetAngle[PITCH] = ratio[0]*(maxAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];
TUATBM 0:92024886c0be 306 }else{
TUATBM 0:92024886c0be 307 ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)minpwm[ELE]);
TUATBM 0:92024886c0be 308 targetAngle[PITCH] = ratio[0]*(minAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH];
TUATBM 0:92024886c0be 309 }
TUATBM 0:92024886c0be 310
TUATBM 0:92024886c0be 311 if(sbus.manualpwm[RUD] > trimpwm[RUD]){
TUATBM 0:92024886c0be 312 ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)maxpwm[RUD]);
TUATBM 0:92024886c0be 313 targetAngle[ROLL] = -1*ratio[1]*(maxAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];
TUATBM 0:92024886c0be 314 }else{
TUATBM 0:92024886c0be 315 ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)minpwm[RUD]);
TUATBM 0:92024886c0be 316 targetAngle[ROLL] = -1*ratio[1]*(minAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL];
TUATBM 0:92024886c0be 317 }
TUATBM 0:92024886c0be 318 }
TUATBM 0:92024886c0be 319
TUATBM 0:92024886c0be 320 inline float CalcRatio(float value, float trim, float limit){
TUATBM 0:92024886c0be 321 return (value - trim) / (limit - trim);
TUATBM 0:92024886c0be 322 }
TUATBM 0:92024886c0be 323
TUATBM 0:92024886c0be 324 bool CheckSW_Up(Channel ch){
TUATBM 0:92024886c0be 325 if(SWITCH_CHECK < sbus.manualpwm[ch]){
TUATBM 0:92024886c0be 326 return true;
TUATBM 0:92024886c0be 327 }else{
TUATBM 0:92024886c0be 328 return false;
TUATBM 0:92024886c0be 329 }
TUATBM 0:92024886c0be 330 }
TUATBM 0:92024886c0be 331
TUATBM 0:92024886c0be 332 int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){
TUATBM 0:92024886c0be 333 if(value > max) return max;
TUATBM 0:92024886c0be 334 if(value < min) return min;
TUATBM 0:92024886c0be 335 return value;
TUATBM 0:92024886c0be 336 }
TUATBM 0:92024886c0be 337
TUATBM 0:92024886c0be 338
TUATBM 0:92024886c0be 339
TUATBM 0:92024886c0be 340 /*---SBUS割り込み処理---*/
TUATBM 0:92024886c0be 341
TUATBM 0:92024886c0be 342 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
TUATBM 0:92024886c0be 343 //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード)
TUATBM 0:92024886c0be 344 void Update_PWM()
TUATBM 0:92024886c0be 345 {
TUATBM 0:92024886c0be 346 static int16_t pwm[8];
TUATBM 0:92024886c0be 347 if(sbus.flg_ch_update == true){
TUATBM 0:92024886c0be 348 switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
TUATBM 0:92024886c0be 349 case Manual:
TUATBM 0:92024886c0be 350 for(uint8_t i=0;i<8;i++){
TUATBM 0:92024886c0be 351 pwm[i] = sbus.manualpwm[i];
TUATBM 0:92024886c0be 352 }
TUATBM 0:92024886c0be 353 oldTHR = sbus.manualpwm[THR];
TUATBM 0:92024886c0be 354 //pc.printf("update_manual\r\n");
TUATBM 0:92024886c0be 355 break;
TUATBM 0:92024886c0be 356
TUATBM 0:92024886c0be 357 case Auto:
TUATBM 0:92024886c0be 358 pwm[AIL] = sbus.manualpwm[AIL];
TUATBM 0:92024886c0be 359 pwm[ELE] = autopwm[ELE];
TUATBM 0:92024886c0be 360 pwm[THR] = autopwm[THR];
TUATBM 0:92024886c0be 361 pwm[RUD] = autopwm[RUD];
TUATBM 0:92024886c0be 362 pwm[Ch5] = sbus.manualpwm[Ch5];
TUATBM 0:92024886c0be 363 pwm[Ch6] = sbus.manualpwm[Ch6];
TUATBM 0:92024886c0be 364 pwm[Ch7] = sbus.manualpwm[Ch7];
TUATBM 0:92024886c0be 365 pwm[Ch8] = sbus.manualpwm[Ch8];
TUATBM 0:92024886c0be 366
TUATBM 0:92024886c0be 367 //pc.printf("update_auto\r\n");
TUATBM 0:92024886c0be 368 break;
TUATBM 0:92024886c0be 369
TUATBM 0:92024886c0be 370 default:
TUATBM 0:92024886c0be 371 for(uint8_t i=0;i<8;i++){
TUATBM 0:92024886c0be 372 pwm[i] = sbus.manualpwm[i];
TUATBM 0:92024886c0be 373 } //pc.printf("update_manual\r\n");
TUATBM 0:92024886c0be 374 break;
TUATBM 0:92024886c0be 375 }
TUATBM 0:92024886c0be 376 }else{
TUATBM 0:92024886c0be 377 pc.printf("0\r\n");
TUATBM 0:92024886c0be 378 }
TUATBM 0:92024886c0be 379 sbus.flg_ch_update = false;
TUATBM 0:92024886c0be 380 //for(uint8_t i=0; i<8; i++){pc.printf("%d, ",pwm[i]);}
TUATBM 0:92024886c0be 381 //pc.printf("\r\n");
TUATBM 0:92024886c0be 382 Output_PWM(pwm);
TUATBM 0:92024886c0be 383 }
TUATBM 0:92024886c0be 384
TUATBM 0:92024886c0be 385
TUATBM 0:92024886c0be 386 //pwmをサーボに出力する関数。
TUATBM 0:92024886c0be 387 void Output_PWM(int16_t pwm[8])
TUATBM 0:92024886c0be 388 {
TUATBM 0:92024886c0be 389 servo1.pulsewidth_us(pwm[0]);
TUATBM 0:92024886c0be 390 servo2.pulsewidth_us(pwm[1]);
TUATBM 0:92024886c0be 391 servo3.pulsewidth_us(pwm[2]);
TUATBM 0:92024886c0be 392 servo4.pulsewidth_us(pwm[3]);
TUATBM 0:92024886c0be 393 servo5.pulsewidth_us(pwm[4]);
TUATBM 0:92024886c0be 394 servo6.pulsewidth_us(pwm[5]);
TUATBM 0:92024886c0be 395 servo7.pulsewidth_us(pwm[6]);
TUATBM 0:92024886c0be 396 servo8.pulsewidth_us(pwm[7]);
TUATBM 0:92024886c0be 397 }
TUATBM 0:92024886c0be 398
TUATBM 0:92024886c0be 399 void ResetTrim(){
TUATBM 0:92024886c0be 400 for(uint8_t i=0; i<4; i++){
TUATBM 0:92024886c0be 401 trimpwm[i] = sbus.manualpwm[i];
TUATBM 0:92024886c0be 402 }
TUATBM 0:92024886c0be 403 pc.printf("reset PWM trim\r\n");
TUATBM 0:92024886c0be 404 }
TUATBM 0:92024886c0be 405
TUATBM 0:92024886c0be 406 void SensingMPU(){
TUATBM 0:92024886c0be 407 //static int16_t deltaT = 0, t_start = 0;
TUATBM 0:92024886c0be 408 //t_start = t.read_us();
TUATBM 0:92024886c0be 409 NVIC_DisableIRQ(USART1_IRQn);
TUATBM 0:92024886c0be 410 if(!mpu9250.sensingAcGyMg()){
TUATBM 0:92024886c0be 411 //pc.printf("failed\r\n");
TUATBM 0:92024886c0be 412 return;
TUATBM 0:92024886c0be 413 }
TUATBM 0:92024886c0be 414 NVIC_EnableIRQ(USART1_IRQn);
TUATBM 0:92024886c0be 415 mpu9250.calculatePostureAngle(nowAngle);
TUATBM 0:92024886c0be 416 /*
TUATBM 0:92024886c0be 417 deltaT = t.read_us() - t_start;
TUATBM 0:92024886c0be 418 pc.printf("t:%d us, ",deltaT);
TUATBM 0:92024886c0be 419 mpu9250.displayAngle();
TUATBM 0:92024886c0be 420 */
TUATBM 0:92024886c0be 421
TUATBM 0:92024886c0be 422 //mpu9250.sensingPostureAngle(nowAngle);
TUATBM 0:92024886c0be 423 }
TUATBM 0:92024886c0be 424
TUATBM 0:92024886c0be 425 //デバッグ用
TUATBM 0:92024886c0be 426 void DebugPrint(){
TUATBM 0:92024886c0be 427
TUATBM 0:92024886c0be 428 //static int16_t deltaT = 0, t_start = 0;
TUATBM 0:92024886c0be 429 //t_start = t.read_us();
TUATBM 0:92024886c0be 430 for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
TUATBM 0:92024886c0be 431 pc.printf("\r\n");
TUATBM 0:92024886c0be 432 //deltaT = t.read_us() - t_start;
TUATBM 0:92024886c0be 433 //pc.printf("t:%d us, ",deltaT);
TUATBM 0:92024886c0be 434 //pc.printf("\r\n");
TUATBM 0:92024886c0be 435
TUATBM 0:92024886c0be 436 //mpu9250.displayAngle();
TUATBM 0:92024886c0be 437 //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]);
TUATBM 0:92024886c0be 438 //pc.printf("\r\n");
TUATBM 0:92024886c0be 439 }
TUATBM 0:92024886c0be 440