航空研究会
フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_53
by 
航空研究会
main.cpp
- Committer:
- taknokolat
- Date:
- 2018-09-26
- Revision:
- 43:4413ee61bc39
- Parent:
- 42:74d72339a8a8
File content as of revision 43:4413ee61bc39:
//mbed
#include "mbed.h"
#include "FATFileSystem.h"
#include "SDFileSystem.h"
//C
#include "math.h"
//sensor
#include "MPU6050_DMP6.h"
//#include "MPU9250.h"
//#include "BMP280.h"
#include "hcsr04.h"
//device
#include "sbus.h"
//config
#include "SkipperSv2.h"
#include "falfalla.h"
//other
#include "pid.h"
#define DEBUG_SEMIAUTO 0
#define DEBUG_PRINT_INLOOP 1
#define KP_ELE 15.0 //2.0
#define KI_ELE 0.0
#define KD_ELE 0.0 //0/0
#define KP_RUD 3.0
#define KI_RUD 0.0
#define KD_RUD 0.0
#define KP_AIL 0.1
#define KI_AIL 0.2
#define KD_AIL 0.2
//#define g_AIL_L_Ratio_rightloop 0.5
#define GAIN_CONTROLVALUE_TO_PWM 3.0
#define RIGHT_ROLL -12.0
#define RIGHT_PITCH -10.0 //5.0
#define LEFT_ROLL 12.0
#define LEFT_PITCH -5.0
#define STRAIGHT_ROLL 4.0
#define STRAIGHT_PITCH 3.0
#define TAKEOFF_THR 0.8
#define LOOP_THR 0.6
//#define g_rightloopRUD 1500
#define RIGHT_ROLL_SHORT -12.0
#define RIGHT_PITCH_SHORT -5.0
#define LEFT_ROLL_SHORT 12.0
#define LEFT_PITCH_SHORT -5.0
#define RIGHTLOOPROLL_APPROACH -17.0
#define LEFTLOOPROLL_APPROACH 16.0
#define RIGHTLOOPPITCH_APPROACH -15.0
#define LEFTLOOPPITCH_APPROACH -13.0
#define rightloopROLL2 -10.0
/*#define rightloopRUD 1300 //1250
#define rightloopshortRUD 1250
#define leftloopRUD 1500
#define leftloopshortRUD 1500
#define glideloopRUD 1300
*/
#define AIL_R_correctionrightloop 0
#define AIL_L_correctionrightloop 0
#define AIL_L_correctionrightloopshort 0
#define AIL_L_correctionleftloop -0
#define AIL_L_correctionleftloopshort 0
#define RIGHTLOOP_RUD 1250
#define RIGHTLOOPSHORT_RUD 1250
#define LEFTLOOP_RUD 1500
#define LEFTLOOPSHORT_RUD 1500
#define GLIDELOOP_RUD 1300
#define RIGHTLOOPRUD_APPROACH 1500
#define LEFTLOOPRUD_APPROACH 1500
#define AIL_L_CORRECTION_RIGHTLOOP 0
#define AIL_L_CORRECTION_RIGHTLOOPSHORT 0
#define AIL_L_CORRECTION_LEFTLOOP 0
#define AIL_L_CORRECTION_LEFTLOOPSHORT 0
#define GLIDE_ROLL -12.0
#define GLIDE_PITCH -3.0
#define AIL_L_RatioRising 0.5
#define AIL_L_RatioDescent 2
//コンパスキャリブレーション
//SkipperS2基板
/*
#define MAGBIAS_X -35.0
#define MAGBIAS_Y 535.0
#define MAGBIAS_Z -50.0
*/
//S2v2 1番基板
#define MAGBIAS_X 395.0
#define MAGBIAS_Y 505.0
#define MAGBIAS_Z -725.0
//S2v2 2番基板
/*
#define MAGBIAS_X 185.0
#define MAGBIAS_Y 220.0
#define MAGBIAS_Z -350.0
*/
#define ELEMENT 1
#define LIMIT_STRAIGHT_YAW 5.0
#define THRESHOLD_TURNINGRADIUS_YAW 60.0
#define ALLOWHEIGHT 15
#ifndef PI
#define PI 3.14159265358979
#endif
const int16_t lengthdivpwm = 320;
const int16_t changeModeCount = 6;
SBUS sbus(PA_9, PA_10); //SBUS
PwmOut servo1(PC_6); // TIM3_CH1 //old echo
PwmOut servo2(PC_7); // TIM3_CH2 //PC_7
PwmOut servo3(PB_0); // TIM3_CH3
PwmOut servo4(PB_1); // TIM3_CH4
PwmOut servo5(PB_6); // TIM4_CH1
PwmOut servo6(PB_7); // TIM4_CH2 //old trigger
//PwmOut servo7(PB_8); // TIM4_CH3 //PB_8 new echo
//PwmOut servo8(PB_9); // TIM4_CH4 //new trigger
RawSerial pc(PA_2,PA_3, 115200); //tx,rx.baudrate pin;PA_2=UART2_TX, PA_3=UART2_RX
//RawSerial pc2(PB_6,PB_7, 115200); //sbus確認用
SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
DigitalOut led1(PA_0); //黄色のコネクタ
DigitalOut led2(PA_1);
DigitalOut led3(PB_4);
DigitalOut led4(PB_5);
//InterruptIn switch2(PC_14);
MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
HCSR04 usensor(PB_9,PB_8); //trig,echo 9,8
PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
PID pid_RUD(g_kpRUD,g_kiRUD,g_kdRUD);
enum Channel{AIL_R, ELE, THR, RUD, DROP, AIL_L, Ch7, Ch8};
enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
enum OperationMode{StartUp, SemiAuto, RightLoop, LeftLoop, GoStraight, BombwithPC, ZERO, Moebius, Glide};
enum BombingMode{Takeoff, Chicken, Transition, Approach};
enum OutputStatus{Manual, Auto};
static OutputStatus output_status = Manual;
OperationMode operation_mode = StartUp;
BombingMode bombing_mode = Takeoff;
static int16_t autopwm[8] = {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();
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++;
}
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, strlen(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, strlen(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': //機首ヲ上ゲヨ
targetAngle[ROLL] = g_gostraightROLL;
autopwm[THR] = minpwm[THR];
autopwm[ELE] = trimpwm[ELE]-100;
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");
}