syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

main.cpp@4:4060309b9cc0, 2014-10-14 (annotated)

Committer:
komaida424
Date:
Tue Oct 14 08:15:03 2014 +0000
Revision:
4:4060309b9cc0
Parent:
3:27407c4984cf
Child:
5:7b02775787a9
rev.2.1

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