Dependencies: mbed MPU6050_2 SDFileSystem3 HCSR04_2
Diff: main.cpp
- Revision:
- 46:ebbe73af073d
- Parent:
- 45:eebdf6fa7b15
- Child:
- 47:b3d78207b08f
diff -r eebdf6fa7b15 -r ebbe73af073d main.cpp
--- a/main.cpp Wed Sep 26 23:43:14 2018 +0000
+++ b/main.cpp Sat Dec 22 06:27:25 2018 +0000
@@ -159,1579 +159,14 @@
BombingMode bombing_mode = Takeoff;
static int16_t autopwm[8] = {1455,1450,1176,1628,1417,1452};
-/*
-//1号機
-static int16_t trimpwm[6] = {1580,1600,1176,1404,1440,1448};
-int16_t maxpwm[6] = {1796,1936,1848,1740,1820,1856};
-int16_t minpwm[6] = {1182,1265,1176,1068,1180,1176};
-const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
-*/
-//2号機
-
-static int16_t trimpwm[6] = {1455,1450,1176,1628,1417,1452};
-int16_t maxpwm[6] = {1672,1786,1848,1964,1712,1860};
-int16_t minpwm[6] = {1057,1115,1176,1292,1180,1180};
-const int16_t reverce[4] = {Reverce_falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]};
-
-
-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;
-Timer t2;
-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 getSF_Serial();
-float ConvertByteintoFloat(char high, char low);
-
-
-//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,
- int *g_rightloopRUD, int *g_rightloopshortRUD,
- int *g_leftloopRUD, int *g_leftloopshortRUD,
- int *g_glideRUD,
- float *g_rightloopROLL_approach,float *g_leftloopROLL_approach,
- int *g_rightloopRUD_approach,int *g_leftloopRUD_approach,
- float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach
- );
-//switch2割り込み
-void ResetTrim();
-
-//自動操縦
-void UpdateTargetAngle_GoStraight(float targetAngle[3]);
-void UpdateTargetAngle_GoStraight_zero(float targetAngle[3]); //着陸時にスロットルが０の時の直進
-void UpdateTargetAngle_Rightloop(float targetAngle[3]);
-void UpdateTargetAngle_Rightloop_short(float targetAngle[3]);
-void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]); //着陸時にスロットルが０の時の右旋回
-void UpdateTargetAngle_Leftloop(float targetAngle[3]);
-void UpdateTargetAngle_Leftloop_short(float targetAngle[3]);
-void UpdateTargetAngle_Leftloop_zero(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 Sbusprintf();
-void DebugPrint();
-
-/*---関数のプロトタイプ宣言終わり---*/
-
-int main()
-{
- setup();
-
-
+int main(void){
while(1){
-
- loop();
-
-
- NVIC_DisableIRQ(USART1_IRQn);
- if(!CheckSW_Up(Ch7)){
- led3=0;
- }else{
- led3=1;
- }
- NVIC_EnableIRQ(USART1_IRQn);
- }
-
-}
-
-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,
- &g_rightloopRUD, &g_rightloopshortRUD,
- &g_leftloopRUD, &g_leftloopshortRUD,
- &g_glideloopRUD,
- &g_rightloopROLL_approach,&g_leftloopROLL_approach,
- &g_rightloopRUD_approach,&g_leftloopRUD_approach,
- &g_rightloopPITCH_approach,&g_leftloopPITCH_approach
- );
-
-
- Init_PWM();
- Init_servo();
- Init_sbus();
- Init_sensors();
- //switch2.rise(ResetTrim);
-
- 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);
- 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;
- }
-
- pc.attach(getSF_Serial, Serial::RxIrq);
- NVIC_SetPriority(USART2_IRQn,4);
-
- 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();
- UpdateTargetAngle(targetAngle);
- CalculateControlValue(targetAngle, controlValue);
- UpdateAutoPWM(controlValue);
-
-
- //NVIC_SetPriority(TIM5_IRQn,4);
- //NVIC_SetPriority(USART2_IRQn,2);
-
- wait_ms(23);
-
- //NVIC_SetPriority(TIM5_IRQn,2);
- //NVIC_SetPriority(USART2_IRQn,4);
-
-
- // pc.printf("6\r\n");
- //NVIC_DisableIRQ(USART2_IRQn);
- //pc.printf("%c",g_landingcommand);
- //NVIC_EnableIRQ(USART2_IRQn);
-#if DEBUG_PRINT_INLOOP
- //Sbusprintf();
- 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(trimpwm[DROP]);
-
- 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(){
- 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");
- pc.attach(getSF_Serial, Serial::RxIrq);
- 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");
- pc.attach(NULL, Serial::RxIrq);
- 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;
- }
- }
-}
-
-
-//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,
- int *g_rightloopRUD, int *g_rightloopshortRUD,
- int *g_leftloopRUD, int *g_leftloopshortRUD,
- int *g_glideloopRUD,
- float *g_rightloopROLL_approach,float *g_leftloopROLL_approach,
- int *g_rightloopRUD_approach,int *g_leftloopRUD_approach,
- float *g_rightloopPITCH_approach,float *g_leftloopPITCH_approach
- ){
-
- pc.printf("SDsetup start.\r\n");
-
- FILE *fp;
- char parameter[40]; //文字列渡す用の配列
- 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",
- "RIGHTLOOP_RUDDER",
- "RIGHTLOOPSHORT_RUDDER",
- "LEFTLOOP_RUDDER",
- "LEFTLOOPSHORT_RUDDER",
- "GLIDELOOP_RUDDER",
- "RIGHTLOOP_ROLL_APPROACH",
- "LEFTLOOP_ROLL_APPROACH",
- "RIGHTLOOP_RUDDER_APPROACH",
- "LEFTLOOP_RUDDER_APPROACH",
- "RIGHTLOOP_PITCH_APPROACH",
- "LEFTLOOP_PITCH_APPROACH",
- };
-
- 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++;
- }
- if(GetParameter(fp,paramNames[23],parameter)) *g_rightloopRUD = atof(parameter);
- else{ *g_rightloopRUD = RIGHTLOOP_RUD;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[24],parameter)) *g_rightloopshortRUD = atof(parameter);
- else{ *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[25],parameter)) *g_leftloopRUD = atof(parameter);
- else{ *g_leftloopshortRUD = LEFTLOOP_RUD;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[26],parameter)) *g_leftloopshortRUD = atof(parameter);
- else{ *g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[27],parameter)) *g_glideloopRUD = atof(parameter);
- else{ *g_glideloopRUD = GLIDELOOP_RUD;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[28],parameter)) *g_rightloopROLL_approach = atof(parameter);
- else{ *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[29],parameter)) *g_leftloopROLL_approach= atof(parameter);
- else{ *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[30],parameter)) *g_rightloopRUD_approach = atof(parameter);
- else{ *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[31],parameter)) *g_leftloopRUD= atof(parameter);
- else{ *g_leftloopRUD= LEFTLOOPRUD_APPROACH;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[32],parameter)) *g_rightloopPITCH_approach = atof(parameter);
- else{ *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[33],parameter)) *g_leftloopPITCH_approach = atof(parameter);
- else{ *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
- SDerrorcount++;
+ printf("Hello World\r\n");
+ //servo1.period_ms(14);
+ servo1.pulsewidth_us(1300);
+ wait(1);
+ //servo1.period_ms(14);
+ servo1.pulsewidth_us(1200);
}
-
- 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;
- *g_rightloopRUD = RIGHTLOOP_RUD;
- *g_rightloopshortRUD = RIGHTLOOPSHORT_RUD;
- *g_leftloopRUD = LEFTLOOP_RUD;
- *g_leftloopshortRUD = LEFTLOOPSHORT_RUD;
- *g_glideloopRUD = GLIDELOOP_RUD;
- *g_rightloopROLL_approach = RIGHTLOOPROLL_APPROACH;
- *g_leftloopROLL_approach = LEFTLOOPROLL_APPROACH;
- *g_rightloopRUD_approach = RIGHTLOOPRUD_APPROACH;
- *g_leftloopRUD_approach = LEFTLOOPRUD_APPROACH;
- *g_rightloopPITCH_approach = RIGHTLOOPPITCH_APPROACH;
- *g_leftloopPITCH_approach = LEFTLOOPPITCH_APPROACH;
-
- 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);
- pc.printf("kpAIL = %f,kiAIL = %f,kdAIL = %f\r\n ",*g_kpAIL,*g_kiAIL,*g_kdAIL);
- pc.printf("RIGHTLOOPRUD = %d,RIGHTLOOPSHORTRUD = %d\r\n",*g_rightloopRUD,*g_rightloopshortRUD);
- pc.printf("LEFTTLOOPRUD = %d,LEFTLOOPSHORTRUD = %d\r\n",*g_leftloopRUD,*g_leftloopshortRUD);
- pc.printf("GLIDELOOPRUD = %d\r\n",*g_glideloopRUD);
- pc.printf("RIGHTLOOP_ROLL_APPROACH = %f, LEFTLOOP_ROLL_APPROACH= %f\r\n",*g_rightloopROLL_approach,*g_leftloopROLL_approach);
- pc.printf("RIGHTLOOP_RUD_APPROACH = %d, LEFTLOOP_RUD_APPROACH = %d\r\n",*g_rightloopRUD_approach,*g_leftloopRUD_approach);
- pc.printf("RIGHTLOOP_PITCH_APPROACH = %f, LEFTLOOP_PITCH_APPROACH = %f\r\n",*g_rightloopPITCH_approach,*g_leftloopPITCH_approach);
-
- 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]){
- NVIC_DisableIRQ(USART1_IRQn);
- 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]);
-
- NVIC_EnableIRQ(USART1_IRQn);
-}
-
-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()
-{
- NVIC_DisableIRQ(USART1_IRQn);
- static int16_t pwm[6];
- static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
- static int16_t FailsafeCounter=0;
- static int16_t ResetCounter=0;
- static int16_t OKCounter=0;
-
- if(sbus.flg_ch_update == true){
-
- switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
- case Manual:
- if(OKCounter!=0) break;
- for(uint8_t i=0;i<6;i++){
- pwm[i] = sbus.manualpwm[i];
- }
- oldTHR = sbus.manualpwm[THR];
- //pc.printf("update_manual\r\n");
- NVIC_EnableIRQ(USART1_IRQn);
- break;
-
- case Auto:
- if(OKCounter!=0) break;
- 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];
-
- NVIC_EnableIRQ(USART1_IRQn);
- break;
-
- default:
- if(OKCounter!=0) break;
- for(uint8_t i=0;i<6;i++){
- pwm[i] = sbus.manualpwm[i];
- } //pc.printf("update_manual\r\n");
- NVIC_EnableIRQ(USART1_IRQn);
- break;
- }
-
- for(uint8_t i=0;i<6;i++){
- if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
- temppwm[i]=pwm[i];
- }
-
- }
- //else(sbus.flg_ch_update == false) pc.printf("0\r\n");
- /* if(sbus.failsafe_status==SBUS_SIGNAL_OK){
- pc.printf("OK\r\n");
- }
- */
- //pc.printf("%d\r\n",sbus.failsafe_status);
-
- if(sbus.failsafe_status==SBUS_SIGNAL_LOST||sbus.failsafe_status==SBUS_SIGNAL_FAILSAFE) FailsafeCounter++;
- else ResetCounter++;
-
- if(ResetCounter>7){
- ResetCounter=0;
- FailsafeCounter=0;
- }
-
- if(FailsafeCounter>10){
- ResetCounter=0;
- for(uint8_t i=0;i<6;i++) pwm[i] = trimpwm[i];
-
- if(sbus.failsafe_status==SBUS_SIGNAL_OK&&sbus.flg_ch_update == true) OKCounter++;
- else OKCounter=0;
-
- if(OKCounter>10){
- OKCounter=0;
- FailsafeCounter=0;
- }
- //pc.printf("OKCounter=%d, FailsafeCounter=%d, sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status);
- }
- //if(sbus.failsafe_status==SBUS_SIGNAL_OK){FailsafeCounter=0;}
-
-
- 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(){
- //NVIC_DisableIRQ(USART1_IRQn);
- //NVIC_DisableIRQ(EXTI0_IRQn);
- //NVIC_DisableIRQ(TIM5_IRQn);
-
-
- static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
-
- static int bufcounter=0;
-
-
-
- if(pc.readable()) { // 受信確認
-
- SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
- if(SFbuf[0]!='S'){
- //pc.printf("x");
- return;
- }
-
-
-
- //pc.printf("%c",SFbuf[bufcounter]);
-
- if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
-
- if(bufcounter==5 && SFbuf[4]=='F'){
-
- g_landingcommand = SFbuf[1];
- //wait_ms(20);
- //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
- if(g_landingcommand=='Y')g_SerialTargetYAW =0.0f;
- bufcounter = 0;
- memset(SFbuf, 0, sizeof(SFbuf));
- NVIC_ClearPendingIRQ(USART2_IRQn);
- //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
- }
-
- else if(bufcounter>=5){
- //pc.printf("Communication Falsed.\r\n");
- memset(SFbuf, 0, sizeof(SFbuf));
- bufcounter = 0;
- NVIC_ClearPendingIRQ(USART2_IRQn);
- }
- }
-
- //NVIC_EnableIRQ(TIM5_IRQn);
- //NVIC_EnableIRQ(EXTI0_IRQn);
- //NVIC_EnableIRQ(USART1_IRQn);
- }
-
-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;
- led4 = 0;
- }
- autopwm[THR]=oldTHR;
-
- newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius);
-
- if(RotateCounter == 0 && newYaw_Moebius >90.0 && newYaw_Moebius < 180.0) {RotateCounter++; led4 = 1; pc.printf("Rotate 90\r\n");}
- if(RotateCounter == 1 && newYaw_Moebius >-180.0 && newYaw_Moebius < -90.0) {RotateCounter++; led4 = 0; pc.printf("Rotate 180\r\n");}
- if(RotateCounter == 2 && newYaw_Moebius >-90.0 && newYaw_Moebius <-10.0) {RotateCounter++; led4 = 1; pc.printf("Rotate 270\r\n");}
- if(RotateCounter == 3 && newYaw_Moebius >0.0 && newYaw_Moebius < 90.0) {RotateCounter++; led4 = 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;
- static bool flg_ground = false;
- int t_diff = 0;
- static int groundcount = 0;
-
- targetAngle[ROLL] = g_glideloopROLL;
-
- autopwm[RUD]=g_glideloopRUD;
- // 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){
- led4 = 1;
- //pc.printf("Call [Stop!] calling!\r\n");
- }
-
- if(g_distance<250 && g_distance > 0) {
- groundcount++;
- }
- NVIC_EnableIRQ(EXTI9_5_IRQn);
- }else{
- t_diff = 0;
- groundcount = 0;
- led4 = 0;
- }
-
- NVIC_DisableIRQ(EXTI9_5_IRQn);
- if(t_diff > 17) autopwm[THR] = SetTHRinRatio(0.5);
-
- else if(g_distance<150 && g_distance>0 ){
- NVIC_DisableIRQ(EXTI9_5_IRQn);
- THRcount++;
- if(THRcount>5) flg_ground = true;
- }
- else THRcount = 0;
- NVIC_EnableIRQ(EXTI9_5_IRQn);
-
- if(flg_ground == true) {
- autopwm[THR] = SetTHRinRatio(0.6);
- targetAngle[PITCH] = g_gostraightPITCH;
- }
- else {
- autopwm[THR] = minpwm[THR];
- targetAngle[PITCH] = g_glideloopPITCH;
- }
-
- NVIC_DisableIRQ(USART1_IRQn);
- if(!CheckSW_Up(Ch7)){
- flg_ground = false;
- }
- NVIC_EnableIRQ(USART1_IRQn);
-}
-
-//離陸-投下-着陸一連
-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);
- NVIC_DisableIRQ(EXTI9_5_IRQn);
- if(g_distance>150) TakeoffCount++;
- else TakeoffCount = 0;
- NVIC_EnableIRQ(EXTI9_5_IRQn);
- if(TakeoffCount>5){
- autopwm[THR] = 1180+320*2*0.5;
- targetAngle[PITCH]=g_gostraightPITCH;
- autopwm[RUD]=trimpwm[RUD];
- //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);
- static int tELE_start = 0;
- static bool flg_ELEup = false;
- int t_def = 0;
-
- autopwm[RUD] = trimpwm[RUD];
-
-
- if(!flg_ELEup && CheckSW_Up(Ch7)){
- tELE_start = t.read_ms();
- flg_ELEup = true;
- pc.printf("timer start\r\n");
- }else if(!CheckSW_Up(Ch7)){
- tELE_start = 0;
- flg_ELEup = false;
- }
- if(flg_ELEup){
- t_def = t.read_ms() - tELE_start;
-
- //1.5秒経過すればELE上げ舵へ
- if(t_def>500) targetAngle[PITCH]=-35.0;
- else{
- t_def = 0;
- targetAngle[PITCH]=g_gostraightPITCH;
- }
- }
- 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] = 1712;
- 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]){
-
- static bool zeroTHR=true;//着陸時のスロットル動作確認用
-
- if(CheckSW_Up(Ch7)){
- output_status = Auto;
- led1 = 1;
- }else{
- output_status = Manual;
- led1 = 0;
- zeroTHR=true;
- }
-
-
- NVIC_DisableIRQ(USART2_IRQn);
- switch(g_landingcommand){
- case 'R': //右旋回セヨ
- if(zeroTHR==false){
- UpdateTargetAngle_Rightloop_zero(targetAngle);
- }
- else{
- targetAngle[ROLL] = g_rightloopROLL_approach;
- targetAngle[PITCH] = g_rightloopPITCH_approach ;
- autopwm[RUD]=g_rightloopRUD_approach; //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;
- }
- }
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'L': //左旋回セヨ
- if(zeroTHR==false){
- UpdateTargetAngle_Leftloop_zero(targetAngle);
- }
- else{
- targetAngle[ROLL] = g_leftloopROLL_approach;
- targetAngle[PITCH] = g_leftloopPITCH_approach;
- autopwm[RUD]=g_leftloopRUD_approach;
- 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;
- }
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'G': //直進セヨ
- if(zeroTHR==false){
- UpdateTargetAngle_GoStraight_zero(targetAngle);
- }
- else{
- targetAngle[ROLL] = g_gostraightROLL;
- targetAngle[PITCH] = g_gostraightPITCH;
- }
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'Y': //指定ノヨー方向ニ移動セヨ
- Rotate(targetAngle, g_SerialTargetYAW);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'U': //機首ヲ上ゲヨ
- static int UpCounter=0;
- UpCounter++;
- if(UpCounter==3){
- while(1){
- targetAngle[ROLL] = g_gostraightROLL;
- autopwm[THR] = minpwm[THR];
- autopwm[ELE] = minpwm[ELE];
- if(CheckSW_Up(Ch7)){
- output_status = Auto;
- led1 = 1;
- }else{
- output_status = Manual;
- led1 = 0;
- zeroTHR=true;
- }
- }
-
- }
- NVIC_EnableIRQ(USART2_IRQn);
-
- break;
-
- /*case 'B': //ブザーヲ鳴ラセ
- //buzzer = 1;
- NVIC_EnableIRQ(USART2_IRQn);
- break;*/
-
- case 'B': //物資ヲ落トセ
- Chicken_Drop();
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'C': //停止セヨ
- targetAngle[ROLL] = 0.0;
- targetAngle[PITCH] = -3.0;
- autopwm[THR] = minpwm[THR];
- zeroTHR=false;
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- default :
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
-
- }
-
-}
-
-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]){
-
- autopwm[RUD] = trimpwm[RUD];
- 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_GoStraight_zero(float targetAngle[3]){
-
- autopwm[RUD] = trimpwm[RUD];
- targetAngle[ROLL] = g_gostraightROLL;
- targetAngle[PITCH] = g_gostraightPITCH;
- autopwm[THR] = minpwm[THR];
-
- //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
-}
-
-//右旋回
-void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
-
- targetAngle[PITCH] = g_rightloopPITCH ;
- autopwm[RUD]=g_rightloopRUD; //RUD固定
- autopwm[THR] = SetTHRinRatio(0.45); //手動スロットル記憶
-
- /*
- if (nowAngle[ROLL]>20.0||nowAngle[ROLL]<-20.0){
- t2.start();
- pc.printf("Timer start.");
- }
- if(0.0<t2.read()<5.0){
- //pc.printf("tagetAngle is changed.");
- targetAngle[ROLL] = rightloopROLL2;
- }
- else {
- t2.stop();
- t2.reset();
- pc.printf("Timer stopped.");
- targetAngle[ROLL] = g_rightloopROLL;
- }
-*/
- 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]);
-}
-
-//右旋回（着陸時スロットル０の時）
-void UpdateTargetAngle_Rightloop_zero(float targetAngle[3]){ //右旋回
- autopwm[THR]=minpwm[THR];
- targetAngle[ROLL] = g_rightloopROLL_approach;
- targetAngle[PITCH] = g_rightloopPITCH_approach ;
- autopwm[RUD]=g_rightloopRUD_approach; //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;
-
- //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]=g_rightloopshortRUD;
- autopwm[THR] = SetTHRinRatio(0.45);
- 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]=g_leftloopRUD;
- autopwm[THR] = SetTHRinRatio(0.45);
- if(autopwm[AIL_R]<trimpwm[AIL_R]){
- autopwm[AIL_L]=trimpwm[AIL_L]+g_AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
- }
- else autopwm[AIL_L]=trimpwm[AIL_L]+g_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]);
-}
-
-//左旋回（着陸時スロットル０のとき）
-void UpdateTargetAngle_Leftloop_zero(float targetAngle[3]){
-
- targetAngle[ROLL] = g_leftloopROLL_approach;
- targetAngle[PITCH] = g_leftloopPITCH_approach;
- autopwm[RUD]=g_leftloopRUD_approach;
- 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]=g_leftloopRUD;
- autopwm[THR] = SetTHRinRatio(0.45);
- 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 Sbusprintf(){
-
- for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]);
- pc.printf("\r\n");
-
- }
-
-
-
-//デバッグ用
-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);
- //NVIC_DisableIRQ(EXTI9_5_IRQn);
- //pc.printf("g_distance = %d",g_distance);
- //NVIC_EnableIRQ(EXTI9_5_IRQn);
- //pc.printf("Mode: %c: ",g_buf[0]);
- //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
- //pc.printf("\r\n");
-}
\ No newline at end of file
+ }
\ No newline at end of file