s
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of AutoFlight2017_now_copy by
Diff: main.cpp
- Revision:
- 1:f31e17341659
- Parent:
- 0:92024886c0be
- Child:
- 2:e7025f2cf0e1
diff -r 92024886c0be -r f31e17341659 main.cpp --- a/main.cpp Tue Aug 01 12:27:13 2017 +0000 +++ b/main.cpp Tue Aug 28 07:12:16 2018 +0000 @@ -1,85 +1,144 @@ +//mbed #include "mbed.h" -//#include "rtos.h" - +#include "FATFileSystem.h" +#include "SDFileSystem.h" +//C #include "math.h" -#include "MPU9250.h" -#include "BMP280.h" +//sensor +#include "MPU6050_DMP6.h" +//#include "MPU9250.h" +//#include "BMP280.h" +#include "hcsr04.h" +//device +#include "sbus.h" +//config #include "SkipperSv2.h" -#include "Estrela.h" -#include "sbus.h" +#include "falfalla.h" +//other #include "pid.h" -#define DEBUG_SEMIAUTO 1 -#define DEBUG_PRINT_INLOOP 1 +#define DEBUG_SEMIAUTO 0 +#define DEBUG_PRINT_INLOOP + +#define KP_ELE 2.0 +#define KI_ELE 0.0 +#define KD_ELE 0.0 +#define KP_RUD 3.0 +#define KI_RUD 0.0 +#define KD_RUD 0.0 + +#define GAIN_CONTROLVALUE_TO_PWM 3.0 + +#define RIGHT_ROLL -17.0 +#define RIGHT_PITCH -6.3 +#define LEFT_ROLL 16.0 +#define LEFT_PITCH -6.0 +#define STRAIGHT_ROLL 2.0 +#define STRAIGHT_PITCH -3.0 +#define TAKEOFF_THR 1.0 +#define LOOP_THR 0.58 -#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 RIGHT_ROLL_SHORT -20.0 +#define RIGHT_PITCH_SHORT -7.0 +#define LEFT_ROLL_SHORT 22.0 +#define LEFT_PITCH_SHORT -6.0 + +#define GLIDE_ROLL 16.0 +#define GLIDE_PITCH 0.0 -#define GAIN_CONTROLVALUE_TO_PWM 3 - +//コンパスキャリブレーション +//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 +*/ -const uint16_t changeModeCount = 50; -const int16_t lengthdivpwm = 320; +#define ELEMENT 1 +#define LIMIT_STRAIGHT_YAW 5.0 +#define LIMIT_LOOPSTOP_YAW 1.0 +#define THRESHOLD_TURNINGRADIUS_YAW 60.0 +#define ALLOWHEIGHT 15 -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}; +#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] = {ExpMin_Falfalla[0],Reverce_falfalla[1],Reverce_falfalla[2],Reverce_falfalla[3]}; SBUS sbus(PA_9, PA_10); //SBUS -PwmOut servo1(PC_6); // TIM3_CH1 +//PwmOut servo1(PC_6); // TIM3_CH1 //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 -PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 -PwmOut servo8(PB_9); // TIM4_CH4 +//PwmOut servo6(PB_7); // TIM4_CH2 //trigger +//PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 +//PwmOut servo8(PB_9); // TIM4_CH4 -Serial pc(PA_2, PA_3); +RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX +SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd"); -DigitalOut led1(PA_0); //緑 -DigitalOut led2(PA_1); //赤 +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); +//InterruptIn switch2(PC_14); +MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン,シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine +HCSR04 usensor(PB_7,PC_6); //trig,echo 9,8 -PID pid_ELE(KP_ELE,KI_ELE,KD_ELE); -PID pid_RUD(KP_RUD,KI_RUD,KD_RUD); +PID pid_ELE(g_kpELE,g_kiELE,g_kdELE); +PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD); -enum Channel{AIL, ELE, THR, RUD, Ch5, Ch6, Ch7, Ch8}; -enum Angle{ROLL, PITCH, YAW}; -enum OperationMode{StartUp, SemiAuto}; +enum Channel{AIL, ELE, THR, RUD, DROP, Ch6, 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; -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}; +BombingMode bombing_mode = Takeoff; +static int16_t autopwm[8] = {1500,1500,1180,1500,1446,1500}; +static int16_t trimpwm[6] = {1500,1500,1180,1500,1446,1500}; +int16_t maxpwm[6] = {1820,1820,1820,1820,1820,1820}; +int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180}; int16_t oldTHR = 1000; static float nowAngle[3] = {0,0,0}; -const float trimAngle[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}; -Timer t; +float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0; -//IWDG_HandleTypeDef hiwdg; +unsigned int g_distance; +Ticker USsensor; +static char g_buf[16]; +char g_landingcommand; +float g_SerialTargetYAW = 0.0; + +Timer t; +Timeout RerurnChickenServo1; +Timeout RerurnChickenServo2; /*-----関数のプロトタイプ宣言-----*/ void setup(); @@ -89,9 +148,15 @@ void Init_servo(); //サーボ初期化 void Init_sbus(); //SBUS初期化 void Init_sensors(); -void DisplayClock(); //クロック状態確認 +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]); @@ -99,14 +164,56 @@ 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[8]); //pwmをサーボへ出力 +void Output_PWM(int16_t pwm[6]); //pwmをサーボへ出力 + +//シリアル割り込み +void SendSerial(); //1文字きたら送り返す +void SendArray(); +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); //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]); + +char Rotate(float targetAngle[3], float TargetYAW, char mode); + +//投下 +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(); @@ -114,59 +221,102 @@ int main() { - setup(); + setup(); + while(1){ + loop(); + + if(!CheckSW_Up(Ch7)){ + led3=0; + }else{ + led3=1; + } } } void setup(){ + //buzzer = 0; led1 = 1; led2 = 1; - led3 = 0; - led4 = 0; + led3 = 1; + led4 = 1; - pc.baud(115200); + 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); + + Init_PWM(); Init_servo(); Init_sbus(); Init_sensors(); - switch2.rise(ResetTrim); + //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(); - DisplayClock(); - + + + 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"); - - mpu9250.sensingAcGyMg(); - mpu9250.calculatePostureAngle(nowAngle); } void loop(){ - static float targetAngle[3], controlValue[2]; + static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0}; SensingMPU(); UpdateTargetAngle(targetAngle); + //Rotate(targetAngle, 30.0); CalculateControlValue(targetAngle, controlValue); UpdateAutoPWM(controlValue); - -#if DEBUG_PRINT_INLOOP + wait_ms(23); +//#if DEBUG_PRINT_INLOOP DebugPrint(); -#endif +//#endif } - - //サーボ初期化関数 -void Init_servo() -{ +void Init_servo(){ - servo1.period_ms(14); - servo1.pulsewidth_us(trimpwm[AIL]); + //servo1.period_ms(14); + //servo1.pulsewidth_us(trimpwm[AIL]); servo2.period_ms(14); servo2.pulsewidth_us(trimpwm[ELE]); @@ -178,21 +328,12 @@ 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); + servo5.pulsewidth_us(1392); pc.printf("servo initialized\r\n"); } -//Sbusを初期化する関数 +//Sbus初期化 void Init_sbus(){ sbus.initialize(); sbus.setLastfuncPoint(Update_PWM); @@ -200,16 +341,20 @@ } void Init_sensors(){ - if(!mpu9250.Initialize()){ + if(mpu6050.setup() == -1){ pc.printf("failed initialize\r\n"); - while(1); + while(1){ + led1 = 1; led2 = 0; led3 = 1; led4 = 0; + wait(1); + led1 = 0; led2 = 1; led3 = 0; led4 = 1; + wait(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]; + 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)); } @@ -225,12 +370,11 @@ } void UpdateTargetAngle(float targetAngle[3]){ - static uint16_t count_op = 0, count_out = 0; - + static int16_t count_op = 0, count_out = 0; #if DEBUG_SEMIAUTO switch(operation_mode){ case StartUp: - if(!CheckSW_Up(Ch5) && CheckSW_Up(Ch6)){ + if(!CheckSW_Up(Ch7) && CheckSW_Up(Ch8)){ count_op++; if(count_op > changeModeCount){ operation_mode = SemiAuto; @@ -261,16 +405,308 @@ break; } - if(CheckSW_Up(Ch5)){ - output_status = Auto; - led1 = 1; - }else{ - output_status = Manual; - led1 = 0; +#else + + switch(operation_mode){ + case StartUp: + if(!CheckSW_Up(Ch7) && !CheckSW_Up(Ch8)){ //ch7;自動・手動切り替え ch8;自動操縦モード切替 + count_op++; + if(count_op > changeModeCount){ + operation_mode = LeftLoop; + pc.printf("Goto LeftLoop mode\r\n"); + count_op = 0; + } + }else count_op = 0; + break; + + case LeftLoop: + 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; + UpdateTargetAngle_Leftloop(targetAngle); + break; + + case RightLoop: + 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_Rightloop(targetAngle); + + //Rotate確認用 + /* + static char mode = 'G'; + mode = Rotate(targetAngle,g_SerialTargetYAW,mode); + */ + 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 = GoStraight; + pc.printf("Goto GoStraight mode\r\n"); + count_op = 0; + } + }else count_op = 0; + Take_off_and_landing(targetAngle); + break; + + case GoStraight: + 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_GoStraight(targetAngle); + break; + + + default: + + operation_mode = StartUp; + break; } #endif + + if(CheckSW_Up(Ch7) && output_status == Manual){ + count_out++; + if(count_out > changeModeCount){ + output_status = Auto; + //led3 = 1; + count_out = 0; + } + }else if(!CheckSW_Up(Ch7) && output_status == Auto){ + count_out++; + if(count_out > changeModeCount){ + output_status = Manual; + count_out = 0; + //led3 = 0; + } + }else count_out = 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 + ){ + + 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" + }; + + 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++; + } + 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; + 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; @@ -286,37 +722,17 @@ 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; + 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]; + autopwm[RUD] = trimpwm[RUD] + reverce[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); } @@ -335,7 +751,9 @@ return value; } - +inline int16_t SetTHRinRatio(float ratio){ + return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio); +} /*---SBUS割り込み処理---*/ @@ -343,98 +761,567 @@ //各stabiGylo関数で算出したpwmを各変数に代入する(自動モード) void Update_PWM() { - static int16_t pwm[8]; + static int16_t pwm[5], sbuspwm[5]; + static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]}; + static int16_t tempsbuspwm[5] = {trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4]}; + static int counter_abnormalpwm=0; if(sbus.flg_ch_update == true){ switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替 - case Manual: - for(uint8_t i=0;i<8;i++){ - pwm[i] = sbus.manualpwm[i]; + case Manual: + for(uint8_t i=1;i<4;i++){ + if(abs(sbus.manualpwm[i]-tempsbuspwm[i])>300){ + counter_abnormalpwm++; + if(counter_abnormalpwm<=1) sbuspwm[i]=tempsbuspwm[i]; + else { + counter_abnormalpwm = 0; + sbuspwm[i] = sbus.manualpwm[i]; + } + } + else{ + counter_abnormalpwm = 0; + sbuspwm[i] = sbus.manualpwm[i]; + } + tempsbuspwm[i]=sbus.manualpwm[i]; } - oldTHR = sbus.manualpwm[THR]; + sbuspwm[4] = sbus.manualpwm[4]; + + for(uint8_t i=1;i<5;i++){ + pwm[i] = sbuspwm[i]; + } + oldTHR = sbuspwm[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]; - + pwm[DROP] = autopwm[DROP]; //pc.printf("update_auto\r\n"); break; default: - for(uint8_t i=0;i<8;i++){ + for(uint8_t i=1;i<5;i++){ pwm[i] = sbus.manualpwm[i]; } //pc.printf("update_manual\r\n"); break; } + for(uint8_t i=1;i<5;i++) if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i]; + + if(pwm[4]>1600 || pwm[4]<1420) pwm[4]=temppwm[4]; + temppwm[4] = pwm[4]; + }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); + sbus.flg_ch_update = false; + + Output_PWM(pwm); } -//pwmをサーボに出力する関数。 -void Output_PWM(int16_t pwm[8]) +//pwmをサーボに出力。 +void Output_PWM(int16_t pwm[5]) { - servo1.pulsewidth_us(pwm[0]); + //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++){ + for(uint8_t i=1; 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(); + + float rpy[3] = {0}, oldrpy[3] = {0}; + static uint16_t count_changeRPY = 0; + static bool flg_checkoutlier = false; NVIC_DisableIRQ(USART1_IRQn); - if(!mpu9250.sensingAcGyMg()){ - //pc.printf("failed\r\n"); - return; - } + NVIC_DisableIRQ(USART2_IRQn); + NVIC_DisableIRQ(TIM5_IRQn); + NVIC_DisableIRQ(EXTI0_IRQn); + NVIC_DisableIRQ(EXTI9_5_IRQn); + + mpu6050.getRollPitchYaw_Skipper(rpy); + NVIC_EnableIRQ(USART1_IRQn); - mpu9250.calculatePostureAngle(nowAngle); - /* - deltaT = t.read_us() - t_start; - pc.printf("t:%d us, ",deltaT); - mpu9250.displayAngle(); - */ + 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(); + if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++; + + if(bufcounter==5 && SFbuf[4]=='F'){ + g_landingcommand = SFbuf[1]; + if(g_landingcommand=='Y' || g_landingcommand=='C')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 && SFbuf[4]!='F'){ + //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 uint8_t precounter=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; + } + + newYaw_Moebius = TranslateNewYaw(nowAngle[YAW], FirstYAW_Moebius); + + if(RotateCounter == 0 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;} + } + if(RotateCounter == 1 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;} + + } + if(RotateCounter == 2 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;} + } + if(RotateCounter == 3 && newYaw_Moebius >10.0f && newYaw_Moebius < 90.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;} + } + if(RotateCounter == 4 && newYaw_Moebius >90.0f && newYaw_Moebius < 180.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 90\r\n"); precounter=0;} + } + if(RotateCounter == 5 && newYaw_Moebius >-180.0f && newYaw_Moebius < -90.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 180\r\n"); precounter=0;} + } + if(RotateCounter == 6 && newYaw_Moebius >-90.0f && newYaw_Moebius < 10.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 1; pc.printf("Rotate 270\r\n"); precounter=0;} + } + if(RotateCounter == 7 && newYaw_Moebius >10.0f && newYaw_Moebius < 180.0f){ + precounter++; + if(precounter>3){RotateCounter++; led2 = 0; pc.printf("Rotate 360\r\n"); precounter=0;} + } + + + if(RotateCounter < 6) 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; + + //時間計測開始設定 + 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点灯 + 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++; + }else{ + t_diff = 0; + groundcount = 0; + led2 = 0; + } + + if(t_diff > 17){ + autopwm[THR] = SetTHRinRatio(0.5); + }else{ + if(g_distance<150 && g_distance>0 ){ + 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; - //mpu9250.sensingPostureAngle(nowAngle); + switch(bombing_mode){ + case Takeoff: + static bool flg_setFirstYaw = false; + static int TakeoffCount = 0; + + if(!flg_setFirstYaw && CheckSW_Up(Ch7)){ + FirstYAW = nowAngle[YAW] + FirstYAW; + 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){ + led2=1; + autopwm[THR] = SetTHRinRatio(g_loopTHR); + pc.printf("Now go to Approach mode!!"); + bombing_mode = Approach; + led2=0; + } + + 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: + UpdateTargetAngle_Approach(targetAngle); + break; + + default: + bombing_mode = Takeoff; + break; + } +} + +//離陸モード +void UpdateTargetAngle_Takeoff(float targetAngle[3]){ + static int tELE_start = 0; + static bool flg_ELEup = false; + int t_def = 0; + 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]=-25.0; + else{ + t_def = 0; + targetAngle[PITCH]=g_gostraightPITCH; + } + } + autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 + targetAngle[ROLL] = 3.5; +/* + //pc.printf("%d \r\n",g_distance); + targetAngle[ROLL] = g_gostraightROLL; + if UptargetAngle[PITCH] = -10.0; + autopwm[THR] = SetTHRinRatio(g_takeoffTHR); //0.7;スロットルの割合 +*/ +} + +//ヨーを目標値にして許容角度になるまで水平旋回 +char Rotate(float targetAngle[3], float TargetYAW, char mode){ + float diffYaw = TranslateNewYaw(TargetYAW, nowAngle[YAW]); + + switch(mode){ + case 'G': //直進 + UpdateTargetAngle_GoStraight(targetAngle); + if(diffYaw > LIMIT_STRAIGHT_YAW) mode = 'R'; + else if(diffYaw < -LIMIT_STRAIGHT_YAW) mode = 'L'; + break; + + case 'R': //右旋回 + UpdateTargetAngle_Rightloop_short(targetAngle); + if(diffYaw < LIMIT_LOOPSTOP_YAW && diffYaw > -LIMIT_STRAIGHT_YAW) mode = 'G'; + break; + + case 'L': //左旋回 + UpdateTargetAngle_Leftloop_short(targetAngle); + if(diffYaw > -LIMIT_LOOPSTOP_YAW && diffYaw < LIMIT_STRAIGHT_YAW) mode = 'G'; + break; + } + + return mode; + +/* + if(diffYaw > LIMIT_STRAIGHT_YAW){ + UpdateTargetAngle_Rightloop_short(targetAngle); + return 1; + }else if(diffYaw < -LIMIT_STRAIGHT_YAW){ + UpdateTargetAngle_Leftloop_short(targetAngle); + return 1; + }else{ + UpdateTargetAngle_GoStraight(targetAngle); + return 0; + } +*/ +} + +//チキラー投下 +void Chicken_Drop(){ + if(CheckSW_Up(Ch7)){ + autopwm[DROP] = 1572; + pc.printf("Bombed!\r\n"); + RerurnChickenServo1.attach(&ReturnChickenServo1, 3); + //operation_mode = Approach; + //buzzer = 0; + } +} + +void ReturnChickenServo1(){ + autopwm[DROP] = 1438; + pc.printf("first reverse\r\n"); + RerurnChickenServo2.attach(&ReturnChickenServo2, 1); +} + +void ReturnChickenServo2(){ + autopwm[DROP] = 1446; + pc.printf("second reverse\r\n"); +} + +//着陸モード(PCからの指令に従う) +void UpdateTargetAngle_Approach(float targetAngle[3]){ + static char rotatemode = 'G'; + if(output_status == Manual) rotatemode = 'G'; + + //pc.putc(g_landingcommand); + switch(g_landingcommand){ + case 'R': //右旋回セヨ + UpdateTargetAngle_Rightloop(targetAngle); + break; + + case 'L': //左旋回セヨ + UpdateTargetAngle_Leftloop(targetAngle); + break; + + case 'G': //直進セヨ + UpdateTargetAngle_GoStraight(targetAngle); + break; + + case 'Y': //指定ノヨー方向ニ移動セヨ + rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode); + break; + + case 'B': //ブザーヲ鳴ラセ + //buzzer = 1; + break; + + case 'D': //物資ヲ落トセ + Chicken_Drop(); + break; + + case 'C': //停止セヨ + rotatemode = Rotate(targetAngle, g_SerialTargetYAW, rotatemode); + autopwm[THR] = minpwm[THR]; + break; + } +} + +void checkHeight(float targetAngle[3]){ + + static int targetHeight = 200; + + 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; +} + +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.05f); + //pc.printf("nose UP"); +} + +void UpdateTargetAngle_NoseDOWN(float targetAngle[3]){ + + //targetAngle[PITCH] -= 2.0f; + autopwm[THR] = SetTHRinRatio(g_loopTHR-0.05f); + //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[THR] = SetTHRinRatio(g_loopTHR); + //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[THR] = SetTHRinRatio(g_loopTHR); + + + //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[THR] = SetTHRinRatio(g_loopTHR); + //checkHeight(targetAngle); + + //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]); +} + +void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){ + + targetAngle[ROLL] = g_leftloopROLLshort; + targetAngle[PITCH] = g_leftloopPITCHshort; + autopwm[THR] = SetTHRinRatio(g_loopTHR); + + //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; - //t_start = t.read_us(); - for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); + /* + static int16_t deltaT = 0, t_start = 0; + deltaT = t.read_u2s() - t_start; + pc.printf("t:%d us, ",deltaT); pc.printf("\r\n"); - //deltaT = t.read_us() - t_start; - //pc.printf("t:%d us, ",deltaT); + t_start = t.read_us(); + */ + + //pc.printf("%d", sbus.manualpwm[4]); + + //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]); + //for(uint8_t i=1; i<5; 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[ELE]); pc.printf("%d\t",autopwm[RUD]); + //pc.printf("%d",g_distance); - //mpu9250.displayAngle(); - //for(uint8_t i=0; i<3; i++) pc.printf("%f ",nowAngle[i]); + //pc.printf("Mode: %c: ",g_buf[0]); + //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW); + //pc.printf("%x ",sbus.failsafe_status); + //pc.printf("\r\n"); -} - +} \ No newline at end of file