syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

main.cpp@8:1db19b529b22, 2021-02-21 (annotated)

Committer:
komaida424
Date:
Sun Feb 21 05:14:57 2021 +0000
Revision:
8:1db19b529b22
Parent:
7:16bf0085d914 
rev 020

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 4:4060309b9cc0 1 /* PWM Output
komaida424 4:4060309b9cc0 2 type | M1 | M2 | M3 | M4 | M5 | M6
komaida424 4:4060309b9cc0 3 ------------+-----------+-----------+-----------+-----------+---------------------
komaida424 4:4060309b9cc0 4 Quad-X |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | -
komaida424 4:4060309b9cc0 5 Quad-H |FL VP Ser |FR VP Ser |BL VP Ser |BR VP Ser |Thr ESC | -
komaida424 6:a50e6d3924f1 6 Quad-3D |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | -
komaida424 6:a50e6d3924f1 7 Delta |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser | - | -
komaida424 6:a50e6d3924f1 8 Delta-TW |FL Tro ESC |L Alv Ser |R Alv Ser |Rud Ser |FR Thr ESC | -
komaida424 4:4060309b9cc0 9 Airplane |Thr ESC |Ail Ser |Ele Ser |Rud Ser |Ail Ser | -
komaida424 4:4060309b9cc0 10
komaida424 6:a50e6d3924f1 11 F:Front, B:Back, L:Left, R:Right
komaida424 6:a50e6d3924f1 12 Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Alv:Alevon
komaida424 4:4060309b9cc0 13 ESC:for ESC, Ser:for Servo, VP:Variable Pitch
komaida424 4:4060309b9cc0 14 */
komaida424 4:4060309b9cc0 15
komaida424 0:cca1c4e84da4 16 #include "mbed.h"
komaida424 2:59ac9df97701 17 #include "math.h"
komaida424 0:cca1c4e84da4 18 #include "I2cPeripherals.h"
komaida424 0:cca1c4e84da4 19 #include "InterruptIn.h"
komaida424 0:cca1c4e84da4 20 #include "config.h"
komaida424 0:cca1c4e84da4 21 #include "PulseWidthCounter.h"
komaida424 0:cca1c4e84da4 22 #include "string"
komaida424 0:cca1c4e84da4 23 #include "SerialLcd.h"
komaida424 8:1db19b529b22 24
komaida424 8:1db19b529b22 25 //#if defined(TARGET_NUCLEO_FXXXXX)
komaida424 8:1db19b529b22 26 // #include "eeprom_flash.h"
komaida424 8:1db19b529b22 27 //#endif
komaida424 4:4060309b9cc0 28 //#include "PID.h"
komaida424 8:1db19b529b22 29 //#include "SoftPWM.h"
komaida424 4:4060309b9cc0 30 #include "Limiter.h"
komaida424 8:1db19b529b22 31 #include "IAP.h"
komaida424 2:59ac9df97701 32
komaida424 8:1db19b529b22 33 #define DEBUG
komaida424 2:59ac9df97701 34 //Serial pc(USBTX, USBRX);
komaida424 8:1db19b529b22 35 //Serial pc(PA_9,PA_10);
komaida424 2:59ac9df97701 36
komaida424 4:4060309b9cc0 37 #if defined(TARGET_LPC1768)
komaida424 4:4060309b9cc0 38 DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
komaida424 4:4060309b9cc0 39 // #ifdef LPCXpresso
komaida424 6:a50e6d3924f1 40 #define LED1 P0_22
komaida424 4:4060309b9cc0 41 // #endif
komaida424 4:4060309b9cc0 42 DigitalOut led1(LED1);
komaida424 4:4060309b9cc0 43 // DigitalOut led2(LED2);
komaida424 4:4060309b9cc0 44 InterruptIn ch1(p5);
komaida424 8:1db19b529b22 45 PulseWidthCounter ch[6] = { PulseWidthCounter(p6),
komaida424 8:1db19b529b22 46 PulseWidthCounter(p7),
komaida424 8:1db19b529b22 47 PulseWidthCounter(p8),
komaida424 8:1db19b529b22 48 PulseWidthCounter(p9),
komaida424 8:1db19b529b22 49 PulseWidthCounter(p10),
komaida424 8:1db19b529b22 50 PulseWidthCounter(p11) };
komaida424 4:4060309b9cc0 51 PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 };
komaida424 8:1db19b529b22 52 // SoftPWM buzz(p20);
komaida424 4:4060309b9cc0 53 I2cPeripherals i2c(p28,p27); //sda scl
komaida424 4:4060309b9cc0 54 SerialLcd lcd(p13,p14);
komaida424 8:1db19b529b22 55 #define PwmNum 6
komaida424 4:4060309b9cc0 56 #define MEM_SIZE 256
komaida424 4:4060309b9cc0 57 #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
komaida424 4:4060309b9cc0 58 IAP iap;
komaida424 8:1db19b529b22 59 #elif defined(TARGET_STM32F1)
komaida424 8:1db19b529b22 60 //#define NAZE32BORD
komaida424 8:1db19b529b22 61 #ifdef NAZE32BORD
komaida424 8:1db19b529b22 62 DigitalOut led1(PC_13);
komaida424 8:1db19b529b22 63 InterruptIn ch1(PA_0);
komaida424 8:1db19b529b22 64 PulseWidthCounter ch[6] = { PA_8,PA_11,PB_6,PB_7,PB_8,PB_9};
komaida424 8:1db19b529b22 65 PwmOut pwm[6] = { PA_0,PA_1,PA_2,PA_3,PA_6,PA_7 };
komaida424 8:1db19b529b22 66 // SoftPWM buzz(PA_2);
komaida424 8:1db19b529b22 67 // I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
komaida424 8:1db19b529b22 68 I2cPeripherals i2c(PB_9,PB_8); //sda scl
komaida424 8:1db19b529b22 69 SerialLcd lcd(PA_2);
komaida424 8:1db19b529b22 70 #define PwmNum 6
komaida424 8:1db19b529b22 71 #define MEM_SIZE 256
komaida424 8:1db19b529b22 72 #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
komaida424 8:1db19b529b22 73 #else
komaida424 8:1db19b529b22 74 DigitalOut led1(PC_13);
komaida424 8:1db19b529b22 75 InterruptIn ch1(PA_0);
komaida424 8:1db19b529b22 76 PulseWidthCounter ch[6] = { PA_11,PA_12,PA_15,PB_3,PB_4,PB_5};
komaida424 8:1db19b529b22 77 PwmOut pwm[6] = { PA_6,PA_7,PB_0,PB_1,PB_10,PB_11 };
komaida424 8:1db19b529b22 78 // SoftPWM buzz(PA_2);
komaida424 8:1db19b529b22 79 // I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
komaida424 8:1db19b529b22 80 I2cPeripherals i2c(PB_9,PB_8); //sda scl
komaida424 8:1db19b529b22 81 SerialLcd lcd(PA_2);
komaida424 8:1db19b529b22 82 #define PwmNum 6
komaida424 8:1db19b529b22 83 #define MEM_SIZE 256
komaida424 8:1db19b529b22 84 #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
komaida424 8:1db19b529b22 85 #endif
komaida424 8:1db19b529b22 86 #elif defined(TARGET_STM32F3)
komaida424 8:1db19b529b22 87 DigitalOut led1(PB_3);
komaida424 8:1db19b529b22 88 InterruptIn ch1(PA_0);
komaida424 8:1db19b529b22 89 PulseWidthCounter ch[6] = { PA_0,PA_1,PB_11,PB_10,PB_4,PB_5};
komaida424 8:1db19b529b22 90 PwmOut pwm[6] = { PA_6,PA_7,PA_11,PA_12,PB_8,PB_9 };
komaida424 8:1db19b529b22 91 // SoftPWM buzz(PA_2);
komaida424 8:1db19b529b22 92 // I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
komaida424 8:1db19b529b22 93 I2cPeripherals i2c(PB_7,PB_6); //sda scl
komaida424 8:1db19b529b22 94 SerialLcd lcd(PA_9);
komaida424 8:1db19b529b22 95 #define PwmNum 6
komaida424 8:1db19b529b22 96 #define MEM_SIZE 256
komaida424 8:1db19b529b22 97 #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
komaida424 8:1db19b529b22 98 #elif defined(TARGET_NUCLEO_F4)
komaida424 8:1db19b529b22 99 DigitalOut led1(PA_5);
komaida424 4:4060309b9cc0 100 InterruptIn ch1(PC_2);
komaida424 4:4060309b9cc0 101 // PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 };
komaida424 4:4060309b9cc0 102 PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 };
komaida424 8:1db19b529b22 103 PwmOut pwm[6] = { D3,D5,D6,D9,D11,D12 };
komaida424 8:1db19b529b22 104 // SoftPWM buzz(PB_13);
komaida424 4:4060309b9cc0 105 // I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
komaida424 8:1db19b529b22 106 I2cPeripherals i2c(PB_9,PB_8); //sda scl
komaida424 8:1db19b529b22 107 SerialLcd lcd(PA_11);
komaida424 8:1db19b529b22 108 #define PwmNum 6
komaida424 4:4060309b9cc0 109 #define MEM_SIZE 256
komaida424 8:1db19b529b22 110 #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
komaida424 4:4060309b9cc0 111 #elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
komaida424 4:4060309b9cc0 112 DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 };
komaida424 4:4060309b9cc0 113 DigitalOut led1(P0_21);
komaida424 4:4060309b9cc0 114 // DigitalOut led2(P0_21);
komaida424 4:4060309b9cc0 115 InterruptIn ch1(P0_9);
komaida424 4:4060309b9cc0 116 PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 };
komaida424 8:1db19b529b22 117 PwmOut pwm[4] = { P0_14,P0_2,P0_23,P0_17 };
komaida424 8:1db19b529b22 118 // SoftPWM buzz(P1_19);
komaida424 4:4060309b9cc0 119 I2cPeripherals i2c(P0_5,P0_4); //sda scl
komaida424 4:4060309b9cc0 120 SerialLcd lcd(P0_19,P0_18);
komaida424 8:1db19b529b22 121 #define PwmNum 4
komaida424 4:4060309b9cc0 122 #define MEM_SIZE 256
komaida424 4:4060309b9cc0 123 #define TARGET_EEPROM_ADDRESS 64
komaida424 4:4060309b9cc0 124 #define INTERNAL_EEPROM
komaida424 4:4060309b9cc0 125 IAP iap;
komaida424 4:4060309b9cc0 126 #elif defined(TARGET_LPC1114) // LPC1114
komaida424 8:1db19b529b22 127 // DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 };
komaida424 4:4060309b9cc0 128 DigitalOut led1(dp28);
komaida424 4:4060309b9cc0 129 InterruptIn ch1(dp4);
komaida424 8:1db19b529b22 130 // PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 };
komaida424 8:1db19b529b22 131 PwmOut pwm[4] = { dp1,dp2,dp18,dp24 };
komaida424 8:1db19b529b22 132 // SoftPWM buzz(dp25);
komaida424 4:4060309b9cc0 133 I2cPeripherals i2c(dp5,dp27); //sda scl
komaida424 4:4060309b9cc0 134 SerialLcd lcd(dp16,dp15);
komaida424 8:1db19b529b22 135 #define PwmNum 4
komaida424 8:1db19b529b22 136 #define MEM_SIZE 256
komaida424 8:1db19b529b22 137 #define EXTERNAL_EEPROM
komaida424 8:1db19b529b22 138 #elif defined(TARGET_TEENSY3_1) // Teensy3.1
komaida424 8:1db19b529b22 139 DigitalOut led1(D13);
komaida424 8:1db19b529b22 140 InterruptIn ch1(D2);
komaida424 8:1db19b529b22 141 PwmOut pwm[6] = { D3,D4,D5,D6,d20,D21 };
komaida424 8:1db19b529b22 142 I2cPeripherals i2c(D18,D19); //sda scl
komaida424 8:1db19b529b22 143 SerialLcd lcd(D3,D4); //TX,RX
komaida424 8:1db19b529b22 144 #define PwmNum 6
komaida424 4:4060309b9cc0 145 #define MEM_SIZE 256
komaida424 4:4060309b9cc0 146 #define EXTERNAL_EEPROM
komaida424 4:4060309b9cc0 147 #endif
komaida424 0:cca1c4e84da4 148
komaida424 0:cca1c4e84da4 149 Timer CurTime;
komaida424 2:59ac9df97701 150 //Timer ElapTime;
komaida424 2:59ac9df97701 151 Timer CycleTime;
komaida424 8:1db19b529b22 152 //Timer FlyghtTime;
komaida424 0:cca1c4e84da4 153 config conf;
komaida424 4:4060309b9cc0 154 //PID pid[4];
komaida424 8:1db19b529b22 155 //Limiter throLimit = 100;
komaida424 8:1db19b529b22 156 Limiter gyroLimit[3] = {0.9,0.9,0.9};
komaida424 8:1db19b529b22 157 Limiter accLimit[3] = {0.95,0.95,0.95};
komaida424 8:1db19b529b22 158 //Limiter pwmLimit[4] = {50,50,50,50};
komaida424 4:4060309b9cc0 159 //PID height;
komaida424 2:59ac9df97701 160 float TotalTime = 0;;
komaida424 2:59ac9df97701 161 int channel = 0;
komaida424 4:4060309b9cc0 162 int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 };
komaida424 4:4060309b9cc0 163 volatile int CH[9];
komaida424 2:59ac9df97701 164 volatile int M[6];
komaida424 2:59ac9df97701 165 volatile float Gyro[3];
komaida424 4:4060309b9cc0 166 volatile float Accel[3]= {0,0,0};
komaida424 4:4060309b9cc0 167 volatile float Accel_Angle[3];
komaida424 4:4060309b9cc0 168 volatile float Accel_Save[3]= {0,0,0};
komaida424 2:59ac9df97701 169 volatile float Angle[3];
komaida424 2:59ac9df97701 170 volatile float Gyro_Ref[3];
komaida424 2:59ac9df97701 171 volatile float Gyro_Save[3];
komaida424 4:4060309b9cc0 172 volatile int Stick[6];
komaida424 4:4060309b9cc0 173 volatile int Stick_Save[6];
komaida424 6:a50e6d3924f1 174 int PWM_Init[6][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X
komaida424 6:a50e6d3924f1 175 1500,1500,1500,1500,1080,1080, //Quad_VP
komaida424 6:a50e6d3924f1 176 1500,1500,1500,1500,1080,1080, //Quad_3D
komaida424 4:4060309b9cc0 177 1080,1500,1500,1500,1500,1080, //Delta
komaida424 4:4060309b9cc0 178 1080,1500,1500,1500,1080,1500, //Delta_TW
komaida424 4:4060309b9cc0 179 1080,1500,1500,1500,1500,1080 //AirPlane
komaida424 4:4060309b9cc0 180 };
komaida424 6:a50e6d3924f1 181 char Servo_idx[6][6] = { 0,0,0,0,0,0, //Quad_X
komaida424 6:a50e6d3924f1 182 1,1,1,1,0,0, //Quad_VP
komaida424 6:a50e6d3924f1 183 0,0,0,0,0,0, //Quad_3D
komaida424 6:a50e6d3924f1 184 0,1,1,1,1,1, //Delta
komaida424 6:a50e6d3924f1 185 0,1,1,1,0,1, //Delta_TW
komaida424 6:a50e6d3924f1 186 0,1,1,1,1,1 //AirPlane
komaida424 6:a50e6d3924f1 187 };
komaida424 2:59ac9df97701 188 //int Stick_Max[3];
komaida424 0:cca1c4e84da4 189 float Press;
komaida424 4:4060309b9cc0 190 float Base_Press;
komaida424 0:cca1c4e84da4 191 char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
komaida424 2:59ac9df97701 192 //volatile bool tick_flag;
komaida424 2:59ac9df97701 193 //volatile bool buzz_flag;
komaida424 4:4060309b9cc0 194 volatile float interval;
komaida424 3:27407c4984cf 195 float pid_interval;
komaida424 4:4060309b9cc0 196 //int pid_reg[4];
komaida424 2:59ac9df97701 197 int loop_cnt;
komaida424 4:4060309b9cc0 198 float target_height;
komaida424 4:4060309b9cc0 199 float cuurent_height;
komaida424 6:a50e6d3924f1 200 float base_Throttl;
komaida424 6:a50e6d3924f1 201 int Throttl;
komaida424 4:4060309b9cc0 202 bool hov_control;
komaida424 4:4060309b9cc0 203 float Rdata;
komaida424 0:cca1c4e84da4 204
komaida424 0:cca1c4e84da4 205 void initialize();
komaida424 8:1db19b529b22 206 void FlashLED(int ,float tm=0.1);
komaida424 0:cca1c4e84da4 207 void SetUp();
komaida424 0:cca1c4e84da4 208 void SetUpPrompt(config&,I2cPeripherals&);
komaida424 0:cca1c4e84da4 209 void PWM_Out(bool);
komaida424 0:cca1c4e84da4 210 void Get_Stick_Pos();
komaida424 0:cca1c4e84da4 211 void CalibrateGyros(void);
komaida424 0:cca1c4e84da4 212 void CalibrateAccel(void);
komaida424 3:27407c4984cf 213 void Get_Gyro(float);
komaida424 3:27407c4984cf 214 bool Get_Accel(float);
komaida424 2:59ac9df97701 215 void Get_Angle(float);
komaida424 0:cca1c4e84da4 216 void ReadConfig();
komaida424 0:cca1c4e84da4 217 void WriteConfig();
komaida424 2:59ac9df97701 218 void Interrupt_Check(bool&,int &,DigitalOut &);
komaida424 0:cca1c4e84da4 219 void ESC_SetUp(void);
komaida424 2:59ac9df97701 220 void Flight_SetUp();
komaida424 2:59ac9df97701 221 //void IMUfilter_Reset(void);
komaida424 2:59ac9df97701 222 void LCD_printf(char *);
komaida424 2:59ac9df97701 223 void LCD_cls();
komaida424 2:59ac9df97701 224 void LCD_locate(int,int);
komaida424 8:1db19b529b22 225 void wait(float);
komaida424 8:1db19b529b22 226
komaida424 4:4060309b9cc0 227 void PulseCheck() //cppm信号のチェック
komaida424 0:cca1c4e84da4 228 {
komaida424 2:59ac9df97701 229 channel++;
komaida424 0:cca1c4e84da4 230 }
komaida424 0:cca1c4e84da4 231
komaida424 4:4060309b9cc0 232 void PulseAnalysis() //cppm信号の解析
komaida424 0:cca1c4e84da4 233 {
komaida424 0:cca1c4e84da4 234 CurTime.stop();
komaida424 0:cca1c4e84da4 235 int PulseWidth = CurTime.read_us();
komaida424 0:cca1c4e84da4 236 CurTime.reset();
komaida424 0:cca1c4e84da4 237 CurTime.start();
komaida424 2:59ac9df97701 238 if ( PulseWidth > 3000 ) channel = 0; //reset pulse count
komaida424 0:cca1c4e84da4 239 else {
komaida424 4:4060309b9cc0 240 if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) {
komaida424 4:4060309b9cc0 241 CH[Signal[channel-1]] = PulseWidth;
komaida424 4:4060309b9cc0 242 }
komaida424 0:cca1c4e84da4 243 }
komaida424 2:59ac9df97701 244 channel++;
komaida424 0:cca1c4e84da4 245 }
komaida424 0:cca1c4e84da4 246
komaida424 0:cca1c4e84da4 247 int main()
komaida424 0:cca1c4e84da4 248 {
komaida424 2:59ac9df97701 249 int i=0;
komaida424 0:cca1c4e84da4 250
komaida424 3:27407c4984cf 251 wait(1.5);
komaida424 0:cca1c4e84da4 252 initialize();
komaida424 2:59ac9df97701 253
komaida424 0:cca1c4e84da4 254 Get_Stick_Pos();
komaida424 8:1db19b529b22 255 #ifdef DEBUG
komaida424 8:1db19b529b22 256 char str[18];
komaida424 8:1db19b529b22 257 while ( THR > 1800 ) {
komaida424 8:1db19b529b22 258 LCD_locate(0,0);
komaida424 8:1db19b529b22 259 sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
komaida424 8:1db19b529b22 260 LCD_printf(str);
komaida424 8:1db19b529b22 261 LCD_locate(0,1);
komaida424 8:1db19b529b22 262 sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
komaida424 8:1db19b529b22 263 LCD_printf(str);
komaida424 8:1db19b529b22 264 Get_Stick_Pos();
komaida424 8:1db19b529b22 265 }
komaida424 8:1db19b529b22 266 #endif
komaida424 6:a50e6d3924f1 267 while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C'
komaida424 6:a50e6d3924f1 268 || ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) ) //Shrottol Low
komaida424 0:cca1c4e84da4 269 {
komaida424 4:4060309b9cc0 270 // if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
komaida424 4:4060309b9cc0 271 // ESC_SetUp();
komaida424 2:59ac9df97701 272 if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High
komaida424 0:cca1c4e84da4 273 {
komaida424 2:59ac9df97701 274 loop_cnt = 0;
komaida424 0:cca1c4e84da4 275 SetUpPrompt(conf,i2c);
komaida424 0:cca1c4e84da4 276 break;
komaida424 0:cca1c4e84da4 277 }
komaida424 6:a50e6d3924f1 278 FlashLED(2);
komaida424 6:a50e6d3924f1 279 wait(0.5);
komaida424 0:cca1c4e84da4 280 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 281 }
komaida424 2:59ac9df97701 282 Flight_SetUp();
komaida424 0:cca1c4e84da4 283 while (1)
komaida424 0:cca1c4e84da4 284 {
komaida424 2:59ac9df97701 285 if ( Stick[COL] < Thro_Zero )
komaida424 0:cca1c4e84da4 286 {
komaida424 0:cca1c4e84da4 287 i = 0;
komaida424 2:59ac9df97701 288 while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero ) //Rudder Left
komaida424 0:cca1c4e84da4 289 {
komaida424 0:cca1c4e84da4 290 if ( i > 100 ) //wait 2 sec
komaida424 0:cca1c4e84da4 291 {
komaida424 8:1db19b529b22 292 // FlyghtTime.stop();
komaida424 2:59ac9df97701 293 if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down
komaida424 2:59ac9df97701 294 loop_cnt = 0;
komaida424 2:59ac9df97701 295 FlashLED(5);
komaida424 4:4060309b9cc0 296
komaida424 8:1db19b529b22 297 for ( int x=0; x<PwmNum; x++ ) {
komaida424 4:4060309b9cc0 298 pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]);
komaida424 4:4060309b9cc0 299 }
komaida424 2:59ac9df97701 300 i2c.start(conf.LCD_Contrast);
komaida424 2:59ac9df97701 301 SetUpPrompt(conf,i2c);
komaida424 2:59ac9df97701 302 }
komaida424 0:cca1c4e84da4 303 CalibrateGyros();
komaida424 2:59ac9df97701 304 Flight_SetUp();
komaida424 0:cca1c4e84da4 305 break;
komaida424 0:cca1c4e84da4 306 }
komaida424 0:cca1c4e84da4 307 wait(0.01); // wait 10 msec
komaida424 0:cca1c4e84da4 308 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 309 i++;
komaida424 0:cca1c4e84da4 310 }
komaida424 0:cca1c4e84da4 311 }
komaida424 0:cca1c4e84da4 312 PWM_Out(true);
komaida424 0:cca1c4e84da4 313 }
komaida424 0:cca1c4e84da4 314
komaida424 0:cca1c4e84da4 315 }
komaida424 0:cca1c4e84da4 316
komaida424 0:cca1c4e84da4 317 void initialize()
komaida424 0:cca1c4e84da4 318 {
komaida424 4:4060309b9cc0 319 i2c.start(conf.LCD_Contrast);
komaida424 8:1db19b529b22 320 for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
komaida424 0:cca1c4e84da4 321 ReadConfig(); //config.inf file read
komaida424 2:59ac9df97701 322
komaida424 2:59ac9df97701 323 channel = 0;
komaida424 8:1db19b529b22 324 //#if defined(TARGET_LPC1114) // LPC1114
komaida424 8:1db19b529b22 325
komaida424 0:cca1c4e84da4 326 ch1.rise(&PulseCheck); //input pulse count
komaida424 2:59ac9df97701 327 wait(0.2);
komaida424 2:59ac9df97701 328 if ( channel > 50 ) {
komaida424 8:1db19b529b22 329 FlashLED(50,0.02);
komaida424 0:cca1c4e84da4 330 ch1.rise(&PulseAnalysis);
komaida424 8:1db19b529b22 331 InPulseMode = 'S'; }
komaida424 0:cca1c4e84da4 332 else InPulseMode = 'P';
komaida424 8:1db19b529b22 333 //#endif
komaida424 2:59ac9df97701 334 led1 = 0;
komaida424 2:59ac9df97701 335 CycleTime.start();
komaida424 8:1db19b529b22 336 // throLimit.differential(conf.Thro_Limit_Val);
komaida424 8:1db19b529b22 337 // throLimit.rate(conf.Thro_Limit_Rate);
komaida424 8:1db19b529b22 338 // Base_Press = (float)i2c.pressure() / 4096;
komaida424 8:1db19b529b22 339 FlashLED(10,0.05);
komaida424 0:cca1c4e84da4 340 }
komaida424 0:cca1c4e84da4 341
komaida424 8:1db19b529b22 342 void FlashLED(int cnt,float tm)
komaida424 0:cca1c4e84da4 343 {
komaida424 0:cca1c4e84da4 344 for ( int i = 0 ; i < cnt ; i++ ) {
komaida424 0:cca1c4e84da4 345 led1 = !led1;
komaida424 8:1db19b529b22 346 // buzz = 0.5f;
komaida424 8:1db19b529b22 347 wait(tm);
komaida424 0:cca1c4e84da4 348 led1 = !led1;
komaida424 8:1db19b529b22 349 // buzz = 0.0f;
komaida424 8:1db19b529b22 350 wait(tm);
komaida424 0:cca1c4e84da4 351 }
komaida424 0:cca1c4e84da4 352 }
komaida424 0:cca1c4e84da4 353
komaida424 0:cca1c4e84da4 354 void ReadConfig()
komaida424 0:cca1c4e84da4 355 {
komaida424 0:cca1c4e84da4 356 #ifdef LocalFileOut
komaida424 0:cca1c4e84da4 357 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
komaida424 0:cca1c4e84da4 358 FILE *fp = fopen("/local/setup.inf", "rb"); // Open "out.txt" on the local file system for writing
komaida424 0:cca1c4e84da4 359 if ( fp != NULL ) {
komaida424 0:cca1c4e84da4 360 float rev = conf.Revision;
komaida424 0:cca1c4e84da4 361 int len = fread(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 362 switch ( len ) {
komaida424 0:cca1c4e84da4 363 case sizeof(config): // File size ok
komaida424 0:cca1c4e84da4 364 if ( rev == conf.Revision ) break;
komaida424 0:cca1c4e84da4 365 default:
komaida424 0:cca1c4e84da4 366 fclose(fp);
komaida424 0:cca1c4e84da4 367 config init;
komaida424 0:cca1c4e84da4 368 conf = init;
komaida424 0:cca1c4e84da4 369 fp = fopen("/local/setup.inf", "wb");
komaida424 0:cca1c4e84da4 370 fwrite(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 371 }
komaida424 0:cca1c4e84da4 372 fclose(fp);
komaida424 0:cca1c4e84da4 373 } else {
komaida424 0:cca1c4e84da4 374 WriteConfig();
komaida424 0:cca1c4e84da4 375 wait(2);
komaida424 0:cca1c4e84da4 376 }
komaida424 0:cca1c4e84da4 377 #else
komaida424 0:cca1c4e84da4 378 char *send;
komaida424 0:cca1c4e84da4 379 char *recv;
komaida424 4:4060309b9cc0 380 int i;
komaida424 8:1db19b529b22 381 char buf[MEM_SIZE];
komaida424 0:cca1c4e84da4 382 config *conf_ptr;
komaida424 0:cca1c4e84da4 383
komaida424 4:4060309b9cc0 384 if ( sizeof(config) > MEM_SIZE ) {
komaida424 8:1db19b529b22 385 // pc.printf("config size over");
komaida424 2:59ac9df97701 386 wait(3);
komaida424 0:cca1c4e84da4 387 return;
komaida424 0:cca1c4e84da4 388 }
komaida424 4:4060309b9cc0 389 //#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
komaida424 8:1db19b529b22 390 #if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM) || defined(STM32_EEPROM)
komaida424 4:4060309b9cc0 391 #if defined(INTERNAL_EEPROM)
komaida424 4:4060309b9cc0 392 iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE );
komaida424 8:1db19b529b22 393 #elif defined(EXTERNAL_EEPROM)
komaida424 4:4060309b9cc0 394 //External Flash Memory Wreite
komaida424 4:4060309b9cc0 395 short pos = 0;
komaida424 4:4060309b9cc0 396 if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 ) {
komaida424 4:4060309b9cc0 397 while(1) {
komaida424 4:4060309b9cc0 398 FlashLED(3);
komaida424 4:4060309b9cc0 399 wait(0.5);
komaida424 4:4060309b9cc0 400 }
komaida424 4:4060309b9cc0 401 }
komaida424 8:1db19b529b22 402 #else //STM32 emulate eeprom
komaida424 8:1db19b529b22 403 // EEPROM_Read(0,buf,MEM_SIZE)
komaida424 8:1db19b529b22 404 readEEPROM(0,(uint32_t*)buf,sizeof(config));
komaida424 4:4060309b9cc0 405 #endif
komaida424 4:4060309b9cc0 406 send = buf;
komaida424 8:1db19b529b22 407 conf_ptr = (config*)buf;
komaida424 8:1db19b529b22 408 //pc.printf("rev=%f",conf_ptr->Revision);
komaida424 4:4060309b9cc0 409 recv = (char*)&conf;
komaida424 8:1db19b529b22 410 // conf_ptr = (config*)buf;
komaida424 4:4060309b9cc0 411 if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
komaida424 4:4060309b9cc0 412 for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
komaida424 4:4060309b9cc0 413 return;
komaida424 4:4060309b9cc0 414 }
komaida424 4:4060309b9cc0 415
komaida424 4:4060309b9cc0 416 #else
komaida424 4:4060309b9cc0 417 int rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 418 if ( rc == SECTOR_NOT_BLANK ) {
komaida424 0:cca1c4e84da4 419 send = sector_start_adress[TARGET_SECTOR];
komaida424 0:cca1c4e84da4 420 recv = (char*)&conf;
komaida424 0:cca1c4e84da4 421 conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
komaida424 0:cca1c4e84da4 422 if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
komaida424 0:cca1c4e84da4 423 for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
komaida424 0:cca1c4e84da4 424 return;
komaida424 0:cca1c4e84da4 425 }
komaida424 0:cca1c4e84da4 426 }
komaida424 4:4060309b9cc0 427 #endif
komaida424 0:cca1c4e84da4 428 WriteConfig();
komaida424 0:cca1c4e84da4 429 #endif
komaida424 0:cca1c4e84da4 430 }
komaida424 0:cca1c4e84da4 431
komaida424 0:cca1c4e84da4 432 void WriteConfig()
komaida424 0:cca1c4e84da4 433 {
komaida424 0:cca1c4e84da4 434 #ifdef LocalFileOut
komaida424 0:cca1c4e84da4 435 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
komaida424 0:cca1c4e84da4 436 FILE *fp = fopen("/local/setup.inf", "wb");
komaida424 0:cca1c4e84da4 437 fwrite(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 438 fclose(fp);
komaida424 0:cca1c4e84da4 439 #else
komaida424 8:1db19b529b22 440 char mem[MEM_SIZE]= " ";;
komaida424 0:cca1c4e84da4 441 char *send;
komaida424 0:cca1c4e84da4 442 int i;
komaida424 4:4060309b9cc0 443 if ( sizeof(config) > MEM_SIZE ) {
komaida424 8:1db19b529b22 444 // pc.printf("config size over");
komaida424 2:59ac9df97701 445 wait(3);
komaida424 0:cca1c4e84da4 446 return;
komaida424 0:cca1c4e84da4 447 }
komaida424 0:cca1c4e84da4 448 send = (char*)&conf;
komaida424 0:cca1c4e84da4 449 for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
komaida424 0:cca1c4e84da4 450 for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
komaida424 4:4060309b9cc0 451 //#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501)
komaida424 8:1db19b529b22 452 #if defined(INTERNAL_EEPROM)
komaida424 8:1db19b529b22 453 iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE );
komaida424 8:1db19b529b22 454 #elif defined(EXTERNAL_EEPROM)
komaida424 4:4060309b9cc0 455 //External Flash Memory Wreite
komaida424 8:1db19b529b22 456 short pos = 0;
komaida424 8:1db19b529b22 457 i2c.write_EEPROM( pos,mem,MEM_SIZE) ;
komaida424 8:1db19b529b22 458 #elif defined(STM32_EEPROM)
komaida424 8:1db19b529b22 459 // EEPROM_Write(0,buf,MEM_SIZE);
komaida424 8:1db19b529b22 460 enableEEPROMWriting();
komaida424 8:1db19b529b22 461 writeEEPROM(0, (uint32_t *)mem,sizeof(config));
komaida424 8:1db19b529b22 462 disableEEPROMWriting();
komaida424 8:1db19b529b22 463 // pc.printf("rev=%f,rev=%d",((config*)mem)->Revision,readEEPROMWord(0));
komaida424 8:1db19b529b22 464 #else
komaida424 8:1db19b529b22 465 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
komaida424 8:1db19b529b22 466 iap.erase( TARGET_SECTOR, TARGET_SECTOR );
komaida424 8:1db19b529b22 467 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
komaida424 8:1db19b529b22 468 iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
komaida424 8:1db19b529b22 469 #endif
komaida424 8:1db19b529b22 470 for ( i=0; i<4; i++ ) {
komaida424 8:1db19b529b22 471 FlashLED(10,0.03);
komaida424 8:1db19b529b22 472 wait(0.5);
komaida424 8:1db19b529b22 473 }
komaida424 0:cca1c4e84da4 474 #endif
komaida424 0:cca1c4e84da4 475 }
komaida424 0:cca1c4e84da4 476
komaida424 0:cca1c4e84da4 477 void Get_Stick_Pos(void)
komaida424 0:cca1c4e84da4 478 {
komaida424 8:1db19b529b22 479 #ifndef TARGET_LPC1114 // LPC1114
komaida424 0:cca1c4e84da4 480 if ( InPulseMode == 'P' ) {
komaida424 0:cca1c4e84da4 481 for (int i=0;i<5;i++) CH[i] = ch[i].count;
komaida424 0:cca1c4e84da4 482 }
komaida424 8:1db19b529b22 483 #endif
komaida424 2:59ac9df97701 484 // Stick_Save[ROL] = Stick[ROL];
komaida424 2:59ac9df97701 485 // Stick_Save[PIT] = Stick[PIT];
komaida424 2:59ac9df97701 486 // Stick_Save[YAW] = Stick[YAW];
komaida424 4:4060309b9cc0 487 // Stick_Save[COL] = Stick[COL];
komaida424 2:59ac9df97701 488
komaida424 0:cca1c4e84da4 489 Stick[ROL] = AIL - conf.Stick_Ref[ROL];
komaida424 0:cca1c4e84da4 490 Stick[PIT] = ELE - conf.Stick_Ref[PIT];
komaida424 0:cca1c4e84da4 491 Stick[YAW] = RUD - conf.Stick_Ref[YAW];
komaida424 0:cca1c4e84da4 492 Stick[COL] = THR - conf.Stick_Ref[COL];
komaida424 0:cca1c4e84da4 493 Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
komaida424 4:4060309b9cc0 494 Stick[AUX2] = AX2 - conf.Stick_Ref[COL];
komaida424 0:cca1c4e84da4 495 }
komaida424 0:cca1c4e84da4 496
komaida424 3:27407c4984cf 497 void Get_Gyro(float interval)
komaida424 0:cca1c4e84da4 498 {
komaida424 2:59ac9df97701 499 float x,y,z;
komaida424 2:59ac9df97701 500 bool err;
komaida424 2:59ac9df97701 501 Gyro_Save[ROL] = Gyro[ROL];
komaida424 2:59ac9df97701 502 Gyro_Save[PIT] = Gyro[PIT];
komaida424 2:59ac9df97701 503 Gyro_Save[YAW] = Gyro[YAW];
komaida424 2:59ac9df97701 504
komaida424 2:59ac9df97701 505 if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z);
komaida424 2:59ac9df97701 506 else err=i2c.angular(&y,&x,&z);
komaida424 2:59ac9df97701 507 if ( err == false ) return;
komaida424 4:4060309b9cc0 508 Gyro[ROL] = gyroLimit[0].calc(x) - Gyro_Ref[0] ;
komaida424 4:4060309b9cc0 509 Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ;
komaida424 4:4060309b9cc0 510 Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ;
komaida424 4:4060309b9cc0 511 //pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]);
komaida424 4:4060309b9cc0 512
komaida424 0:cca1c4e84da4 513 }
komaida424 0:cca1c4e84da4 514
komaida424 3:27407c4984cf 515 bool Get_Accel(float interval)
komaida424 2:59ac9df97701 516 {
komaida424 2:59ac9df97701 517 float x,y,z;
komaida424 3:27407c4984cf 518 bool err;
komaida424 4:4060309b9cc0 519 // Accel_Save[ROL] = Accel_Angle[ROL];
komaida424 4:4060309b9cc0 520 // Accel_Save[PIT] = Accel_Angle[PIT];
komaida424 4:4060309b9cc0 521 // Accel_Save[YAW] = Accel_Angle[YAW];
komaida424 2:59ac9df97701 522 if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
komaida424 2:59ac9df97701 523 else err=i2c.Acceleration(&y,&x,&z);
komaida424 2:59ac9df97701 524 if ( err == false ) return false;
komaida424 4:4060309b9cc0 525 //pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z);
komaida424 3:27407c4984cf 526
komaida424 6:a50e6d3924f1 527 x = accLimit[0].calc(x) - conf.Accel_Ref[0];
komaida424 6:a50e6d3924f1 528 y = accLimit[1].calc(y) - conf.Accel_Ref[1];
komaida424 6:a50e6d3924f1 529 z = accLimit[2].calc(z) - conf.Accel_Ref[2];
komaida424 4:4060309b9cc0 530 //pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]);
komaida424 4:4060309b9cc0 531 Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f;
komaida424 4:4060309b9cc0 532 Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f;
komaida424 4:4060309b9cc0 533 Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f;
komaida424 4:4060309b9cc0 534 return true;
komaida424 2:59ac9df97701 535 }
komaida424 2:59ac9df97701 536
komaida424 2:59ac9df97701 537 void Get_Angle(float interval)
komaida424 0:cca1c4e84da4 538 {
komaida424 2:59ac9df97701 539 float x,y,z;
komaida424 6:a50e6d3924f1 540 // Get_Accel(interval);
komaida424 3:27407c4984cf 541 Get_Gyro(interval);
komaida424 2:59ac9df97701 542
komaida424 4:4060309b9cc0 543 x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f;
komaida424 4:4060309b9cc0 544 y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f;
komaida424 4:4060309b9cc0 545 z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f;
komaida424 3:27407c4984cf 546 if ( Get_Accel(interval) == true ) {
komaida424 2:59ac9df97701 547 float i = 3.14 * 2 * conf.Cutoff_Freq * interval;
komaida424 2:59ac9df97701 548 Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
komaida424 2:59ac9df97701 549 Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
komaida424 2:59ac9df97701 550 }
komaida424 3:27407c4984cf 551 else {
komaida424 2:59ac9df97701 552 Angle[ROL] += x * interval;
komaida424 2:59ac9df97701 553 Angle[PIT] += y * interval;
komaida424 2:59ac9df97701 554 }
komaida424 2:59ac9df97701 555 Angle[YAW] += z * interval;
komaida424 4:4060309b9cc0 556 //pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]);
komaida424 0:cca1c4e84da4 557 }
komaida424 2:59ac9df97701 558
komaida424 0:cca1c4e84da4 559 void Get_Pressure()
komaida424 0:cca1c4e84da4 560 {
komaida424 4:4060309b9cc0 561 float P = (float)i2c.pressure()/4096;
komaida424 4:4060309b9cc0 562 // Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f );
komaida424 4:4060309b9cc0 563 Press = 8.43f * ( P - Base_Press );
komaida424 4:4060309b9cc0 564 }
komaida424 0:cca1c4e84da4 565
komaida424 0:cca1c4e84da4 566 void CalibrateGyros(void)
komaida424 0:cca1c4e84da4 567 {
komaida424 2:59ac9df97701 568 int i;
komaida424 2:59ac9df97701 569 float x,y,z;
komaida424 2:59ac9df97701 570 float k[3]={0,0,0};
komaida424 0:cca1c4e84da4 571 wait(1);
komaida424 2:59ac9df97701 572 Angle[0]=Angle[1]=Angle[2]=0;
komaida424 0:cca1c4e84da4 573 for(i=0; i<16; i++) {
komaida424 2:59ac9df97701 574 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
komaida424 2:59ac9df97701 575 else i2c.angular(&y,&x,&z);
komaida424 0:cca1c4e84da4 576 k[0] += x;
komaida424 0:cca1c4e84da4 577 k[1] += y;
komaida424 0:cca1c4e84da4 578 k[2] += z;
komaida424 2:59ac9df97701 579 wait(0.01);
komaida424 0:cca1c4e84da4 580 }
komaida424 2:59ac9df97701 581 for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16;
komaida424 2:59ac9df97701 582 // FlashLED(3);
komaida424 0:cca1c4e84da4 583 }
komaida424 0:cca1c4e84da4 584
komaida424 0:cca1c4e84da4 585 void CalibrateAccel(void)
komaida424 0:cca1c4e84da4 586 {
komaida424 2:59ac9df97701 587 int i;
komaida424 2:59ac9df97701 588 float x,y,z;
komaida424 2:59ac9df97701 589 float k[3]={0,0,0};
komaida424 2:59ac9df97701 590 conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0;
komaida424 0:cca1c4e84da4 591 for(i=0; i<16; i++) {
komaida424 2:59ac9df97701 592 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
komaida424 2:59ac9df97701 593 else i2c.Acceleration(&y,&x,&z);
komaida424 0:cca1c4e84da4 594 k[0] += x;
komaida424 0:cca1c4e84da4 595 k[1] += y;
komaida424 0:cca1c4e84da4 596 k[2] += z;
komaida424 2:59ac9df97701 597 wait(0.01);
komaida424 0:cca1c4e84da4 598 }
komaida424 2:59ac9df97701 599 conf.Accel_Ref[0] = k[0]/16;
komaida424 2:59ac9df97701 600 conf.Accel_Ref[1] = k[1]/16;
komaida424 3:27407c4984cf 601 conf.Accel_Ref[2] = k[2]/16-1;
komaida424 4:4060309b9cc0 602 // conf.Accel_Ref[2] = k[2]/16;
komaida424 2:59ac9df97701 603 // FlashLED(3);
komaida424 0:cca1c4e84da4 604 }
komaida424 0:cca1c4e84da4 605
komaida424 0:cca1c4e84da4 606 void PWM_Out(bool mode)
komaida424 0:cca1c4e84da4 607 {
komaida424 4:4060309b9cc0 608 int reg[3];
komaida424 7:16bf0085d914 609 int i,j,k;
komaida424 0:cca1c4e84da4 610 float gain;
komaida424 4:4060309b9cc0 611 // float cur_height;
komaida424 0:cca1c4e84da4 612
komaida424 2:59ac9df97701 613 interval = CycleTime.read();
komaida424 2:59ac9df97701 614 CycleTime.reset();
komaida424 6:a50e6d3924f1 615 if ( interval > 0.005f && mode ) return;
komaida424 2:59ac9df97701 616 TotalTime += interval;
komaida424 4:4060309b9cc0 617 if ( TotalTime > 0.5f ) {
komaida424 2:59ac9df97701 618 led1 = !led1;
komaida424 8:1db19b529b22 619 // if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5;
komaida424 8:1db19b529b22 620 // else buzz=0.0;
komaida424 2:59ac9df97701 621 TotalTime = 0;
komaida424 2:59ac9df97701 622 }
komaida424 2:59ac9df97701 623
komaida424 2:59ac9df97701 624 Get_Angle(interval);
komaida424 3:27407c4984cf 625 pid_interval += interval;
komaida424 4:4060309b9cc0 626 if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return;
komaida424 4:4060309b9cc0 627 pid_interval = 0;
komaida424 4:4060309b9cc0 628
komaida424 3:27407c4984cf 629 Get_Stick_Pos();
komaida424 4:4060309b9cc0 630 switch ( conf.Model_Type ) {
komaida424 4:4060309b9cc0 631 case Quad_X:
komaida424 6:a50e6d3924f1 632 M1 = M2 = M3 = M4 = THR + conf.Throttl_Trim;
komaida424 4:4060309b9cc0 633 break;
komaida424 6:a50e6d3924f1 634 case Quad_VP:
komaida424 6:a50e6d3924f1 635 M1 = M2 = M3 = M4 = Stick[AUX2] + conf.Stick_Ref[COL];
komaida424 6:a50e6d3924f1 636 M5 = THR + conf.Throttl_Trim;
komaida424 6:a50e6d3924f1 637 break;
komaida424 6:a50e6d3924f1 638 case Quad_3D:
komaida424 7:16bf0085d914 639 j = THR + conf.Throttl_Trim;
komaida424 7:16bf0085d914 640 if ( Stick[GAIN] < 0 ) {
komaida424 7:16bf0085d914 641 k = THR - conf.Reverse_Point;
komaida424 7:16bf0085d914 642 if ( abs(k) < THR_MIDRANGE ) {
komaida424 7:16bf0085d914 643 if ( k > 0 ) j = conf.Reverse_Point + THR_MIDRANGE;
komaida424 7:16bf0085d914 644 else j = conf.Reverse_Point - THR_MIDRANGE;
komaida424 7:16bf0085d914 645 }
komaida424 7:16bf0085d914 646 }
komaida424 7:16bf0085d914 647 // i = (int)throLimit.calc((float)THR);
komaida424 7:16bf0085d914 648 // if ( abs(i-conf.Reverse_Point) > 100 ) i = THR;
komaida424 7:16bf0085d914 649 M1 = M2 = M3 = M4 = j;
komaida424 4:4060309b9cc0 650 break;
komaida424 4:4060309b9cc0 651 case Delta:
komaida424 4:4060309b9cc0 652 case Delta_TW:
komaida424 4:4060309b9cc0 653 case AirPlane:
komaida424 6:a50e6d3924f1 654 M1 = THR + conf.Throttl_Trim;
komaida424 6:a50e6d3924f1 655 M2 = conf.Stick_Ref[ROL];
komaida424 6:a50e6d3924f1 656 M3 = conf.Stick_Ref[PIT];
komaida424 6:a50e6d3924f1 657 M4 = conf.Stick_Ref[YAW];
komaida424 4:4060309b9cc0 658 if ( conf.Model_Type == AirPlane )
komaida424 6:a50e6d3924f1 659 M5 = conf.Stick_Ref[ROL];
komaida424 6:a50e6d3924f1 660 else M5 = M1;
komaida424 4:4060309b9cc0 661 break;
komaida424 4:4060309b9cc0 662 }
komaida424 4:4060309b9cc0 663
komaida424 2:59ac9df97701 664 for ( i=0;i<3;i++ ) {
komaida424 2:59ac9df97701 665
komaida424 0:cca1c4e84da4 666 // Stick Angle Mixing
komaida424 0:cca1c4e84da4 667 if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
komaida424 6:a50e6d3924f1 668 else gain = ( (float)fabsf(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
komaida424 6:a50e6d3924f1 669 if ( Stick[i] > 0 ) gain -= gain * ( fabsf(Stick[i]) / 400 ) * conf.Active_Gyro_Gain;
komaida424 2:59ac9df97701 670 if ( Stick[GAIN] > 0
komaida424 4:4060309b9cc0 671 || i == YAW
komaida424 4:4060309b9cc0 672 || conf.Model_Type > 0
komaida424 6:a50e6d3924f1 673 || i2c.GetAddr(ACCEL_ADDR) == 0
komaida424 2:59ac9df97701 674 ) {
komaida424 2:59ac9df97701 675 reg[i] = Stick[i] * conf.Stick_Mix[i];
komaida424 2:59ac9df97701 676 reg[i] += Gyro[i] * gain * GYRO_ADJUST;
komaida424 2:59ac9df97701 677 }
komaida424 2:59ac9df97701 678 else {
komaida424 4:4060309b9cc0 679 reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i];
komaida424 4:4060309b9cc0 680 reg[i] += Gyro[i] * gain * GYRO_ADJUST;
komaida424 2:59ac9df97701 681 }
komaida424 4:4060309b9cc0 682
komaida424 4:4060309b9cc0 683 // pid_reg[i] = reg[i];
komaida424 0:cca1c4e84da4 684 }
komaida424 3:27407c4984cf 685 pid_interval = 0;
komaida424 4:4060309b9cc0 686
komaida424 6:a50e6d3924f1 687 // int Reverse = 1;
komaida424 4:4060309b9cc0 688 switch ( conf.Model_Type ) {
komaida424 6:a50e6d3924f1 689 case Quad_3D:
komaida424 6:a50e6d3924f1 690 // if ( THR < conf.Reverse_Point ) {
komaida424 6:a50e6d3924f1 691 // Reverse = -1;
komaida424 6:a50e6d3924f1 692 // }
komaida424 6:a50e6d3924f1 693 case Quad_X:
komaida424 6:a50e6d3924f1 694 case Quad_VP:
komaida424 4:4060309b9cc0 695 //Calculate Roll Pulse Width
komaida424 4:4060309b9cc0 696 M1 += reg[ROL];
komaida424 4:4060309b9cc0 697 M2 -= reg[ROL];
komaida424 4:4060309b9cc0 698 M3 -= reg[ROL];
komaida424 4:4060309b9cc0 699 M4 += reg[ROL];
komaida424 0:cca1c4e84da4 700
komaida424 4:4060309b9cc0 701 //Calculate Pitch Pulse Width
komaida424 4:4060309b9cc0 702 M1 += reg[PIT];
komaida424 4:4060309b9cc0 703 M2 += reg[PIT];
komaida424 4:4060309b9cc0 704 M3 -= reg[PIT];
komaida424 4:4060309b9cc0 705 M4 -= reg[PIT];
komaida424 0:cca1c4e84da4 706
komaida424 4:4060309b9cc0 707 //Calculate Yaw Pulse Width
komaida424 4:4060309b9cc0 708 M1 -= reg[YAW];
komaida424 4:4060309b9cc0 709 M2 += reg[YAW];
komaida424 4:4060309b9cc0 710 M3 -= reg[YAW];
komaida424 4:4060309b9cc0 711 M4 += reg[YAW];
komaida424 4:4060309b9cc0 712 break;
komaida424 6:a50e6d3924f1 713 case Delta:
komaida424 6:a50e6d3924f1 714 case Delta_TW:
komaida424 4:4060309b9cc0 715 //Calculate Roll Pulse Width
komaida424 4:4060309b9cc0 716 M2 += reg[ROL];
komaida424 4:4060309b9cc0 717 M3 -= reg[ROL];
komaida424 4:4060309b9cc0 718 //Calculate Pitch Pulse Width
komaida424 4:4060309b9cc0 719 M2 += reg[PIT];
komaida424 4:4060309b9cc0 720 M3 += reg[PIT];
komaida424 4:4060309b9cc0 721 //Calculate Yaw Pulse Width
komaida424 4:4060309b9cc0 722 M4 -= reg[YAW];
komaida424 4:4060309b9cc0 723 //Calculate Yaw Pulse Width
komaida424 4:4060309b9cc0 724 if ( conf.Model_Type == Delta_TW ) {
komaida424 4:4060309b9cc0 725 M1 += reg[YAW];
komaida424 4:4060309b9cc0 726 M5 -= reg[YAW];
komaida424 4:4060309b9cc0 727 }
komaida424 4:4060309b9cc0 728 break;
komaida424 6:a50e6d3924f1 729 case AirPlane:
komaida424 6:a50e6d3924f1 730 //Calculate Roll Pulse Width
komaida424 4:4060309b9cc0 731 M2 -= reg[ROL];
komaida424 4:4060309b9cc0 732 M5 -= reg[ROL];
komaida424 6:a50e6d3924f1 733 //Calculate Pitch Pulse Width
komaida424 4:4060309b9cc0 734 M3 += reg[PIT];
komaida424 4:4060309b9cc0 735 //Calculate Yaw Pulse Width
komaida424 4:4060309b9cc0 736 M4 -= reg[YAW];
komaida424 4:4060309b9cc0 737 break;
komaida424 4:4060309b9cc0 738 }
komaida424 7:16bf0085d914 739 j = conf.Model_Type;
komaida424 8:1db19b529b22 740 for ( i=0; i<PwmNum; i++ ) {
komaida424 6:a50e6d3924f1 741 if ( M[i] > Pulse_Max ) M[i] = Pulse_Max;
komaida424 6:a50e6d3924f1 742 if ( M[i] < Pulse_Min ) M[i] = Pulse_Min;
komaida424 7:16bf0085d914 743 if ( Servo_idx[j][i] == 1 )
komaida424 4:4060309b9cc0 744 {
komaida424 6:a50e6d3924f1 745 M[i] = conf.Stick_Ref[i] + ( ( M[i] - conf.Stick_Ref[i] ) * conf.Servo_Dir[i] );
komaida424 6:a50e6d3924f1 746 }
komaida424 6:a50e6d3924f1 747 else {
komaida424 7:16bf0085d914 748 if ( j == Quad_3D ) {
komaida424 6:a50e6d3924f1 749 if ( Stick[GAIN] > 0 ) {
komaida424 6:a50e6d3924f1 750 if ( THR < conf.Reverse_Point )
komaida424 6:a50e6d3924f1 751 M[i] = conf.Reverse_Point;
komaida424 6:a50e6d3924f1 752 }
komaida424 6:a50e6d3924f1 753 }
komaida424 6:a50e6d3924f1 754 else {
komaida424 7:16bf0085d914 755 if ( Stick[COL] < Thro_Zero ) M[i] = conf.Stick_Ref[COL];
komaida424 6:a50e6d3924f1 756 }
komaida424 4:4060309b9cc0 757 }
komaida424 0:cca1c4e84da4 758 }
komaida424 2:59ac9df97701 759 if ( mode ) {
komaida424 7:16bf0085d914 760 // h = conf.Stick_Ref[THR];
komaida424 8:1db19b529b22 761 for ( i=0;i<PwmNum;i++ ) {
komaida424 4:4060309b9cc0 762 // while ( !pwmpin[i] );
komaida424 4:4060309b9cc0 763 if ( conf.PWM_Mode == 1 )
komaida424 6:a50e6d3924f1 764 pwm[i].pulsewidth_us(M[i]);
komaida424 6:a50e6d3924f1 765 else pwm[i].pulsewidth_us(M[i]-conf.Stick_Ref[COL]);
komaida424 2:59ac9df97701 766 }
komaida424 2:59ac9df97701 767 }
komaida424 2:59ac9df97701 768
komaida424 0:cca1c4e84da4 769 }
komaida424 0:cca1c4e84da4 770
komaida424 4:4060309b9cc0 771
komaida424 0:cca1c4e84da4 772 void ESC_SetUp(void) {
komaida424 0:cca1c4e84da4 773 while(1) {
komaida424 0:cca1c4e84da4 774 Get_Stick_Pos();
komaida424 8:1db19b529b22 775 for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
komaida424 2:59ac9df97701 776 wait(0.015);
komaida424 0:cca1c4e84da4 777 }
komaida424 2:59ac9df97701 778 }
komaida424 2:59ac9df97701 779
komaida424 2:59ac9df97701 780 void Flight_SetUp(void)
komaida424 2:59ac9df97701 781 {
komaida424 2:59ac9df97701 782 int i;
komaida424 8:1db19b529b22 783 for ( i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
komaida424 8:1db19b529b22 784 for ( i=0;i<PwmNum;i++ ) pwm[i].period_us(conf.PWM_Interval);
komaida424 8:1db19b529b22 785 for ( i=0; i<PwmNum; i++ ) {
komaida424 4:4060309b9cc0 786 pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
komaida424 4:4060309b9cc0 787 }
komaida424 4:4060309b9cc0 788 hov_control = false;
komaida424 8:1db19b529b22 789 // throLimit.differential(conf.Thro_Limit_Val);
komaida424 8:1db19b529b22 790 // throLimit.rate(conf.Thro_Limit_Rate);
komaida424 2:59ac9df97701 791 Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
komaida424 2:59ac9df97701 792 loop_cnt = 0;
komaida424 8:1db19b529b22 793 // FlyghtTime.start();
komaida424 2:59ac9df97701 794 CycleTime.start();
komaida424 3:27407c4984cf 795 pid_interval = 0;
komaida424 4:4060309b9cc0 796 Stick_Save[COL] = Stick[COL];
komaida424 2:59ac9df97701 797 FlashLED(5);
komaida424 2:59ac9df97701 798 }
komaida424 2:59ac9df97701 799
komaida424 2:59ac9df97701 800 void LCD_locate(int clm,int row)
komaida424 2:59ac9df97701 801 {
komaida424 2:59ac9df97701 802 lcd.locate(clm,row);
komaida424 2:59ac9df97701 803 i2c.locate(clm,row);
komaida424 2:59ac9df97701 804 }
komaida424 2:59ac9df97701 805
komaida424 2:59ac9df97701 806 void LCD_cls()
komaida424 2:59ac9df97701 807 {
komaida424 2:59ac9df97701 808 lcd.cls();
komaida424 2:59ac9df97701 809 i2c.cls();
komaida424 2:59ac9df97701 810 }
komaida424 2:59ac9df97701 811
komaida424 2:59ac9df97701 812 void LCD_printf(char* str)
komaida424 2:59ac9df97701 813 {
komaida424 8:1db19b529b22 814 lcd.write(str);
komaida424 8:1db19b529b22 815 i2c.write_lcd(str);
komaida424 8:1db19b529b22 816 }
komaida424 8:1db19b529b22 817
komaida424 8:1db19b529b22 818 void wait(float tm)
komaida424 8:1db19b529b22 819 {
komaida424 8:1db19b529b22 820 wait_us(tm*1000000);
komaida424 2:59ac9df97701 821 }
komaida424 2:59ac9df97701 822 ;
komaida424 0:cca1c4e84da4 823
komaida424 0:cca1c4e84da4 824
komaida424 0:cca1c4e84da4 825
komaida424 0:cca1c4e84da4 826
komaida424 0:cca1c4e84da4 827
komaida424 2:59ac9df97701 828
komaida424 2:59ac9df97701 829
komaida424 3:27407c4984cf 830
komaida424 3:27407c4984cf 831
komaida424 4:4060309b9cc0 832
komaida424 4:4060309b9cc0 833
komaida424 4:4060309b9cc0 834
komaida424 4:4060309b9cc0 835
komaida424 4:4060309b9cc0 836
komaida424 4:4060309b9cc0 837