![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
MODSERIAL
Dependencies: HCSR04_2 MODSERIAL MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_22 by
main.cpp
- Committer:
- HARUKIDELTA
- Date:
- 2018-09-16
- Revision:
- 12:763bf416ba4b
- Parent:
- 11:c0b9ad25d3db
- Child:
- 13:77b13d5f4d78
File content as of revision 12:763bf416ba4b:
//mbed #include "mbed.h" #include "FATFileSystem.h" #include "SDFileSystem.h" //C #include "math.h" //sensor #include "MPU6050_DMP6.h" //#include "MPU9250.h" //#include "BMP280.h" #include "hcsr04.h" //device #include "sbus.h" //config #include "SkipperSv2.h" #include "falfalla.h" //other #include "pid.h" #define DEBUG_SEMIAUTO 0 #define DEBUG_PRINT_INLOOP 1 #define KP_ELE 15.0 //2.0 #define KI_ELE 0.0 #define KD_ELE 0.0 //0/0 #define KP_RUD 3.0 #define KI_RUD 0.0 #define KD_RUD 0.0 #define KP_AIL 0.1 #define KI_AIL 0.2 #define KD_AIL 0.2 //#define g_AIL_L_Ratio_rightloop 0.5 #define GAIN_CONTROLVALUE_TO_PWM 3.0 #define RIGHT_ROLL -12.0 #define RIGHT_PITCH -10.0 //5.0 #define LEFT_ROLL 12.0 #define LEFT_PITCH -5.0 #define STRAIGHT_ROLL 4.0 #define STRAIGHT_PITCH 3.0 #define TAKEOFF_THR 0.8 #define LOOP_THR 0.6 #define g_rightloopRUD 1500 #define RIGHT_ROLL_SHORT -12.0 #define RIGHT_PITCH_SHORT -5.0 #define LEFT_ROLL_SHORT 12.0 #define LEFT_PITCH_SHORT -5.0 #define rightloopRUD 1300 //1250 #define AIL_R_correctionrightloop 0 #define AIL_L_correctionrightloop -400 #define rightloopshortRUD 1250 #define AIL_L_correctionrightloopshort 0 #define leftloopRUD 1500 #define AIL_L_correctionleftloop -0 #define leftloopshortRUD 1500 #define AIL_L_correctionleftloopshort 0 #define glideloopRUD 1300 #define RIGHTLOOP_RUD 1250 #define RIGHTLOOPSHORT_RUD 1250 #define LEFTLOOP_RUD 1500 #define LEFTLOOPSHORT_RUD 1500 #define GLIDELOOP_RUD 1300 #define AIL_L_CORRECTION_RIGHTLOOP 0 #define AIL_L_CORRECTION_RIGHTLOOPSHORT 0 #define AIL_L_CORRECTION_LEFTLOOP 0 #define AIL_L_CORRECTION_LEFTLOOPSHORT 0 #define GLIDE_ROLL -12.0 #define GLIDE_PITCH -3.0 #define AIL_L_RatioRising 0.5 #define AIL_L_RatioDescent 2 //コンパスキャリブレーション //SkipperS2基板 /* #define MAGBIAS_X -35.0 #define MAGBIAS_Y 535.0 #define MAGBIAS_Z -50.0 */ //S2v2 1番基板 #define MAGBIAS_X 395.0 #define MAGBIAS_Y 505.0 #define MAGBIAS_Z -725.0 //S2v2 2番基板 /* #define MAGBIAS_X 185.0 #define MAGBIAS_Y 220.0 #define MAGBIAS_Z -350.0 */ #define ELEMENT 1 #define LIMIT_STRAIGHT_YAW 5.0 #define THRESHOLD_TURNINGRADIUS_YAW 60.0 #define ALLOWHEIGHT 15 #ifndef PI #define PI 3.14159265358979 #endif const int16_t lengthdivpwm = 320; const int16_t changeModeCount = 6; const float trim[4] = {Trim_Falfalla[0],Trim_Falfalla[1],Trim_Falfalla[2],Trim_Falfalla[3]}; const float expMax[4] = {ExpMax_Falfalla[0],ExpMax_Falfalla[1],ExpMax_Falfalla[2],ExpMax_Falfalla[3]}; const float expMin[4] = {ExpMin_Falfalla[0],ExpMin_Falfalla[1],ExpMin_Falfalla[2],ExpMin_Falfalla[3]}; const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; SBUS sbus(PA_9, PA_10); //SBUS PwmOut servo1(PC_6); // TIM3_CH1 //old echo PwmOut servo2(PC_7); // TIM3_CH2 //PC_7 PwmOut servo3(PB_0); // TIM3_CH3 PwmOut servo4(PB_1); // TIM3_CH4 PwmOut servo5(PB_6); // TIM4_CH1 PwmOut servo6(PB_7); // TIM4_CH2 //old trigger //PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo //PwmOut servo8(PB_9); // TIM4_CH4 //new trigger RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX //RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用 SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); DigitalOut led1(PA_0); //黄色のコネクタ DigitalOut led2(PA_1); DigitalOut led3(PB_4); DigitalOut led4(PB_5); //InterruptIn switch2(PC_14); MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン,シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine //HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL); PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD); enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8}; enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度 enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide}; enum BombingMode{Takeoff, Chicken, Transition, Approach}; enum OutputStatus{Manual, Auto}; static OutputStatus output_status = Manual; OperationMode operation_mode = StartUp; BombingMode bombing_mode = Takeoff; static int16_t autopwm[8] = {1500,1500,1180,1500,1392,1500}; static int16_t trimpwm[6] = {1500,1500,1180,1500,1392,1600}; int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820}; int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180}; int16_t oldTHR = 1000; int16_t g_AIL_L_Ratio_rightloop = 0.5; static float nowAngle[3] = {0,0,0}; const float trimAngle[3] = {0.0, 0.0, 0.0}; const float maxAngle[2] = {90, 90}; const float minAngle[2] = {-90, -90}; float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; unsigned int g_distance; //Ticker USsensor; static char g_buf[16]; char g_landingcommand='Z'; float g_SerialTargetYAW; Timer t; Timeout RerurnChickenServo1; Timeout RerurnChickenServo2; /*-----関数のプロトタイプ宣言-----*/ void setup(); void loop(); void Init_PWM(); void Init_servo(); //サーボ初期化 void Init_sbus(); //SBUS初期化 void Init_sensors(); void DisplayClock(); //クロック状態確認 //センサの値取得 void SensingMPU(); void UpdateDist(); //void offsetRollPitch(float FirstROLL, float FirstPITCH); //void TransYaw(float FirstYAW); float TranslateNewYaw(float beforeYaw, float newzeroYaw); void UpdateTargetAngle(float targetAngle[3]); void CalculateControlValue(float targetAngle[3], float controlValue[3]); void UpdateAutoPWM(float controlValue[3]); void ConvertPWMintoRAD(float targetAngle[3]); inline float CalcRatio(float value, float trim, float limit); bool CheckSW_Up(Channel ch); int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min); inline int16_t SetTHRinRatio(float ratio); //sbus割り込み void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力 //シリアル割り込み void SendSerial(); //1文字きたら送り返す void SendArray(); void getSF_Serial(); float ConvertByteintoFloat(char high, char low); void ISR_Serial_Rx(); //SD設定 int GetParameter(FILE *fp, const char *paramName,char parameter[]); int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, float *g_rightloopROLL, float *g_rightloopPITCH, float *g_leftloopROLL, float *g_leftloopPITCH, float *g_gostraightROLL, float *g_gostraightPITCH, float *g_takeoffTHR, float *g_loopTHR, float *g_rightloopROLLshort, float *g_rightloopPITCHshort, float *g_leftloopROLLshort, float *g_leftloopPITCHshort, float *g_glideloopROLL, float *g_glideloopPITCH, float *g_kpAIL, float *g_kiAIL, float *g_kdAIL); //switch2割り込み void ResetTrim(); //自動操縦 void UpdateTargetAngle_GoStraight(float targetAngle[3]); void UpdateTargetAngle_Rightloop(float targetAngle[3]); void UpdateTargetAngle_Rightloop_short(float targetAngle[3]); void UpdateTargetAngle_Leftloop(float targetAngle[3]); void UpdateTargetAngle_Leftloop_short(float targetAngle[3]); void UpdateTargetAngle_Moebius(float targetAngle[3]); void UpdateTargetAngle_Glide(float targetAngle[3]); void UpdateTargetAngle_Takeoff(float targetAngle[3]); void UpdateTargetAngle_Approach(float targetAngle[3]); void Take_off_and_landing(float targetAngle[3]); int Rotate(float targetAngle[3], float TargetYAW); //投下 void Chicken_Drop(); void ReturnChickenServo1(); void ReturnChickenServo2(); //超音波による高度補正 void checkHeight(float targetAngle[3]); void UpdateTargetAngle_NoseUP(float targetAngle[3]); void UpdateTargetAngle_NoseDOWN(float targetAngle[3]); //デバッグ用 void DebugPrint(); /*---関数のプロトタイプ宣言終わり---*/ int main() { setup(); while(1){ loop(); if(!CheckSW_Up(Ch7)){ led3=0; }else{ led3=1; } } } void setup(){ //buzzer = 0; led1 = 1; led2 = 1; led3 = 1; led4 = 1; SetOptions(&g_kpELE, &g_kiELE, &g_kdELE, &g_kpRUD, &g_kiRUD, &g_kdRUD, &g_rightloopROLL, &g_rightloopPITCH, &g_leftloopROLL, &g_leftloopPITCH, &g_gostraightROLL, &g_gostraightPITCH, &g_takeoffTHR, &g_loopTHR, &g_rightloopROLLshort, &g_rightloopPITCHshort, &g_leftloopROLLshort, &g_leftloopPITCHshort, &g_glideloopROLL, &g_glideloopPITCH, &g_kpAIL, &g_kiAIL,&g_kdAIL); Init_PWM(); Init_servo(); Init_sbus(); Init_sensors(); //switch2.rise(ResetTrim); pc.attach(getSF_Serial, Serial::RxIrq); //USsensor.attach(&UpdateDist, 0.05); NVIC_SetPriority(USART1_IRQn,0); NVIC_SetPriority(EXTI0_IRQn,1); NVIC_SetPriority(TIM5_IRQn,2); NVIC_SetPriority(EXTI9_5_IRQn,3); NVIC_SetPriority(USART2_IRQn,4); DisplayClock(); t.start(); pc.printf("MPU calibration start\r\n"); float offsetstart = t.read(); while(t.read() - offsetstart < 26){ SensingMPU(); for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); pc.printf("\r\n"); led1 = !led1; led2 = !led2; led3 = !led3; led4 = !led4; } FirstROLL = nowAngle[ROLL]; FirstPITCH = nowAngle[PITCH]; nowAngle[ROLL] -=FirstROLL; nowAngle[PITCH] -=FirstPITCH; led1 = 0; led2 = 0; led3 = 0; led4 = 0; wait(0.2); pc.printf("All initialized\r\n"); } void loop(){ static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; SensingMPU(); NVIC_DisableIRQ(USART2_IRQn); UpdateTargetAngle(targetAngle); //Rotate(targetAngle, 30.0); CalculateControlValue(targetAngle, controlValue); NVIC_DisableIRQ(USART1_IRQn); UpdateAutoPWM(controlValue); NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); wait_ms(23); pc.printf("%c",g_landingcommand); #if DEBUG_PRINT_INLOOP DebugPrint(); #endif } //サーボ初期化関数 void Init_servo(){ servo1.period_ms(14); servo1.pulsewidth_us(trimpwm[AIL_R]); servo2.period_ms(14); servo2.pulsewidth_us(trimpwm[ELE]); servo3.period_ms(14); servo3.pulsewidth_us(trimpwm[THR]); servo4.period_ms(14); servo4.pulsewidth_us(trimpwm[RUD]); servo5.period_ms(14); servo5.pulsewidth_us(1392); servo6.period_ms(14); servo6.pulsewidth_us(trimpwm[AIL_L]); pc.printf("servo initialized\r\n"); } //Sbus初期化 void Init_sbus(){ sbus.initialize(); sbus.setLastfuncPoint(Update_PWM); sbus.startInterrupt(); } void Init_sensors(){ if(mpu6050.setup() == -1){ pc.printf("failed initialize\r\n"); while(1){ led1 = 1; led2 = 0; led3 = 1; led4 = 0; wait(1); led1 = 0; led2 = 1; led3 = 0; led4 = 1; wait(1); } } } void Init_PWM(){ for (uint8_t i = 0; i < 6; ++i){ trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100)); maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100)); minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100)); } pc.printf("PWM initialized\r\n"); } void DisplayClock(){ pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000); pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000); pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000); pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000); pc.printf("\r\n"); } void UpdateTargetAngle(float targetAngle[3]){ static int16_t count_op = 0; #if DEBUG_SEMIAUTO switch(operation_mode){ case StartUp: if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = SemiAuto; pc.printf("Goto SemiAuto mode\r\n"); count_op = 0; } }else count_op = 0; break; case SemiAuto: /* 大会用では以下のif文を入れてoperation_modeを変える if(CheckSW_Up(Ch6)){ count_op++; if(count_op>changeModeCount){ output_status = XXX; led2 = 0; pc.printf("Goto XXX mode\r\n"); count_op = 0; }else count_op = 0; ConvertPWMintoRAD(targetAngle); } */ ConvertPWMintoRAD(targetAngle); break; default: operation_mode = SemiAuto; break; } #else switch(operation_mode){ case StartUp: if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え ch8;自動操縦モード切替 count_op++; if(count_op > changeModeCount){ operation_mode = RightLoop; pc.printf("Goto RightLoop mode\r\n"); count_op = 0; } }else count_op = 0; break; case RightLoop: if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = LeftLoop; pc.printf("Goto LeftLoop mode\r\n"); count_op = 0; } }else count_op = 0; UpdateTargetAngle_Rightloop(targetAngle); break; case LeftLoop: if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = GoStraight; pc.printf("Goto GoStraight mode\r\n"); count_op = 0; } }else count_op = 0; UpdateTargetAngle_Leftloop(targetAngle); break; case GoStraight: if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = Moebius; pc.printf("Goto Moebius mode\r\n"); count_op = 0; } }else count_op = 0; UpdateTargetAngle_GoStraight(targetAngle); break; case Moebius: if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = Glide; pc.printf("Goto Glide mode\r\n"); count_op = 0; } }else count_op = 0; UpdateTargetAngle_Moebius(targetAngle); break; case Glide: if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = BombwithPC; pc.printf("Goto Bombing mode\r\n"); count_op = 0; } }else count_op = 0; UpdateTargetAngle_Glide(targetAngle); break; case BombwithPC: if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = RightLoop; pc.printf("Goto RightLoop mode\r\n"); count_op = 0; } }else count_op = 0; Take_off_and_landing(targetAngle); break; default: operation_mode = StartUp; break; } #endif if(CheckSW_Up(Ch7)){ output_status = Auto; led1 = 1; }else{ output_status = Manual; led1 = 0; } } int GetParameter(FILE *fp, const char *paramName,char parameter[]){ int i=0, j=0; int strmax = 200; char str[strmax]; rewind(fp); //ファイル位置を先頭に while(1){ if (fgets(str, strmax, fp) == NULL) { return 0; } if (!strncmp(str, paramName, strlen(paramName))) { while (str[i++] != '=') {} while (str[i] != '\n') { parameter[j++] = str[i++]; } parameter[j] = '\0'; return 1; } } return 0; } //sdによる設定 int SetOptions(float *g_kpELE, float *g_kiELE, float *g_kdELE, float *g_kpRUD, float *g_kiRUD, float *g_kdRUD, float *g_rightloopROLL, float *g_rightloopPITCH, float *g_leftloopROLL, float *g_leftloopPITCH, float *g_gostraightROLL, float *g_gostraightPITCH, float *g_takeoffTHR, float *g_loopTHR, float *g_rightloopROLLshort, float *g_rightloopPITCHshort, float *g_leftloopROLLshort, float *g_leftloopPITCHshort, float *g_glideloopROLL, float *g_glideloopPITCH, float *g_kpAIL, float *g_kiAIL, float *g_kdAIL ){ pc.printf("SDsetup start.\r\n"); FILE *fp; char parameter[30]; //文字列渡す用の配列 int SDerrorcount = 0; //取得できなかった数を返す const char *paramNames[] = { "KP_ELEVATOR", "KI_ELEVATOR", "KD_ELEVATOR", "KP_RUDDER", "KI_RUDDER", "KD_RUDDER", "RIGHTLOOP_ROLL", "RIGHTLOOP_PITCH", "LEFTLOOP_ROLL", "LEFTLOOP_PITCH", "GOSTRAIGHT_ROLL", "GOSTRAIGHT_PITCH", "TAKEOFF_THR_RATE", "LOOP_THR_RATE", "RIGHTLOOP_ROLL_SHORT", "RIGHTLOOP_PITCH_SHORT", "LEFTLOOP_ROLL_SHORT", "LEFTLOOP_PITCH_SHORT", "AUTOGLIDE_ROLL", "AUTOGLIDE PITCH", "KP_AILERON", "KI_AILERON", "KD_AILERON" }; fp = fopen("/sd/option.txt","r"); if(fp != NULL){ //開けたら pc.printf("File was openned.\r\n"); if(GetParameter(fp,paramNames[0],parameter)) *g_kpELE = atof(parameter); else{ *g_kpELE = KP_ELE; SDerrorcount++; } if(GetParameter(fp,paramNames[1],parameter)) *g_kiELE = atof(parameter); else{ *g_kiELE = KI_ELE; SDerrorcount++; } if(GetParameter(fp,paramNames[2],parameter)) *g_kdELE = atof(parameter); else{ *g_kdELE = KD_ELE; SDerrorcount++; } if(GetParameter(fp,paramNames[3],parameter)) *g_kpRUD = atof(parameter); else{ *g_kpRUD = KP_RUD; SDerrorcount++; } if(GetParameter(fp,paramNames[4],parameter)) *g_kiRUD = atof(parameter); else{ *g_kiRUD = KI_RUD; SDerrorcount++; } if(GetParameter(fp,paramNames[5],parameter)) *g_kdRUD = atof(parameter); else{ *g_kdRUD = KD_RUD; SDerrorcount++; } if(GetParameter(fp,paramNames[6],parameter)) *g_rightloopROLL = atof(parameter); else{ *g_rightloopROLL = RIGHT_ROLL; SDerrorcount++; } if(GetParameter(fp,paramNames[7],parameter)) *g_rightloopPITCH = atof(parameter); else{ *g_rightloopPITCH = RIGHT_PITCH; SDerrorcount++; } if(GetParameter(fp,paramNames[8],parameter)) *g_leftloopROLL = atof(parameter); else{ *g_leftloopROLL = LEFT_ROLL; SDerrorcount++; } if(GetParameter(fp,paramNames[9],parameter)) *g_leftloopPITCH = atof(parameter); else{ *g_leftloopPITCH = LEFT_PITCH; SDerrorcount++; } if(GetParameter(fp,paramNames[10],parameter)) *g_gostraightROLL = atof(parameter); else{ *g_gostraightROLL = STRAIGHT_ROLL; SDerrorcount++; } if(GetParameter(fp,paramNames[11],parameter)) *g_gostraightPITCH = atof(parameter); else{ *g_gostraightPITCH = STRAIGHT_PITCH; SDerrorcount++; } if(GetParameter(fp,paramNames[12],parameter)) *g_takeoffTHR = atof(parameter); else{ *g_takeoffTHR = TAKEOFF_THR; SDerrorcount++; } if(GetParameter(fp,paramNames[13],parameter)) *g_loopTHR = atof(parameter); else{ *g_loopTHR = LOOP_THR; SDerrorcount++; } if(GetParameter(fp,paramNames[14],parameter)) *g_rightloopROLLshort = atof(parameter); else{ *g_rightloopROLLshort = RIGHT_ROLL_SHORT; SDerrorcount++; } if(GetParameter(fp,paramNames[15],parameter)) *g_rightloopPITCHshort = atof(parameter); else{ *g_rightloopPITCHshort = RIGHT_PITCH_SHORT; SDerrorcount++; } if(GetParameter(fp,paramNames[16],parameter)) *g_leftloopROLLshort = atof(parameter); else{ *g_leftloopROLLshort = LEFT_ROLL_SHORT; SDerrorcount++; } if(GetParameter(fp,paramNames[17],parameter)) *g_leftloopPITCHshort = atof(parameter); else{ *g_leftloopPITCHshort = LEFT_PITCH_SHORT; SDerrorcount++; } if(GetParameter(fp,paramNames[18],parameter)) *g_glideloopROLL = atof(parameter); else{ *g_glideloopROLL = GLIDE_ROLL; SDerrorcount++; } if(GetParameter(fp,paramNames[19],parameter)) *g_glideloopPITCH = atof(parameter); else{ *g_glideloopPITCH = GLIDE_PITCH; SDerrorcount++; } if(GetParameter(fp,paramNames[20],parameter)) *g_kpAIL = atof(parameter); else{ *g_kpAIL = KP_AIL; SDerrorcount++; } if(GetParameter(fp,paramNames[21],parameter)) *g_kiAIL = atof(parameter); else{ *g_kiAIL = KI_AIL; SDerrorcount++; } if(GetParameter(fp,paramNames[22],parameter)) *g_kdAIL = atof(parameter); else{ *g_kdAIL = KP_AIL; SDerrorcount++; } fclose(fp); }else{ //ファイルがなかったら pc.printf("fp was null.\r\n"); *g_kpELE = KP_ELE; *g_kiELE = KI_ELE; *g_kdELE = KD_ELE; *g_kpRUD = KP_RUD; *g_kiRUD = KI_RUD; *g_kdRUD = KD_RUD; *g_rightloopROLL = RIGHT_ROLL; *g_rightloopPITCH = RIGHT_PITCH; *g_leftloopROLL = LEFT_ROLL; *g_leftloopPITCH = LEFT_PITCH; *g_gostraightROLL = STRAIGHT_ROLL; *g_gostraightPITCH = STRAIGHT_PITCH; *g_takeoffTHR = TAKEOFF_THR; *g_loopTHR = LOOP_THR; *g_kpAIL = KP_AIL; //パラメータ変えるのお忘れなく!! *g_kiAIL = KI_AIL; *g_kdAIL = KD_AIL; SDerrorcount = -1; } pc.printf("SDsetup finished.\r\n"); if(SDerrorcount == 0) pc.printf("setting option is success\r\n"); else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n"); else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount); pc.printf("kpELE = %f, kiELE = %f, kdELE = %f\r\n", *g_kpRUD, *g_kiRUD, *g_kdRUD); pc.printf("kpRUD = %f, kiRUD = %f, kdRUD = %f\r\n", *g_kpELE, *g_kiELE, *g_kdELE); pc.printf("rightloopROLL = %f, rightloopPITCH = %f\r\n", *g_rightloopROLL, *g_rightloopPITCH); pc.printf("leftloopROLL = %f, g_leftloopPITCH = %f\r\n", *g_leftloopROLL, *g_leftloopPITCH); pc.printf("gostraightROLL = %f, g_gostraightPITCH = %f\r\n", *g_gostraightROLL, *g_gostraightPITCH); pc.printf("g_takeoffTHR = %f, g_loopTHR = %f\r\n", *g_takeoffTHR, *g_loopTHR); pc.printf("rightloopROLLshort = %f, rightloopPITCHshort = %f\r\n", *g_rightloopROLLshort, *g_rightloopPITCHshort); pc.printf("leftloopROLLshort = %f, g_leftloopPITCHshort = %f\r\n", *g_leftloopROLLshort, *g_leftloopPITCHshort); pc.printf("glideROLL = %f, glidePITCH = %f\r\n", *g_glideloopROLL, *g_glideloopPITCH); return SDerrorcount; } void CalculateControlValue(float targetAngle[3], float controlValue[3]){ static int t_last; int t_now; float dt; t_now = t.read_us(); dt = (float)((t_now - t_last)/1000000.0f) ; t_last = t_now; //controlValue[ROLL] = pid_RUD.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); controlValue[ROLL] = pid_AIL.calcPID(nowAngle[ROLL], targetAngle[ROLL], dt); //エルロンでロール制御 controlValue[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); } void UpdateAutoPWM(float controlValue[3]){ int16_t addpwm[2]; //-500~500 addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ:機首下げ正 レバー:機首上げ正 addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ:右回転正(8月13日時点;左回転が正!) レバー:右回転正 autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH]; //rewrite autopwm[AIL_R] = trimpwm[AIL_R] - reverce[AIL_R] * addpwm[ROLL]; //autopwm[THR] = oldTHR; autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]); } inline float CalcRatio(float value, float trim, float limit){ return (value - trim) / (limit - trim); } bool CheckSW_Up(Channel ch){ if(SWITCH_CHECK < sbus.manualpwm[ch]){ return true; }else{ return false; } } int16_t ThresholdMaxMin(int16_t value, int16_t max, int16_t min){ if(value > max) return max; if(value < min) return min; return value; } inline int16_t SetTHRinRatio(float ratio){ return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); } /*---SBUS割り込み処理---*/ //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) void Update_PWM() { static int16_t pwm[6]; static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]}; if(sbus.flg_ch_update == true){ NVIC_DisableIRQ(USART1_IRQn); switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 case Manual: for(uint8_t i=0;i<6;i++){ pwm[i] = sbus.manualpwm[i]; } /*pc.printf("%d ,",pwm[0]);//R pc.printf("%d ,\r\n",pwm[5]);//L*/ oldTHR = sbus.manualpwm[THR]; //pc.printf("update_manual\r\n"); break; case Auto: pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL]; pwm[ELE] = autopwm[ELE]; pwm[THR] = autopwm[THR]; pwm[RUD] = autopwm[RUD]; pwm[DROP] = autopwm[DROP]; pwm[AIL_L] = autopwm[AIL_L]; //pc.printf("%d ,",pwm[AIL_R]);//R //pc.printf("%d ,\r\n",pwm[AIL_L]);//L //pc.printf("update_auto\r\n"); break; default: for(uint8_t i=0;i<6;i++){ pwm[i] = sbus.manualpwm[i]; } //pc.printf("update_manual\r\n"); break; NVIC_EnableIRQ(USART1_IRQn); } for(uint8_t i=0;i<6;i++){ if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; temppwm[i]=pwm[i]; } }else{ pc.printf("0\r\n"); } sbus.flg_ch_update = false; Output_PWM(pwm); } //pwmをサーボに出力。 void Output_PWM(int16_t pwm[5]) { NVIC_DisableIRQ(USART1_IRQn); servo1.pulsewidth_us(pwm[0]); servo2.pulsewidth_us(pwm[1]); servo3.pulsewidth_us(pwm[2]); servo4.pulsewidth_us(pwm[3]); servo5.pulsewidth_us(pwm[4]); servo6.pulsewidth_us(pwm[5]); NVIC_EnableIRQ(USART1_IRQn); } void ResetTrim(){ for(uint8_t i=0; i<6; i++){ //i=4から書き換え_投下サーボは入ってない模様 trimpwm[i] = sbus.manualpwm[i]; } pc.printf("reset PWM trim\r\n"); } void SensingMPU(){ //static int16_t deltaT = 0, t_start = 0; //t_start = t.read_us(); float rpy[3] = {0}, oldrpy[3] = {0}; static uint16_t count_changeRPY = 0; static bool flg_checkoutlier = false; NVIC_DisableIRQ(USART1_IRQn); NVIC_DisableIRQ(USART2_IRQn); NVIC_DisableIRQ(TIM5_IRQn); NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止 NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止 mpu6050.getRollPitchYaw_Skipper(rpy); NVIC_EnableIRQ(USART1_IRQn); NVIC_EnableIRQ(USART2_IRQn); NVIC_EnableIRQ(TIM5_IRQn); NVIC_EnableIRQ(EXTI0_IRQn); NVIC_EnableIRQ(EXTI9_5_IRQn); //外れ値対策 for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI; rpy[ROLL] -= FirstROLL; rpy[PITCH] -= FirstPITCH; rpy[YAW] -= FirstYAW; for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}} if(!flg_checkoutlier || count_changeRPY >= 2){ for(uint8_t i=0; i<3; i++){ nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均 } count_changeRPY = 0; }else count_changeRPY++; flg_checkoutlier = false; } float TranslateNewYaw(float beforeYaw, float newzeroYaw){ float newYaw = beforeYaw - newzeroYaw; if(newYaw<-180.0f) newYaw += 360.0f; else if(newYaw>180.0f) newYaw -= 360.0f; return newYaw; } void getSF_Serial(){ static char SFbuf[16]; static int bufcounter=0; SFbuf[bufcounter]=pc.getc(); pc.printf("%c",SFbuf[bufcounter]); if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++; if(bufcounter==5 && SFbuf[4]=='F'){ g_landingcommand = SFbuf[1]; wait_ms(5); if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]); bufcounter = 0; memset(SFbuf, 0, strlen(SFbuf)); //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); } else if(bufcounter>=5 ){ pc.printf("Communication Falsed.\r\n"); bufcounter = 0; } } float ConvertByteintoFloat(char high, char low){ //int16_t intvalue = (int16_t)high*256 + (int16_t)low; int16_t intvalue = (int16_t)(((int16_t)high << 8) | low); // Turn the MSB and LSB into a signed 16-bit value float floatvalue = (float)intvalue; return floatvalue; } //超音波割り込み /*void UpdateDist(){ g_distance = usensor.get_dist_cm(); usensor.start(); }*/ //8の字旋回 void UpdateTargetAngle_Moebius(float targetAngle[3]){ static uint8_t RotateCounter=0; static bool flg_setInStartAuto = false; static float FirstYAW_Moebius = 0.0; float newYaw_Moebius; if(!flg_setInStartAuto && CheckSW_Up(Ch7)){ FirstYAW_Moebius = nowAngle[YAW]; RotateCounter = 0; flg_setInStartAuto = true; }else if(!CheckSW_Up(Ch7)){ flg_setInStartAuto = false; led2 = 0; } autopwm[THR]=oldTHR; newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n");} if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n");} if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-10.0) {RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n");} if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led2 = 0; pc.printf("Change Rotate direction\r\n");} if(RotateCounter <= 3) UpdateTargetAngle_Rightloop_short(targetAngle); else UpdateTargetAngle_Leftloop_short(targetAngle); //左旋回 } //自動滑空 void UpdateTargetAngle_Glide(float targetAngle[3]){ static int THRcount = 0; static int t_start = 0; static bool flg_tstart = false; int t_diff = 0; static int groundcount = 0; targetAngle[ROLL] = g_glideloopROLL; targetAngle[PITCH] = g_glideloopPITCH; // autopwm[THR]=oldTHR; //時間計測開始設定 if(!flg_tstart && CheckSW_Up(Ch7)){ t_start = t.read(); flg_tstart = true; pc.printf("timer start\r\n"); }else if(!CheckSW_Up(Ch7)){ t_start = 0; flg_tstart = false; } //フラグが偽であれば計測は行わない if(flg_tstart){ t_diff = t.read() - t_start; //一定高度or15秒でled点灯 NVIC_DisableIRQ(EXTI9_5_IRQn); if((groundcount>5 && g_distance>0) || t_diff > 15){ led2 = 1; //pc.printf("Call [Stop!] calling!\r\n"); } if(g_distance<180 && g_distance > 0) groundcount++; NVIC_EnableIRQ(EXTI9_5_IRQn); }else{ t_diff = 0; groundcount = 0; led2 = 0; } if(t_diff > 17){ autopwm[THR] = SetTHRinRatio(0.5); }else{ NVIC_DisableIRQ(EXTI9_5_IRQn); if(g_distance<150 && g_distance>0 ){ NVIC_EnableIRQ(EXTI9_5_IRQn); THRcount++; if(THRcount>5){ autopwm[THR] = SetTHRinRatio(0.6); //pc.printf("throttle ON\r\n"); } }else{ autopwm[THR] = 1180; THRcount = 0; } } } //離陸-投下-着陸一連 void Take_off_and_landing(float targetAngle[3]){ /* if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff; switch(bombing_mode){ case Takeoff: static bool flg_setFirstYaw = false; static int TakeoffCount = 0; if(!flg_setFirstYaw && CheckSW_Up(Ch7)){ FirstYAW = nowAngle[YAW]; flg_setFirstYaw = true; }else if(flg_setFirstYaw && !CheckSW_Up(Ch7)){ flg_setFirstYaw = false; } UpdateTargetAngle_Takeoff(targetAngle); if(g_distance>150) TakeoffCount++; else TakeoffCount = 0; if(TakeoffCount>5){ autopwm[THR] = 1180+320*2*0.5; pc.printf("Now go to Approach mode!!"); bombing_mode = Approach; } break; //case Chicken: //break; case Transition: static int ApproachCount = 0; targetAngle[YAW]=180.0; int Judge = Rotate(targetAngle, g_SerialTargetYAW); if(Judge==0) ApproachCount++; if(ApproachCount>5) bombing_mode = Approach; break; case Approach: */ autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合 UpdateTargetAngle_Approach(targetAngle); /* break; default: bombing_mode = Takeoff; break; } */ } //離陸モード void UpdateTargetAngle_Takeoff(float targetAngle[3]){ //pc.printf("%d \r\n",g_distance); targetAngle[ROLL] = g_gostraightROLL; targetAngle[PITCH] = g_loopTHR; autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 } //ヨーを目標値にして許容角度になるまで水平旋回 int Rotate(float targetAngle[3], float TargetYAW){ float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); if(diffYaw > LIMIT_STRAIGHT_YAW){ /* if(diffYaw > THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Rightloop_short(targetAngle); else UpdateTargetAngle_Rightloop(targetAngle); */ UpdateTargetAngle_Rightloop_short(targetAngle); return 1; }else if(diffYaw < -LIMIT_STRAIGHT_YAW){ UpdateTargetAngle_Leftloop_short(targetAngle); /* if(diffYaw < -THRESHOLD_TURNINGRADIUS_YAW) UpdateTargetAngle_Leftloop_short(targetAngle); else UpdateTargetAngle_Leftloop(targetAngle); */ return 1; }else{ UpdateTargetAngle_GoStraight(targetAngle); return 0; } } //チキラー投下 void Chicken_Drop(){ if(CheckSW_Up(Ch7)){ autopwm[DROP] = 1512; pc.printf("Bombed!\r\n"); RerurnChickenServo1.attach(&ReturnChickenServo1, 3); //operation_mode = Approach; //buzzer = 0; pc.printf("Goto LeftLoop mode\r\n"); } } void ReturnChickenServo1(){ autopwm[DROP] = 1344; pc.printf("first reverse\r\n"); RerurnChickenServo2.attach(&ReturnChickenServo2, 1); } void ReturnChickenServo2(){ autopwm[DROP] = 1392; pc.printf("second reverse\r\n"); } //着陸モード(PCからの指令に従う) void UpdateTargetAngle_Approach(float targetAngle[3]){ NVIC_DisableIRQ(USART2_IRQn); switch(g_landingcommand){ case 'R': //右旋回セヨ targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; autopwm[RUD]=rightloopRUD; //RUD固定 if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; break; case 'L': //左旋回セヨ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; autopwm[RUD]=leftloopRUD; if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; break; case 'G': //直進セヨ targetAngle[ROLL] = g_gostraightROLL; targetAngle[PITCH] = g_gostraightPITCH; break; case 'Y': //指定ノヨー方向ニ移動セヨ Rotate(targetAngle, g_SerialTargetYAW); break; case 'B': //ブザーヲ鳴ラセ //buzzer = 1; break; case 'D': //物資ヲ落トセ Chicken_Drop(); break; case 'C': //停止セヨ targetAngle[ROLL] = 0.0; targetAngle[PITCH] = -3.0; autopwm[THR] = minpwm[THR]; break; } NVIC_EnableIRQ(USART2_IRQn); } void checkHeight(float targetAngle[3]){ static int targetHeight = 200; NVIC_DisableIRQ(EXTI9_5_IRQn); if(g_distance < targetHeight + ALLOWHEIGHT){ UpdateTargetAngle_NoseUP(targetAngle); if(CheckSW_Up(Ch7)) led2 = 1; } else if(g_distance > targetHeight - ALLOWHEIGHT){ UpdateTargetAngle_NoseDOWN(targetAngle); if(CheckSW_Up(Ch7)) led2 = 1; } else led2=0; NVIC_EnableIRQ(EXTI9_5_IRQn); } void UpdateTargetAngle_NoseUP(float targetAngle[3]){ //targetAngle[PITCH] += 2.0f; //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6); autopwm[THR] = SetTHRinRatio(g_loopTHR+0.05); //pc.printf("nose UP"); } void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){ //targetAngle[PITCH] -= 2.0f; autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05); //pc.printf("nose DOWN"); } //直進 void UpdateTargetAngle_GoStraight(float targetAngle[3]){ targetAngle[ROLL] = g_gostraightROLL; targetAngle[PITCH] = g_gostraightPITCH; autopwm[THR] = SetTHRinRatio(g_loopTHR); //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } //右旋回 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回 targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; autopwm[RUD]=rightloopRUD; //RUD固定 autopwm[THR] = SetTHRinRatio(0.5); //手動スロットル記憶 if(autopwm[AIL_R]>trimpwm[AIL_R]){ //エルロン上がりやすさ調節 autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop; //checkHeight(targetAngle); //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } //右旋回(着陸時スロットル0の時) void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回 targetAngle[ROLL] = g_rightloopROLL; targetAngle[PITCH] = g_rightloopPITCH ; autopwm[RUD]=rightloopRUD; autopwm[THR] = minpwm[THR];//SetTHRinRatio(g_loopTHR); if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //checkHeight(targetAngle); //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } void UpdateTargetAngle_Rightloop_short(float targetAngle[3]){ //右旋回 targetAngle[ROLL] = g_rightloopROLLshort; targetAngle[PITCH] = g_rightloopPITCHshort; autopwm[RUD]=rightloopshortRUD; autopwm[THR] = oldTHR; if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } //左旋回 void UpdateTargetAngle_Leftloop(float targetAngle[3]){ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; autopwm[RUD]=leftloopRUD; autopwm[THR] = oldTHR; if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //checkHeight(targetAngle); //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); } //左旋回(着陸時スロットル0のとき) void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){ targetAngle[ROLL] = g_leftloopROLL; targetAngle[PITCH] = g_leftloopPITCH; autopwm[RUD]=leftloopRUD; autopwm[THR] = minpwm[THR]; if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_Ratio_leftloop; //checkHeight(targetAngle); //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]); } void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){ targetAngle[ROLL] = g_leftloopROLLshort; targetAngle[PITCH] = g_leftloopPITCHshort; autopwm[RUD]=leftloopRUD; autopwm[THR] = oldTHR; if(autopwm[AIL_R]>trimpwm[AIL_R]){ autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising; } else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloopshort+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent; //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); } //デバッグ用 void DebugPrint(){ /* static int16_t deltaT = 0, t_start = 0; deltaT = t.read_u2s() - t_start; pc.printf("t:%d us, ",deltaT); pc.printf("\r\n"); t_start = t.read_us(); */ //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]); //pc.printf("\r\n"); //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ 小数点以下2ケタ //pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]); //pc.printf("%d",g_distance); //pc.printf("Mode: %c: ",g_buf[0]); //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); //pc.printf("\r\n"); }