s
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of AutoFlight2017_now_copy by
main.cpp
- Committer:
- TUATBM
- Date:
- 2017-08-01
- Revision:
- 0:92024886c0be
- Child:
- 1:f31e17341659
File content as of revision 0:92024886c0be:
#include "mbed.h" //#include "rtos.h" #include "math.h" #include "MPU9250.h" #include "BMP280.h" #include "SkipperSv2.h" #include "Estrela.h" #include "sbus.h" #include "pid.h" #define DEBUG_SEMIAUTO 1 #define DEBUG_PRINT_INLOOP 1 #define KP_ELE 2 #define KI_ELE 0 #define KD_ELE 0 #define KP_RUD 2 #define KI_RUD 0 #define KD_RUD 0 #define GAIN_CONTROLVALUE_TO_PWM 3 #define MAGBIAS_X 185.0 #define MAGBIAS_Y 220.0 #define MAGBIAS_Z -350.0 const uint16_t changeModeCount = 50; const int16_t lengthdivpwm = 320; const int16_t trim[4] = {0,14,-100,-10}; const float expMax[4] = {100,115,100,89}; const float expMin[4] = {100,47,100,110}; SBUS sbus(PA_9, PA_10); //SBUS PwmOut servo1(PC_6); // TIM3_CH1 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 PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 PwmOut servo8(PB_9); // TIM4_CH4 Serial pc(PA_2, PA_3); DigitalOut led1(PA_0); //緑 DigitalOut led2(PA_1); //赤 DigitalOut led3(PB_4); DigitalOut led4(PB_5); InterruptIn switch2(PC_14); MPU9250 mpu9250(PC_9,PA_8,&pc); PID pid_ELE(KP_ELE,KI_ELE,KD_ELE); PID pid_RUD(KP_RUD,KI_RUD,KD_RUD); enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8}; enum Angle{ROLL, PITCH, YAW}; enum OperationMode{StartUp, SemiAuto}; enum OutputStatus{Manual, Auto}; static OutputStatus output_status = Manual; OperationMode operation_mode = StartUp; static int16_t autopwm[8] = {1500,1500,1000,1500}; static int16_t trimpwm[4] = {1500,1500,1000,1500}; int16_t maxpwm[4] = {1820,1820,1820,1820}; int16_t minpwm[4] = {1180,1180,1180,1180}; int16_t oldTHR = 1000; static float nowAngle[3] = {0,0,0}; const float trimAngle[3] = {0,0,0}; const float maxAngle[2] = {90, 90}; const float minAngle[2] = {-90, -90}; Timer t; //IWDG_HandleTypeDef hiwdg; /*-----関数のプロトタイプ宣言-----*/ void setup(); void loop(); void Init_PWM(); void Init_servo(); //サーボ初期化 void Init_sbus(); //SBUS初期化 void Init_sensors(); void DisplayClock(); //クロック状態確認 void SensingMPU(); 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); //sbus割り込み void Update_PWM(); //マニュアル・自動モードのpwmデータを整形しpwm変数に入力 void Output_PWM(int16_t pwm[8]); //pwmをサーボへ出力 //switch2割り込み void ResetTrim(); //デバッグ用 void DebugPrint(); /*---関数のプロトタイプ宣言終わり---*/ int main() { setup(); while(1){ loop(); } } void setup(){ led1 = 1; led2 = 1; led3 = 0; led4 = 0; pc.baud(115200); Init_PWM(); Init_servo(); Init_sbus(); Init_sensors(); switch2.rise(ResetTrim); t.start(); DisplayClock(); led1 = 0; led2 = 0; led3 = 0; led4 = 0; pc.printf("All initialized\r\n"); mpu9250.sensingAcGyMg(); mpu9250.calculatePostureAngle(nowAngle); } void loop(){ static float targetAngle[3], controlValue[2]; SensingMPU(); UpdateTargetAngle(targetAngle); CalculateControlValue(targetAngle, controlValue); UpdateAutoPWM(controlValue); #if DEBUG_PRINT_INLOOP DebugPrint(); #endif } //サーボ初期化関数 void Init_servo() { servo1.period_ms(14); servo1.pulsewidth_us(trimpwm[AIL]); 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(1500); servo6.period_ms(14); servo6.pulsewidth_us(1500); servo7.period_ms(14); servo7.pulsewidth_us(1500); servo8.period_ms(14); servo8.pulsewidth_us(1500); pc.printf("servo initialized\r\n"); } //Sbusを初期化する関数 void Init_sbus(){ sbus.initialize(); sbus.setLastfuncPoint(Update_PWM); sbus.startInterrupt(); } void Init_sensors(){ if(!mpu9250.Initialize()){ pc.printf("failed initialize\r\n"); while(1); } mpu9250.setMagBias(MAGBIAS_X,MAGBIAS_Y,MAGBIAS_Z); } void Init_PWM(){ for (uint8_t i = 0; i < 4; ++i){ trimpwm[i] = 1500 + trim[i]; 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 uint16_t count_op = 0, count_out = 0; #if DEBUG_SEMIAUTO switch(operation_mode){ case StartUp: if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){ 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; } if(CheckSW_Up(Ch5)){ output_status = Auto; led1 = 1; }else{ output_status = Manual; led1 = 0; } #endif } 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[PITCH] = pid_ELE.calcPID(nowAngle[PITCH], targetAngle[PITCH], dt); } void UpdateAutoPWM(float controlValue[3]){ int16_t addpwm[2]; //-500~500 addpwm[ROLL] = GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL]; //センサ:右回転正 レバー:右回転正 addpwm[PITCH] = -1 * GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH]; //センサ:機首下げ正 レバー:機首上げ正 autopwm[ELE] = trimpwm[ELE] + addpwm[PITCH]; autopwm[RUD] = trimpwm[RUD] + addpwm[ROLL]; autopwm[THR] = oldTHR; autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]); autopwm[RUD] = ThresholdMaxMin(autopwm[RUD], maxpwm[RUD], minpwm[RUD]); } void ConvertPWMintoRAD(float targetAngle[3]){ float ratio[2]; if(sbus.manualpwm[ELE] > trimpwm[ELE]){ ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)maxpwm[ELE]); targetAngle[PITCH] = ratio[0]*(maxAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH]; }else{ ratio[0] = CalcRatio((float)sbus.manualpwm[ELE], (float)trimpwm[ELE], (float)minpwm[ELE]); targetAngle[PITCH] = ratio[0]*(minAngle[PITCH] - trimAngle[PITCH]) + trimAngle[PITCH]; } if(sbus.manualpwm[RUD] > trimpwm[RUD]){ ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)maxpwm[RUD]); targetAngle[ROLL] = -1*ratio[1]*(maxAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL]; }else{ ratio[1] = CalcRatio((float)sbus.manualpwm[RUD], (float)trimpwm[RUD], (float)minpwm[RUD]); targetAngle[ROLL] = -1*ratio[1]*(minAngle[ROLL] - trimAngle[ROLL]) + trimAngle[ROLL]; } } 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; } /*---SBUS割り込み処理---*/ //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード) //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) void Update_PWM() { static int16_t pwm[8]; if(sbus.flg_ch_update == true){ switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 case Manual: for(uint8_t i=0;i<8;i++){ pwm[i] = sbus.manualpwm[i]; } oldTHR = sbus.manualpwm[THR]; //pc.printf("update_manual\r\n"); break; case Auto: pwm[AIL] = sbus.manualpwm[AIL]; pwm[ELE] = autopwm[ELE]; pwm[THR] = autopwm[THR]; pwm[RUD] = autopwm[RUD]; pwm[Ch5] = sbus.manualpwm[Ch5]; pwm[Ch6] = sbus.manualpwm[Ch6]; pwm[Ch7] = sbus.manualpwm[Ch7]; pwm[Ch8] = sbus.manualpwm[Ch8]; //pc.printf("update_auto\r\n"); break; default: for(uint8_t i=0;i<8;i++){ pwm[i] = sbus.manualpwm[i]; } //pc.printf("update_manual\r\n"); break; } }else{ pc.printf("0\r\n"); } sbus.flg_ch_update = false; //for(uint8_t i=0; i<8; i++){pc.printf("%d, ",pwm[i]);} //pc.printf("\r\n"); Output_PWM(pwm); } //pwmをサーボに出力する関数。 void Output_PWM(int16_t pwm[8]) { 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]); servo7.pulsewidth_us(pwm[6]); servo8.pulsewidth_us(pwm[7]); } void ResetTrim(){ for(uint8_t i=0; i<4; i++){ 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(); NVIC_DisableIRQ(USART1_IRQn); if(!mpu9250.sensingAcGyMg()){ //pc.printf("failed\r\n"); return; } NVIC_EnableIRQ(USART1_IRQn); mpu9250.calculatePostureAngle(nowAngle); /* deltaT = t.read_us() - t_start; pc.printf("t:%d us, ",deltaT); mpu9250.displayAngle(); */ //mpu9250.sensingPostureAngle(nowAngle); } //デバッグ用 void DebugPrint(){ //static int16_t deltaT = 0, t_start = 0; //t_start = t.read_us(); for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); pc.printf("\r\n"); //deltaT = t.read_us() - t_start; //pc.printf("t:%d us, ",deltaT); //pc.printf("\r\n"); //mpu9250.displayAngle(); //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]); //pc.printf("\r\n"); }