syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

main.cpp@2:59ac9df97701, 2013-11-15 (annotated)

Committer:
komaida424
Date:
Fri Nov 15 20:53:36 2013 +0000
Revision:
2:59ac9df97701
Parent:
0:cca1c4e84da4
Child:
3:27407c4984cf
ver.1.25

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 0:cca1c4e84da4 1 #include "mbed.h"
komaida424 2:59ac9df97701 2 #include "math.h"
komaida424 0:cca1c4e84da4 3 #include "I2cPeripherals.h"
komaida424 0:cca1c4e84da4 4 #include "InterruptIn.h"
komaida424 0:cca1c4e84da4 5 #include "config.h"
komaida424 0:cca1c4e84da4 6 #include "PulseWidthCounter.h"
komaida424 0:cca1c4e84da4 7 #include "string"
komaida424 0:cca1c4e84da4 8 #include "SerialLcd.h"
komaida424 0:cca1c4e84da4 9 #include "IAP.h"
komaida424 2:59ac9df97701 10 #include "PID.h"
komaida424 2:59ac9df97701 11 #include "SoftPWM.h"
komaida424 2:59ac9df97701 12
komaida424 2:59ac9df97701 13 //Serial pc(USBTX, USBRX);
komaida424 2:59ac9df97701 14
komaida424 2:59ac9df97701 15 //Gravity at Earth's surface in m/s/s
komaida424 2:59ac9df97701 16 #define g0 9.812865328
komaida424 2:59ac9df97701 17 //Convert from radians to degrees.
komaida424 2:59ac9df97701 18 #define toDegrees(x) (x * 57.2957795)
komaida424 2:59ac9df97701 19 //Convert from degrees to radians.
komaida424 2:59ac9df97701 20 #define toRadians(x) (x * 0.01745329252)
komaida424 2:59ac9df97701 21 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
komaida424 2:59ac9df97701 22 #define GYROSCOPE_GAIN (1 / 14.375)
komaida424 2:59ac9df97701 23 //Full scale resolution on the ADXL345 is 4mg/LSB.
komaida424 2:59ac9df97701 24 #define ACCELEROMETER_GAIN (0.004 * g0)
komaida424 2:59ac9df97701 25 //Updating filter at 40Hz.
komaida424 2:59ac9df97701 26 #define FILTER_RATE 0.05
komaida424 2:59ac9df97701 27 //At rest the gyroscope is centred around 0 and goes between about
komaida424 2:59ac9df97701 28 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
komaida424 2:59ac9df97701 29 //5/15 = 0.3 degrees/sec.
komaida424 0:cca1c4e84da4 30
komaida424 2:59ac9df97701 31 #ifdef TARGET_LPC1114 // LPC1114
komaida424 2:59ac9df97701 32 // PulseOut pwm[4] = { dp1,dp2,dp18,dp24 );
komaida424 2:59ac9df97701 33 #define LED1 dp28
komaida424 2:59ac9df97701 34 #define p5 dp4
komaida424 2:59ac9df97701 35 #define p6 dp9
komaida424 2:59ac9df97701 36 #define p7 dp10
komaida424 2:59ac9df97701 37 #define p8 dp11
komaida424 2:59ac9df97701 38 #define p9 dp25
komaida424 2:59ac9df97701 39 #define p10 dp26
komaida424 2:59ac9df97701 40 #define p13 dp16
komaida424 2:59ac9df97701 41 #define p21 dp1
komaida424 2:59ac9df97701 42 #define p22 dp2
komaida424 2:59ac9df97701 43 #define p23 dp18
komaida424 2:59ac9df97701 44 #define p24 dp24
komaida424 2:59ac9df97701 45 #define p27 dp27
komaida424 2:59ac9df97701 46 #define p28 dp5
komaida424 2:59ac9df97701 47 #else //LPC1768
komaida424 2:59ac9df97701 48 #ifdef LPCXpresso
komaida424 2:59ac9df97701 49 #define LED1 P0_22
komaida424 2:59ac9df97701 50 #endif
komaida424 2:59ac9df97701 51 #endif
komaida424 2:59ac9df97701 52 DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
komaida424 0:cca1c4e84da4 53 DigitalOut led1(LED1);
komaida424 0:cca1c4e84da4 54 DigitalOut led2(LED2);
komaida424 2:59ac9df97701 55 InterruptIn ch1(p5);
komaida424 2:59ac9df97701 56 PulseWidthCounter ch[5] = { p6,p7,p8,p9,p10 };
komaida424 2:59ac9df97701 57 #if defined(SOFT_PWM) || defined(TARGET_LPC1114)
komaida424 2:59ac9df97701 58 SoftPWM pwm[4] = { p21,p22,p23,p24 };
komaida424 2:59ac9df97701 59 #else
komaida424 2:59ac9df97701 60 PwmOut pwm[4] = { p21,p22,p23,p24 };
komaida424 0:cca1c4e84da4 61 #endif
komaida424 2:59ac9df97701 62 SoftPWM buzz(p26);
komaida424 0:cca1c4e84da4 63 Timer CurTime;
komaida424 2:59ac9df97701 64 //Timer ElapTime;
komaida424 2:59ac9df97701 65 Timer CycleTime;
komaida424 2:59ac9df97701 66 Timer FlyghtTime;
komaida424 2:59ac9df97701 67 //Ticker tick;
komaida424 2:59ac9df97701 68 //Ticker tick100ms;
komaida424 2:59ac9df97701 69 //Ticker mixTime;
komaida424 0:cca1c4e84da4 70 I2cPeripherals i2c(p28,p27); //sda scl
komaida424 0:cca1c4e84da4 71 SerialLcd lcd(p13);
komaida424 0:cca1c4e84da4 72 config conf;
komaida424 2:59ac9df97701 73 PID pid[3];
komaida424 2:59ac9df97701 74 #ifdef TARGET_LPC1114
komaida424 2:59ac9df97701 75 //LPC1114 Flash Memory read/write
komaida424 2:59ac9df97701 76 #define MEM_SIZE 256
komaida424 2:59ac9df97701 77 #define TARGET_SECTOR 7 // use sector 29 as target sector if it is on LPC1768
komaida424 2:59ac9df97701 78 #else
komaida424 2:59ac9df97701 79 //LPC1768 Flash Memory read/write
komaida424 2:59ac9df97701 80 #define MEM_SIZE 256
komaida424 2:59ac9df97701 81 #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
komaida424 2:59ac9df97701 82 #endif
komaida424 2:59ac9df97701 83 #ifndef LocalFileOut
komaida424 2:59ac9df97701 84 IAP iap;
komaida424 2:59ac9df97701 85 #endif
komaida424 2:59ac9df97701 86 float TotalTime = 0;;
komaida424 2:59ac9df97701 87 int channel = 0;
komaida424 2:59ac9df97701 88 //int intrupt_cnt = 0;
komaida424 2:59ac9df97701 89 //int intrupt_cnt2 = 0;
komaida424 2:59ac9df97701 90 volatile int CH[5];
komaida424 2:59ac9df97701 91 volatile int M[6];
komaida424 2:59ac9df97701 92 volatile float Gyro[3];
komaida424 2:59ac9df97701 93 volatile float Accel[3];
komaida424 2:59ac9df97701 94 volatile float Angle[3];
komaida424 2:59ac9df97701 95 volatile float Gyro_Ref[3];
komaida424 2:59ac9df97701 96 volatile float Gyro_Save[3];
komaida424 2:59ac9df97701 97 volatile int Stick[5];
komaida424 2:59ac9df97701 98 volatile int Stick_Save[3];
komaida424 2:59ac9df97701 99 //int Stick_Max[3];
komaida424 0:cca1c4e84da4 100 float Press;
komaida424 0:cca1c4e84da4 101 char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
komaida424 2:59ac9df97701 102 //volatile bool tick_flag;
komaida424 2:59ac9df97701 103 //volatile bool buzz_flag;
komaida424 2:59ac9df97701 104 float interval;
komaida424 2:59ac9df97701 105 int pid_reg[3];
komaida424 2:59ac9df97701 106 int loop_cnt;
komaida424 0:cca1c4e84da4 107
komaida424 0:cca1c4e84da4 108 void initialize();
komaida424 0:cca1c4e84da4 109 void FlashLED(int );
komaida424 0:cca1c4e84da4 110 void SetUp();
komaida424 0:cca1c4e84da4 111 void SetUpPrompt(config&,I2cPeripherals&);
komaida424 0:cca1c4e84da4 112 void PWM_Out(bool);
komaida424 0:cca1c4e84da4 113 void Get_Stick_Pos();
komaida424 0:cca1c4e84da4 114 void CalibrateGyros(void);
komaida424 0:cca1c4e84da4 115 void CalibrateAccel(void);
komaida424 0:cca1c4e84da4 116 void Get_Gyro();
komaida424 2:59ac9df97701 117 bool Get_Accel();
komaida424 2:59ac9df97701 118 void Get_Angle(float);
komaida424 0:cca1c4e84da4 119 void ReadConfig();
komaida424 0:cca1c4e84da4 120 void WriteConfig();
komaida424 2:59ac9df97701 121 void Interrupt_Check(bool&,int &,DigitalOut &);
komaida424 0:cca1c4e84da4 122 void ESC_SetUp(void);
komaida424 2:59ac9df97701 123 void Flight_SetUp();
komaida424 2:59ac9df97701 124 //void IMUfilter_Reset(void);
komaida424 2:59ac9df97701 125 void LCD_printf(char *);
komaida424 2:59ac9df97701 126 void LCD_cls();
komaida424 2:59ac9df97701 127 void LCD_locate(int,int);
komaida424 2:59ac9df97701 128 /*
komaida424 2:59ac9df97701 129 void tick_interrupt()
komaida424 2:59ac9df97701 130 {
komaida424 2:59ac9df97701 131 tick_flag = true;
komaida424 2:59ac9df97701 132 }
komaida424 2:59ac9df97701 133 */
komaida424 0:cca1c4e84da4 134 void PulseCheck()
komaida424 0:cca1c4e84da4 135 {
komaida424 2:59ac9df97701 136 channel++;
komaida424 0:cca1c4e84da4 137 }
komaida424 0:cca1c4e84da4 138
komaida424 0:cca1c4e84da4 139 void PulseAnalysis() //Interrupt Pin5
komaida424 0:cca1c4e84da4 140 {
komaida424 0:cca1c4e84da4 141 CurTime.stop();
komaida424 0:cca1c4e84da4 142 int PulseWidth = CurTime.read_us();
komaida424 0:cca1c4e84da4 143 CurTime.reset();
komaida424 0:cca1c4e84da4 144 CurTime.start();
komaida424 2:59ac9df97701 145 if ( PulseWidth > 3000 ) channel = 0; //reset pulse count
komaida424 0:cca1c4e84da4 146 else {
komaida424 0:cca1c4e84da4 147 if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
komaida424 2:59ac9df97701 148 switch( channel ) {
komaida424 0:cca1c4e84da4 149 case IR_THR:
komaida424 0:cca1c4e84da4 150 THR = PulseWidth;
komaida424 0:cca1c4e84da4 151 break;
komaida424 0:cca1c4e84da4 152 case IR_AIL:
komaida424 0:cca1c4e84da4 153 AIL = PulseWidth;
komaida424 0:cca1c4e84da4 154 break;
komaida424 0:cca1c4e84da4 155 case IR_ELE:
komaida424 0:cca1c4e84da4 156 ELE = PulseWidth;
komaida424 0:cca1c4e84da4 157 break;
komaida424 0:cca1c4e84da4 158 case IR_RUD:
komaida424 0:cca1c4e84da4 159 RUD = PulseWidth;
komaida424 0:cca1c4e84da4 160 break;
komaida424 0:cca1c4e84da4 161 case IR_AUX:
komaida424 0:cca1c4e84da4 162 AUX = PulseWidth;
komaida424 0:cca1c4e84da4 163 break;
komaida424 0:cca1c4e84da4 164 }
komaida424 0:cca1c4e84da4 165 }
komaida424 0:cca1c4e84da4 166 }
komaida424 2:59ac9df97701 167 channel++;
komaida424 0:cca1c4e84da4 168 }
komaida424 0:cca1c4e84da4 169
komaida424 0:cca1c4e84da4 170 int main()
komaida424 0:cca1c4e84da4 171 {
komaida424 2:59ac9df97701 172 int i=0;
komaida424 0:cca1c4e84da4 173
komaida424 2:59ac9df97701 174 wait(0.5);
komaida424 0:cca1c4e84da4 175 initialize();
komaida424 2:59ac9df97701 176
komaida424 0:cca1c4e84da4 177 Get_Stick_Pos();
komaida424 2:59ac9df97701 178 while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low
komaida424 0:cca1c4e84da4 179 {
komaida424 2:59ac9df97701 180 if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High
komaida424 2:59ac9df97701 181 ESC_SetUp();
komaida424 2:59ac9df97701 182 if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High
komaida424 0:cca1c4e84da4 183 {
komaida424 2:59ac9df97701 184 loop_cnt = 0;
komaida424 0:cca1c4e84da4 185 SetUpPrompt(conf,i2c);
komaida424 0:cca1c4e84da4 186 break;
komaida424 0:cca1c4e84da4 187 }
komaida424 0:cca1c4e84da4 188 FlashLED(3);
komaida424 2:59ac9df97701 189 wait(0.3);
komaida424 0:cca1c4e84da4 190 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 191 }
komaida424 2:59ac9df97701 192 Flight_SetUp();
komaida424 0:cca1c4e84da4 193 while (1)
komaida424 0:cca1c4e84da4 194 {
komaida424 2:59ac9df97701 195 if ( Stick[COL] < Thro_Zero )
komaida424 0:cca1c4e84da4 196 {
komaida424 0:cca1c4e84da4 197 i = 0;
komaida424 2:59ac9df97701 198 while ( Stick[YAW] < -Stick_Limit && Stick[COL] < Thro_Zero ) //Rudder Left
komaida424 0:cca1c4e84da4 199 {
komaida424 0:cca1c4e84da4 200 if ( i > 100 ) //wait 2 sec
komaida424 0:cca1c4e84da4 201 {
komaida424 2:59ac9df97701 202 FlyghtTime.stop();
komaida424 2:59ac9df97701 203 if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down
komaida424 2:59ac9df97701 204 loop_cnt = 0;
komaida424 2:59ac9df97701 205 FlashLED(5);
komaida424 2:59ac9df97701 206 for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
komaida424 2:59ac9df97701 207 i2c.start(conf.LCD_Contrast);
komaida424 2:59ac9df97701 208 SetUpPrompt(conf,i2c);
komaida424 2:59ac9df97701 209 }
komaida424 0:cca1c4e84da4 210 CalibrateGyros();
komaida424 2:59ac9df97701 211 Flight_SetUp();
komaida424 0:cca1c4e84da4 212 break;
komaida424 0:cca1c4e84da4 213 }
komaida424 0:cca1c4e84da4 214 wait(0.01); // wait 10 msec
komaida424 0:cca1c4e84da4 215 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 216 i++;
komaida424 0:cca1c4e84da4 217 }
komaida424 0:cca1c4e84da4 218 }
komaida424 0:cca1c4e84da4 219 PWM_Out(true);
komaida424 0:cca1c4e84da4 220 }
komaida424 0:cca1c4e84da4 221
komaida424 0:cca1c4e84da4 222 }
komaida424 0:cca1c4e84da4 223
komaida424 0:cca1c4e84da4 224 void initialize()
komaida424 0:cca1c4e84da4 225 {
komaida424 2:59ac9df97701 226 buzz.period_us(500);
komaida424 0:cca1c4e84da4 227 ReadConfig(); //config.inf file read
komaida424 2:59ac9df97701 228
komaida424 2:59ac9df97701 229 i2c.start(conf.LCD_Contrast);
komaida424 2:59ac9df97701 230 channel = 0;
komaida424 0:cca1c4e84da4 231 ch1.rise(&PulseCheck); //input pulse count
komaida424 2:59ac9df97701 232 wait(0.2);
komaida424 2:59ac9df97701 233 if ( channel > 50 ) {
komaida424 0:cca1c4e84da4 234 ch1.rise(&PulseAnalysis);
komaida424 0:cca1c4e84da4 235 InPulseMode = 'S';
komaida424 0:cca1c4e84da4 236 }
komaida424 0:cca1c4e84da4 237 else InPulseMode = 'P';
komaida424 2:59ac9df97701 238 led1 = 0;
komaida424 2:59ac9df97701 239 CycleTime.start();
komaida424 0:cca1c4e84da4 240 for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
komaida424 2:59ac9df97701 241 FlashLED(3);
komaida424 0:cca1c4e84da4 242 }
komaida424 0:cca1c4e84da4 243
komaida424 0:cca1c4e84da4 244 void FlashLED(int cnt)
komaida424 0:cca1c4e84da4 245 {
komaida424 0:cca1c4e84da4 246 for ( int i = 0 ; i < cnt ; i++ ) {
komaida424 0:cca1c4e84da4 247 led1 = !led1;
komaida424 2:59ac9df97701 248 buzz = 0.5f;
komaida424 2:59ac9df97701 249 wait(0.1);
komaida424 0:cca1c4e84da4 250 led1 = !led1;
komaida424 2:59ac9df97701 251 buzz = 0.0f;
komaida424 2:59ac9df97701 252 wait(0.1);
komaida424 0:cca1c4e84da4 253 }
komaida424 0:cca1c4e84da4 254 }
komaida424 0:cca1c4e84da4 255
komaida424 0:cca1c4e84da4 256 void ReadConfig()
komaida424 0:cca1c4e84da4 257 {
komaida424 0:cca1c4e84da4 258 #ifdef LocalFileOut
komaida424 0:cca1c4e84da4 259 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
komaida424 0:cca1c4e84da4 260 FILE *fp = fopen("/local/setup.inf", "rb"); // Open "out.txt" on the local file system for writing
komaida424 0:cca1c4e84da4 261 if ( fp != NULL ) {
komaida424 0:cca1c4e84da4 262 float rev = conf.Revision;
komaida424 0:cca1c4e84da4 263 int len = fread(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 264 switch ( len ) {
komaida424 0:cca1c4e84da4 265 case sizeof(config): // File size ok
komaida424 0:cca1c4e84da4 266 if ( rev == conf.Revision ) break;
komaida424 0:cca1c4e84da4 267 default:
komaida424 0:cca1c4e84da4 268 fclose(fp);
komaida424 0:cca1c4e84da4 269 config init;
komaida424 0:cca1c4e84da4 270 conf = init;
komaida424 0:cca1c4e84da4 271 fp = fopen("/local/setup.inf", "wb");
komaida424 0:cca1c4e84da4 272 fwrite(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 273 }
komaida424 0:cca1c4e84da4 274 fclose(fp);
komaida424 0:cca1c4e84da4 275 } else {
komaida424 0:cca1c4e84da4 276 WriteConfig();
komaida424 0:cca1c4e84da4 277 wait(2);
komaida424 0:cca1c4e84da4 278 }
komaida424 0:cca1c4e84da4 279 #else
komaida424 0:cca1c4e84da4 280 char *send;
komaida424 0:cca1c4e84da4 281 char *recv;
komaida424 0:cca1c4e84da4 282 int i,rc;
komaida424 0:cca1c4e84da4 283 config *conf_ptr;
komaida424 0:cca1c4e84da4 284
komaida424 0:cca1c4e84da4 285 if ( sizeof(config) > 255 ) {
komaida424 2:59ac9df97701 286 LCD_printf("config size over");
komaida424 2:59ac9df97701 287 wait(3);
komaida424 0:cca1c4e84da4 288 return;
komaida424 0:cca1c4e84da4 289 }
komaida424 0:cca1c4e84da4 290 rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 291 if ( rc == SECTOR_NOT_BLANK ) {
komaida424 0:cca1c4e84da4 292 send = sector_start_adress[TARGET_SECTOR];
komaida424 0:cca1c4e84da4 293 recv = (char*)&conf;
komaida424 0:cca1c4e84da4 294 conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
komaida424 0:cca1c4e84da4 295 if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
komaida424 0:cca1c4e84da4 296 for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
komaida424 0:cca1c4e84da4 297 return;
komaida424 0:cca1c4e84da4 298 }
komaida424 0:cca1c4e84da4 299 }
komaida424 0:cca1c4e84da4 300 WriteConfig();
komaida424 0:cca1c4e84da4 301 #endif
komaida424 0:cca1c4e84da4 302 }
komaida424 0:cca1c4e84da4 303
komaida424 0:cca1c4e84da4 304 void WriteConfig()
komaida424 0:cca1c4e84da4 305 {
komaida424 0:cca1c4e84da4 306 #ifdef LocalFileOut
komaida424 0:cca1c4e84da4 307 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
komaida424 0:cca1c4e84da4 308 FILE *fp = fopen("/local/setup.inf", "wb");
komaida424 0:cca1c4e84da4 309 fwrite(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 310 fclose(fp);
komaida424 0:cca1c4e84da4 311 #else
komaida424 0:cca1c4e84da4 312 char mem[MEM_SIZE];
komaida424 0:cca1c4e84da4 313 char *send;
komaida424 0:cca1c4e84da4 314 int i;
komaida424 0:cca1c4e84da4 315 if ( sizeof(config) > 255 ) {
komaida424 2:59ac9df97701 316 LCD_printf("config size over");
komaida424 2:59ac9df97701 317 wait(3);
komaida424 0:cca1c4e84da4 318 return;
komaida424 0:cca1c4e84da4 319 }
komaida424 0:cca1c4e84da4 320 send = (char*)&conf;
komaida424 0:cca1c4e84da4 321 for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
komaida424 0:cca1c4e84da4 322 for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
komaida424 0:cca1c4e84da4 323 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 324 iap.erase( TARGET_SECTOR, TARGET_SECTOR );
komaida424 2:59ac9df97701 325 //pc.printf("erase\n");
komaida424 0:cca1c4e84da4 326 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 327 iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
komaida424 2:59ac9df97701 328 //pc.printf("write\n");
komaida424 0:cca1c4e84da4 329 #endif
komaida424 0:cca1c4e84da4 330 }
komaida424 0:cca1c4e84da4 331
komaida424 0:cca1c4e84da4 332 void Get_Stick_Pos(void)
komaida424 0:cca1c4e84da4 333 {
komaida424 0:cca1c4e84da4 334 if ( InPulseMode == 'P' ) {
komaida424 0:cca1c4e84da4 335 for (int i=0;i<5;i++) CH[i] = ch[i].count;
komaida424 0:cca1c4e84da4 336 }
komaida424 2:59ac9df97701 337 // Stick_Save[ROL] = Stick[ROL];
komaida424 2:59ac9df97701 338 // Stick_Save[PIT] = Stick[PIT];
komaida424 2:59ac9df97701 339 // Stick_Save[YAW] = Stick[YAW];
komaida424 2:59ac9df97701 340
komaida424 0:cca1c4e84da4 341 Stick[ROL] = AIL - conf.Stick_Ref[ROL];
komaida424 0:cca1c4e84da4 342 Stick[PIT] = ELE - conf.Stick_Ref[PIT];
komaida424 0:cca1c4e84da4 343 Stick[YAW] = RUD - conf.Stick_Ref[YAW];
komaida424 0:cca1c4e84da4 344 Stick[COL] = THR - conf.Stick_Ref[COL];
komaida424 0:cca1c4e84da4 345 Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
komaida424 0:cca1c4e84da4 346 }
komaida424 0:cca1c4e84da4 347
komaida424 0:cca1c4e84da4 348 void Get_Gyro()
komaida424 0:cca1c4e84da4 349 {
komaida424 2:59ac9df97701 350 float x,y,z;
komaida424 2:59ac9df97701 351 bool err;
komaida424 2:59ac9df97701 352 Gyro_Save[ROL] = Gyro[ROL];
komaida424 2:59ac9df97701 353 Gyro_Save[PIT] = Gyro[PIT];
komaida424 2:59ac9df97701 354 Gyro_Save[YAW] = Gyro[YAW];
komaida424 2:59ac9df97701 355
komaida424 2:59ac9df97701 356 if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z);
komaida424 2:59ac9df97701 357 else err=i2c.angular(&y,&x,&z);
komaida424 2:59ac9df97701 358 if ( err == false ) return;
komaida424 2:59ac9df97701 359 Gyro[ROL] = x - Gyro_Ref[0] ;
komaida424 2:59ac9df97701 360 Gyro[PIT] = y - Gyro_Ref[1] ;
komaida424 2:59ac9df97701 361 Gyro[YAW] = z - Gyro_Ref[2] ;
komaida424 2:59ac9df97701 362
komaida424 2:59ac9df97701 363 if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL];
komaida424 2:59ac9df97701 364 if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT];
komaida424 2:59ac9df97701 365 if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW];
komaida424 0:cca1c4e84da4 366 }
komaida424 0:cca1c4e84da4 367
komaida424 2:59ac9df97701 368 bool Get_Accel()
komaida424 2:59ac9df97701 369 {
komaida424 2:59ac9df97701 370 float x,y,z;
komaida424 2:59ac9df97701 371 bool err;
komaida424 2:59ac9df97701 372 if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z);
komaida424 2:59ac9df97701 373 else err=i2c.Acceleration(&y,&x,&z);
komaida424 2:59ac9df97701 374 if ( err == false ) return false;
komaida424 2:59ac9df97701 375 float wx = x - conf.Accel_Ref[0];
komaida424 2:59ac9df97701 376 float wy = y - conf.Accel_Ref[1];
komaida424 2:59ac9df97701 377 float wz = z - conf.Accel_Ref[2];
komaida424 2:59ac9df97701 378 Accel[ROL] = atan(wx/sqrt( pow(wy,2)+pow(wz,2)))*180/3.14;
komaida424 2:59ac9df97701 379 Accel[PIT] = atan(wy/sqrt( pow(wx,2)+pow(wz,2)))*180/3.14;
komaida424 2:59ac9df97701 380 Accel[YAW] = atan(sqrt( pow(wx,2)+pow(wy,2))/wz)*180/3.14;
komaida424 2:59ac9df97701 381 return true;
komaida424 2:59ac9df97701 382 }
komaida424 2:59ac9df97701 383
komaida424 2:59ac9df97701 384 void Get_Angle(float interval)
komaida424 0:cca1c4e84da4 385 {
komaida424 2:59ac9df97701 386 Get_Gyro();
komaida424 2:59ac9df97701 387 Get_Accel();
komaida424 2:59ac9df97701 388 float x,y,z;
komaida424 2:59ac9df97701 389
komaida424 2:59ac9df97701 390 x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5;
komaida424 2:59ac9df97701 391 y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5;
komaida424 2:59ac9df97701 392 z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5;
komaida424 2:59ac9df97701 393 if ( Get_Accel() == true ) {
komaida424 2:59ac9df97701 394 float i = 3.14 * 2 * conf.Cutoff_Freq * interval;
komaida424 2:59ac9df97701 395 Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval;
komaida424 2:59ac9df97701 396 Angle[PIT] += -Angle[PIT] * i + Accel[PIT] * i + y * interval;
komaida424 2:59ac9df97701 397 }
komaida424 2:59ac9df97701 398 else {
komaida424 2:59ac9df97701 399 Angle[ROL] += x * interval;
komaida424 2:59ac9df97701 400 Angle[PIT] += y * interval;
komaida424 2:59ac9df97701 401 }
komaida424 2:59ac9df97701 402 Angle[YAW] += z * interval;
komaida424 0:cca1c4e84da4 403 }
komaida424 2:59ac9df97701 404
komaida424 0:cca1c4e84da4 405 void Get_Pressure()
komaida424 0:cca1c4e84da4 406 {
komaida424 0:cca1c4e84da4 407 Press = i2c.pressure();
komaida424 0:cca1c4e84da4 408 }
komaida424 0:cca1c4e84da4 409
komaida424 0:cca1c4e84da4 410 void CalibrateGyros(void)
komaida424 0:cca1c4e84da4 411 {
komaida424 2:59ac9df97701 412 int i;
komaida424 2:59ac9df97701 413 float x,y,z;
komaida424 2:59ac9df97701 414 float k[3]={0,0,0};
komaida424 0:cca1c4e84da4 415 wait(1);
komaida424 2:59ac9df97701 416 Angle[0]=Angle[1]=Angle[2]=0;
komaida424 0:cca1c4e84da4 417 for(i=0; i<16; i++) {
komaida424 2:59ac9df97701 418 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
komaida424 2:59ac9df97701 419 else i2c.angular(&y,&x,&z);
komaida424 0:cca1c4e84da4 420 k[0] += x;
komaida424 0:cca1c4e84da4 421 k[1] += y;
komaida424 0:cca1c4e84da4 422 k[2] += z;
komaida424 2:59ac9df97701 423 wait(0.01);
komaida424 0:cca1c4e84da4 424 }
komaida424 2:59ac9df97701 425 for( i=0; i<3; i++ ) Gyro_Ref[i] = k[i]/16;
komaida424 2:59ac9df97701 426 // FlashLED(3);
komaida424 0:cca1c4e84da4 427 }
komaida424 0:cca1c4e84da4 428
komaida424 0:cca1c4e84da4 429 void CalibrateAccel(void)
komaida424 0:cca1c4e84da4 430 {
komaida424 2:59ac9df97701 431 int i;
komaida424 2:59ac9df97701 432 float x,y,z;
komaida424 2:59ac9df97701 433 float k[3]={0,0,0};
komaida424 2:59ac9df97701 434 conf.Accel_Ref[0]=conf.Accel_Ref[1]=conf.Accel_Ref[2]=0;
komaida424 0:cca1c4e84da4 435 for(i=0; i<16; i++) {
komaida424 2:59ac9df97701 436 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
komaida424 2:59ac9df97701 437 else i2c.Acceleration(&y,&x,&z);
komaida424 0:cca1c4e84da4 438 k[0] += x;
komaida424 0:cca1c4e84da4 439 k[1] += y;
komaida424 0:cca1c4e84da4 440 k[2] += z;
komaida424 2:59ac9df97701 441 wait(0.01);
komaida424 0:cca1c4e84da4 442 }
komaida424 2:59ac9df97701 443 conf.Accel_Ref[0] = k[0]/16;
komaida424 2:59ac9df97701 444 conf.Accel_Ref[1] = k[1]/16;
komaida424 2:59ac9df97701 445 conf.Accel_Ref[2] = k[2]/16-9.8;
komaida424 2:59ac9df97701 446 // FlashLED(3);
komaida424 0:cca1c4e84da4 447 }
komaida424 0:cca1c4e84da4 448
komaida424 0:cca1c4e84da4 449 void PWM_Out(bool mode)
komaida424 0:cca1c4e84da4 450 {
komaida424 0:cca1c4e84da4 451 int reg[3];
komaida424 0:cca1c4e84da4 452 int i;
komaida424 0:cca1c4e84da4 453 float gain;
komaida424 0:cca1c4e84da4 454
komaida424 0:cca1c4e84da4 455 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 456 M1 = M2 = M3 = M4 = Stick[COL];
komaida424 2:59ac9df97701 457 interval = CycleTime.read();
komaida424 2:59ac9df97701 458 CycleTime.reset();
komaida424 2:59ac9df97701 459 if ( interval > 0.1 ) return;
komaida424 2:59ac9df97701 460 TotalTime += interval;
komaida424 2:59ac9df97701 461 if ( TotalTime > 0.5 ) {
komaida424 2:59ac9df97701 462 led1 = !led1;
komaida424 2:59ac9df97701 463 if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f;
komaida424 2:59ac9df97701 464 else buzz=0.0;
komaida424 2:59ac9df97701 465 TotalTime = 0;
komaida424 2:59ac9df97701 466 }
komaida424 2:59ac9df97701 467
komaida424 2:59ac9df97701 468 Get_Angle(interval);
komaida424 2:59ac9df97701 469
komaida424 2:59ac9df97701 470 for ( i=0;i<3;i++ ) {
komaida424 2:59ac9df97701 471
komaida424 0:cca1c4e84da4 472 // Stick Angle Mixing
komaida424 0:cca1c4e84da4 473 if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
komaida424 0:cca1c4e84da4 474 else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
komaida424 2:59ac9df97701 475 if ( Stick[GAIN] > 0
komaida424 2:59ac9df97701 476 // || i == YAW
komaida424 2:59ac9df97701 477 // || (fabsf)(Angle[i]) > conf.Control_Exchange_Angle
komaida424 2:59ac9df97701 478 ) {
komaida424 2:59ac9df97701 479 reg[i] = Stick[i] * conf.Stick_Mix[i];
komaida424 2:59ac9df97701 480 reg[i] += Gyro[i] * gain * GYRO_ADJUST;
komaida424 2:59ac9df97701 481 }
komaida424 2:59ac9df97701 482 else {
komaida424 2:59ac9df97701 483 if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) {
komaida424 2:59ac9df97701 484 if ( abs(Stick_Save[i]) <= abs(Stick[i]>>2) ) {
komaida424 2:59ac9df97701 485 Stick_Save[i] = Stick[i]>>2;
komaida424 2:59ac9df97701 486 }
komaida424 2:59ac9df97701 487 else {
komaida424 2:59ac9df97701 488 // pc.printf("PID RESET.%3d",i);wait(1);
komaida424 2:59ac9df97701 489 Stick_Save[i] = 0;
komaida424 2:59ac9df97701 490 pid[i].reset();
komaida424 2:59ac9df97701 491 Angle[i] = 0;
komaida424 2:59ac9df97701 492 }
komaida424 2:59ac9df97701 493 }
komaida424 2:59ac9df97701 494 reg[i] = Stick[i] * conf.Stick_Mix[i];
komaida424 2:59ac9df97701 495 reg[i] += pid[i].calc(Angle[i],0,interval);
komaida424 2:59ac9df97701 496 // reg[i] = pid[i].calc(Angle[i],-(float)Stick[i]*60/400,interval);
komaida424 2:59ac9df97701 497 }
komaida424 2:59ac9df97701 498 pid_reg[i] = reg[i];
komaida424 0:cca1c4e84da4 499 }
komaida424 0:cca1c4e84da4 500 //Calculate Roll Pulse Width
komaida424 0:cca1c4e84da4 501 M1 += reg[ROL];
komaida424 0:cca1c4e84da4 502 M2 -= reg[ROL];
komaida424 0:cca1c4e84da4 503 M3 -= reg[ROL];
komaida424 0:cca1c4e84da4 504 M4 += reg[ROL];
komaida424 0:cca1c4e84da4 505
komaida424 0:cca1c4e84da4 506 //Calculate Pitch Pulse Width
komaida424 0:cca1c4e84da4 507 M1 += reg[PIT];
komaida424 0:cca1c4e84da4 508 M2 += reg[PIT];
komaida424 0:cca1c4e84da4 509 M3 -= reg[PIT];
komaida424 0:cca1c4e84da4 510 M4 -= reg[PIT];
komaida424 0:cca1c4e84da4 511
komaida424 0:cca1c4e84da4 512 //Calculate Yaw Pulse Width
komaida424 0:cca1c4e84da4 513 M1 -= reg[YAW];
komaida424 0:cca1c4e84da4 514 M2 += reg[YAW];
komaida424 0:cca1c4e84da4 515 M3 -= reg[YAW];
komaida424 0:cca1c4e84da4 516 M4 += reg[YAW];
komaida424 0:cca1c4e84da4 517
komaida424 0:cca1c4e84da4 518 for ( i=0;i<4;i++ )
komaida424 0:cca1c4e84da4 519 {
komaida424 2:59ac9df97701 520 // if ( Stick[COL] > 150 && M[i] < 150 ) M[i] = 150;
komaida424 0:cca1c4e84da4 521 if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
komaida424 0:cca1c4e84da4 522 if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // this is the motor idle level
komaida424 0:cca1c4e84da4 523 }
komaida424 2:59ac9df97701 524
komaida424 2:59ac9df97701 525 if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0;
komaida424 2:59ac9df97701 526
komaida424 2:59ac9df97701 527 if ( mode ) {
komaida424 2:59ac9df97701 528 for ( i=0;i<4;i++ ) {
komaida424 2:59ac9df97701 529 while ( !pwmpin[i] );
komaida424 2:59ac9df97701 530 if ( conf.PWM_Mode == 1 )
komaida424 2:59ac9df97701 531 pwm[i].pulsewidth_us(conf.ESC_Low+M[i]);
komaida424 2:59ac9df97701 532 else pwm[i].pulsewidth_us(M[i]);
komaida424 2:59ac9df97701 533 }
komaida424 2:59ac9df97701 534 }
komaida424 2:59ac9df97701 535
komaida424 0:cca1c4e84da4 536 }
komaida424 0:cca1c4e84da4 537
komaida424 0:cca1c4e84da4 538 void ESC_SetUp(void) {
komaida424 0:cca1c4e84da4 539 while(1) {
komaida424 0:cca1c4e84da4 540 Get_Stick_Pos();
komaida424 2:59ac9df97701 541 for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
komaida424 2:59ac9df97701 542 wait(0.015);
komaida424 0:cca1c4e84da4 543 }
komaida424 2:59ac9df97701 544 }
komaida424 2:59ac9df97701 545
komaida424 2:59ac9df97701 546 void Flight_SetUp(void)
komaida424 2:59ac9df97701 547 {
komaida424 2:59ac9df97701 548 int i;
komaida424 2:59ac9df97701 549 for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(0);
komaida424 2:59ac9df97701 550 for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
komaida424 2:59ac9df97701 551 for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min);
komaida424 2:59ac9df97701 552 for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])*0.02
komaida424 2:59ac9df97701 553 ,conf.PID_Limit,conf.Integral_Limit);
komaida424 2:59ac9df97701 554 Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
komaida424 2:59ac9df97701 555 loop_cnt = 0;
komaida424 2:59ac9df97701 556 FlyghtTime.start();
komaida424 2:59ac9df97701 557 CycleTime.start();
komaida424 2:59ac9df97701 558 FlashLED(5);
komaida424 2:59ac9df97701 559 }
komaida424 2:59ac9df97701 560
komaida424 2:59ac9df97701 561 void LCD_locate(int clm,int row)
komaida424 2:59ac9df97701 562 {
komaida424 2:59ac9df97701 563 lcd.locate(clm,row);
komaida424 2:59ac9df97701 564 i2c.locate(clm,row);
komaida424 2:59ac9df97701 565 }
komaida424 2:59ac9df97701 566
komaida424 2:59ac9df97701 567 void LCD_cls()
komaida424 2:59ac9df97701 568 {
komaida424 2:59ac9df97701 569 lcd.cls();
komaida424 2:59ac9df97701 570 i2c.cls();
komaida424 2:59ac9df97701 571 }
komaida424 2:59ac9df97701 572
komaida424 2:59ac9df97701 573 void LCD_printf(char* str)
komaida424 2:59ac9df97701 574 {
komaida424 2:59ac9df97701 575 lcd.printf(str);
komaida424 2:59ac9df97701 576 i2c.printf(str);
komaida424 2:59ac9df97701 577 }
komaida424 2:59ac9df97701 578 ;
komaida424 0:cca1c4e84da4 579
komaida424 0:cca1c4e84da4 580
komaida424 0:cca1c4e84da4 581
komaida424 0:cca1c4e84da4 582
komaida424 0:cca1c4e84da4 583
komaida424 2:59ac9df97701 584
komaida424 2:59ac9df97701 585