航空研究会
/
Autoflight2018_final_clean
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of
Autoflight2018_final
by 
航空研究会
Diff: main.cpp
- Revision:
- 54:a6b7bab193b1
- Parent:
- 53:2226bf635d21
--- a/main.cpp Tue Oct 09 12:59:33 2018 +0000
+++ b/main.cpp Fri Oct 12 10:31:12 2018 +0000
@@ -364,8 +364,8 @@
led4 = !led4;
}
- pc.attach(getSF_Serial, Serial::RxIrq);
- NVIC_SetPriority(USART2_IRQn,4);
+ pc.attach(getSF_Serial, Serial::RxIrq);
+ NVIC_SetPriority(USART2_IRQn,4);
FirstROLL = nowAngle[ROLL];
FirstPITCH = nowAngle[PITCH];
@@ -968,7 +968,7 @@
switch(output_status){ //マニュアルモード,自動モード,自動着陸もモードを切替
case Manual:
- if(OKCounter!=0) break;
+ if(OKCounter!=0) break;
for(uint8_t i=0;i<6;i++){
pwm[i] = sbus.manualpwm[i];
}
@@ -976,9 +976,9 @@
//pc.printf("update_manual\r\n");
NVIC_EnableIRQ(USART1_IRQn);
break;
-
+
case Auto:
- if(OKCounter!=0) break;
+ if(OKCounter!=0) break;
pwm[AIL_R] = autopwm[AIL_R]; //sbus.manualpwm[AIL];
pwm[ELE] = autopwm[ELE];
pwm[THR] = autopwm[THR];
@@ -988,37 +988,37 @@
NVIC_EnableIRQ(USART1_IRQn);
break;
-
+
default:
- if(OKCounter!=0) break;
+ 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];
+ }
- 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");
+ //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;
- }
-
+ */
+ //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];
@@ -1030,8 +1030,8 @@
OKCounter=0;
FailsafeCounter=0;
}
- //pc.printf("OKCounter=%d, FailsafeCounter=%d, sbus.failsafe_status=%d\r\n",OKCounter,FailsafeCounter,sbus.failsafe_status);
- }
+ //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;}
@@ -1118,58 +1118,58 @@
//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;
+ 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()) { // 受信確認
- if(pc.readable()) { // 受信確認
-
- SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
- if(SFbuf[0]!='S'){
- //pc.printf("x");
- return;
- }
+ SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
+ if(SFbuf[0]!='S'){
+ //pc.printf("x");
+ return;
+ }
- //pc.printf("%c",SFbuf[bufcounter]);
+ //pc.printf("%c",SFbuf[bufcounter]);
+
+ if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
- if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
-
- if(bufcounter==5 && SFbuf[4]=='F'){
+ if(bufcounter==5 && SFbuf[4]=='F'){
- g_landingcommand = SFbuf[1];
- //pc.printf("%c",g_landingcommand);
- //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);
- }
+ g_landingcommand = SFbuf[1];
+ //pc.printf("%c",g_landingcommand);
+ //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);
- }
+ 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);
- }
+ //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;
+ //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;
}
@@ -1258,29 +1258,29 @@
led4 = 0;
}
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
+ if(g_distance<180 && g_distance>0 ){
NVIC_DisableIRQ(EXTI9_5_IRQn);
- if(g_distance<180 && g_distance>0 ){
- NVIC_DisableIRQ(EXTI9_5_IRQn);
- THRcount++;
- if(THRcount>5) flg_ground = true;
+ 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_autoonPITCH;
}
- else THRcount = 0;
- NVIC_EnableIRQ(EXTI9_5_IRQn);
-
- if(flg_ground == true) {
- autopwm[THR] = SetTHRinRatio(0.6);
- targetAngle[PITCH] = g_autoonPITCH;
- }
- else {
- autopwm[THR] = minpwm[THR];
- targetAngle[PITCH] = g_glideloopPITCH;
- }
-
- NVIC_DisableIRQ(USART1_IRQn);
- if(!CheckSW_Up(Ch7)){
- flg_ground = false;
- }
- NVIC_EnableIRQ(USART1_IRQn);
+ else {
+ autopwm[THR] = minpwm[THR];
+ targetAngle[PITCH] = g_glideloopPITCH;
+ }
+
+ NVIC_DisableIRQ(USART1_IRQn);
+ if(!CheckSW_Up(Ch7)){
+ flg_ground = false;
+ }
+ NVIC_EnableIRQ(USART1_IRQn);
}
//自動滑空
@@ -1331,33 +1331,33 @@
led4 = 0;
}
+ NVIC_DisableIRQ(EXTI9_5_IRQn);
+ if(t_diff > 22){
+ autopwm[THR] = SetTHRinRatio(0.6);
+ targetAngle[PITCH] = g_autoonPITCH;
+ }
+ else if(g_distance<180 && g_distance>0 ){
NVIC_DisableIRQ(EXTI9_5_IRQn);
- if(t_diff > 22){
- autopwm[THR] = SetTHRinRatio(0.6);
- targetAngle[PITCH] = g_autoonPITCH;
- }
- else if(g_distance<180 && g_distance>0 ){
- NVIC_DisableIRQ(EXTI9_5_IRQn);
- THRcount++;
- if(THRcount>5) flg_ground = true;
+ 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_autoonPITCH;
}
- else THRcount = 0;
- NVIC_EnableIRQ(EXTI9_5_IRQn);
-
- if(flg_ground == true) {
- autopwm[THR] = SetTHRinRatio(0.6);
- targetAngle[PITCH] = g_autoonPITCH;
- }
- else {
- autopwm[THR] = minpwm[THR];
- targetAngle[PITCH] = g_glideloopPITCH;
- }
-
- NVIC_DisableIRQ(USART1_IRQn);
- if(!CheckSW_Up(Ch7)){
- flg_ground = false;
- }
- NVIC_EnableIRQ(USART1_IRQn);
+ else {
+ autopwm[THR] = minpwm[THR];
+ targetAngle[PITCH] = g_glideloopPITCH;
+ }
+
+ NVIC_DisableIRQ(USART1_IRQn);
+ if(!CheckSW_Up(Ch7)){
+ flg_ground = false;
+ }
+ NVIC_EnableIRQ(USART1_IRQn);
}*/
//離陸-投下-着陸一連
@@ -1394,7 +1394,7 @@
break;
//case Chicken:
- //break;
+ //break;
/*
case Transition:
static int ApproachCount = 0;
@@ -1499,155 +1499,154 @@
void ReturnChickenServo2(){
autopwm[DROP] = 1392;
pc.printf("second reverse\r\n");
- }
+}
//着陸モード（PCからの指令に従う）
void UpdateTargetAngle_Approach(float targetAngle[3]){
-
- static bool zeroTHR=true;//着陸時のスロットル動作確認用
-
- NVIC_DisableIRQ(USART2_IRQn);
-
- if(CheckSW_Up(Ch7)){
- output_status = Auto;
- led1 = 1;
- }else{
- output_status = Manual;
- led1 = 0;
- zeroTHR=true;
- //g_landingcommand='G';
- }
+
+ static bool zeroTHR=true;//着陸時のスロットル動作確認用
+
+ NVIC_DisableIRQ(USART2_IRQn);
+
+ if(CheckSW_Up(Ch7)){
+ output_status = Auto;
+ led1 = 1;
+ }else{
+ output_status = Manual;
+ led1 = 0;
+ zeroTHR=true;
+ //g_landingcommand='G';
+ }
-
- switch(g_landingcommand){
- case 'R': //右旋回セヨ
+
+ switch(g_landingcommand){
+ case 'R': //右旋回セヨ
NVIC_EnableIRQ(USART2_IRQn);
//if(zeroTHR==false){
UpdateTargetAngle_Rightloop_zero(targetAngle);
- /*}
- else{
+ /*}
+ 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;
+ 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;
- }
- }*/
+ else {autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+ autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
+ }*/
- break;
+ break;
+
+ case 'L': //左旋回セヨ
+ NVIC_EnableIRQ(USART2_IRQn);
+ //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;
+ }*/
+
+ break;
- case 'L': //左旋回セヨ
- NVIC_EnableIRQ(USART2_IRQn);
- //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;
- }*/
+ case 'G': //直進セヨ
+ NVIC_EnableIRQ(USART2_IRQn);
+ //if(zeroTHR==false){
+ UpdateTargetAngle_GoStraight_zero(targetAngle);
+ /* }
+ else{
+ targetAngle[ROLL] = g_gostraightROLL;
+ targetAngle[PITCH] = g_gostraightPITCH;
+ }*/
+
+ break;
+
+ case 'Y': //指定ノヨー方向ニ移動セヨ
+ NVIC_EnableIRQ(USART2_IRQn);
+ Rotate(targetAngle, g_SerialTargetYAW);
+ //if(zeroTHR==false){
+ autopwm[THR]=minpwm[THR];
+ //}
+ break;
- break;
-
- case 'G': //直進セヨ
- NVIC_EnableIRQ(USART2_IRQn);
- //if(zeroTHR==false){
- UpdateTargetAngle_GoStraight_zero(targetAngle);
- /* }
- else{
- targetAngle[ROLL] = g_gostraightROLL;
- targetAngle[PITCH] = g_gostraightPITCH;
- }*/
-
- break;
-
- case 'Y': //指定ノヨー方向ニ移動セヨ
- NVIC_EnableIRQ(USART2_IRQn);
- Rotate(targetAngle, g_SerialTargetYAW);
- //if(zeroTHR==false){
- autopwm[THR]=minpwm[THR];
- // }
- break;
+ case 'U': //機首ヲ上ゲヨ
+ NVIC_EnableIRQ(USART2_IRQn);
+ static int UpCounter=0;
+ UpCounter++;
+ if(UpCounter==3){
+ while(1){
+ //targetAngle[ROLL] = g_gostraightROLL;
+ autopwm[THR] = minpwm[THR];
+ autopwm[ELE] = minpwm[ELE];
+ autopwm[RUD]=trimpwm[RUD];
+ if(CheckSW_Up(Ch7)){
+ output_status = Auto;
+ led1 = 1;
+ }else{
+ output_status = Manual;
+ led1 = 0;
+ zeroTHR=true;
+ //g_landingcommand='G';
+ }
+ }
- case 'U': //機首ヲ上ゲヨ
- NVIC_EnableIRQ(USART2_IRQn);
- static int UpCounter=0;
- UpCounter++;
- if(UpCounter==3){
- while(1){
- //targetAngle[ROLL] = g_gostraightROLL;
- autopwm[THR] = minpwm[THR];
- autopwm[ELE] = minpwm[ELE];
- autopwm[RUD]=trimpwm[RUD];
- if(CheckSW_Up(Ch7)){
- output_status = Auto;
- led1 = 1;
- }else{
- output_status = Manual;
- led1 = 0;
- zeroTHR=true;
- //g_landingcommand='G';
- }
- }
-
- }
-
- break;
+ }
+
+ break;
+
+ /*case 'B': //ブザーヲ鳴ラセ
+ //buzzer = 1;
+ NVIC_EnableIRQ(USART2_IRQn);
+ break;*/
- /*case 'B': //ブザーヲ鳴ラセ
- //buzzer = 1;
- NVIC_EnableIRQ(USART2_IRQn);
- break;*/
-
- case 'B': //物資ヲ落トセ
- NVIC_EnableIRQ(USART2_IRQn);
- Chicken_Drop();
-
- break;
-
- case 'C': //停止セヨ
- NVIC_EnableIRQ(USART2_IRQn);
- targetAngle[ROLL] = g_gostraightROLL;
- targetAngle[PITCH] = -3.0;
- autopwm[THR] = minpwm[THR];
- zeroTHR=false;
- break;
-
- default :
- NVIC_EnableIRQ(USART2_IRQn);
- break;
+ case 'B': //物資ヲ落トセ
+ NVIC_EnableIRQ(USART2_IRQn);
+ Chicken_Drop();
+
+ break;
+
+ case 'C': //停止セヨ
+ NVIC_EnableIRQ(USART2_IRQn);
+ targetAngle[ROLL] = g_gostraightROLL;
+ targetAngle[PITCH] = -3.0;
+ autopwm[THR] = minpwm[THR];
+ zeroTHR=false;
+ break;
+ default :
+ NVIC_EnableIRQ(USART2_IRQn);
+ break;
- }
-
+
+ }
+
}
void checkHeight(float targetAngle[3]){
- static int targetHeight = 200;
+ 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);
+ 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]){
+void UpdateTargetAngle_NoseUP(float targetAngle[3]){
//targetAngle[PITCH] += 2.0f;
//if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
@@ -1700,16 +1699,16 @@
//pc.printf("tagetAngle is changed.");
targetAngle[ROLL] = rightloopROLL2;
}
- else {
- t2.stop();
- t2.reset();
- pc.printf("Timer stopped.");
- targetAngle[ROLL] = g_rightloopROLL;
- }
+ 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;
@@ -1725,10 +1724,10 @@
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;
+ 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]);
@@ -1800,7 +1799,7 @@
for(uint8_t i=0; i<8; i++) pc.printf("ch.%d = %d ",i+1,sbus.manualpwm[i]);
pc.printf("\r\n");
- }
+}