syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

main.cpp@6:a50e6d3924f1, 2015-02-24 (annotated)

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