syouichi imamori
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 0:cca1c4e84da4
- Child:
- 2:59ac9df97701
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Jul 11 19:18:44 2013 +0000
@@ -0,0 +1,408 @@
+#include "mbed.h"
+#include "I2cPeripherals.h"
+//#include "I2cLCD.h"
+#include "InterruptIn.h"
+//#include "ITG3200.h"
+//#include "L3GD20.h"
+#include "config.h"
+#include "PulseWidthCounter.h"
+#include "string"
+#include "SerialLcd.h"
+#include "IAP.h"
+//LPC1768 Flash Memory read/write
+#define MEM_SIZE 256
+#define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
+IAP iap;
+
+#ifdef LPCXpresso
+DigitalOut led1(P0_22);
+#define led2 led1
+#else
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+#endif
+InterruptIn ch1(p5);
+PulseWidthCounter ch[5] = { PulseWidthCounter(p6,POSITIVE),
+ PulseWidthCounter(p7,POSITIVE),
+ PulseWidthCounter(p8,POSITIVE),
+ PulseWidthCounter(p9,POSITIVE),
+ PulseWidthCounter(p10,POSITIVE)
+ };
+PwmOut pwm[4] = { p21,p22,p23,p24 };
+Timer CurTime;
+Timer ElapTime;
+I2cPeripherals i2c(p28,p27); //sda scl
+#ifdef SERIAL_LCD
+SerialLcd lcd(p13);
+#endif
+config conf;
+int StartTime;
+int Channel = 0;
+int CH[5];
+int M[6];
+int Gyro[3];
+int Accel[3];
+int Gyro_Ref[3];
+int Accel_Ref[3];
+int Stick[5];
+float Press;
+char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
+
+void initialize();
+void FlashLED(int );
+void SetUp();
+#ifdef SERIAL_LCD
+void SetUpPrompt(config&,SerialLcd&);
+#else
+void SetUpPrompt(config&,I2cPeripherals&);
+#endif
+void PWM_Out(bool);
+void Get_Stick_Pos();
+void CalibrateGyros(void);
+void CalibrateAccel(void);
+void Get_Gyro();
+void Get_Accel();
+void ReadConfig();
+void WriteConfig();
+void ESC_SetUp(void);
+
+void PulseCheck()
+{
+ Channel++;
+}
+
+void PulseAnalysis() //Interrupt Pin5
+{
+ CurTime.stop();
+ int PulseWidth = CurTime.read_us();
+ CurTime.reset();
+ CurTime.start();
+ if ( PulseWidth > 3000 ) Channel = 0; //reset pulse count
+ else {
+ if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
+ switch( Channel ) {
+ case IR_THR:
+ THR = PulseWidth;
+ break;
+ case IR_AIL:
+ AIL = PulseWidth;
+ break;
+ case IR_ELE:
+ ELE = PulseWidth;
+ break;
+ case IR_RUD:
+ RUD = PulseWidth;
+ break;
+ case IR_AUX:
+ AUX = PulseWidth;
+ break;
+ }
+ }
+ }
+ Channel++;
+}
+
+int main()
+{
+ int i,j=0;
+
+ initialize();
+ wait(0.5);
+ Get_Stick_Pos();
+ while ( Stick[COL] > 30 || conf.StartMode == 'C' ) //Shrottol Low
+ {
+ if ( Stick[COL] > 350 || conf.StartMode == 'C' ) // Shrottle High
+ {
+#ifdef SERIAL_LCD
+ SetUpPrompt(conf,lcd);
+#else
+ SetUpPrompt(conf,i2c);
+#endif
+ for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ break;
+ }
+ FlashLED(3);
+ wait(1);
+ Get_Stick_Pos();
+ }
+ led2 = 1;
+ ElapTime.start();
+
+ while (1)
+ {
+ if ( Stick[COL] < 30 )
+ {
+ i = 0;
+ ElapTime.stop();
+ while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 )
+ {
+ if ( i > 100 ) //wait 2 sec
+ {
+ CalibrateGyros();
+ CalibrateAccel();
+ FlashLED(6);
+ ElapTime.start();
+ break;
+ }
+ wait(0.01); // wait 10 msec
+ Get_Stick_Pos();
+ i++;
+ }
+ }
+ j++;
+ if (j>100) { j=0; led2 = !led2;}
+ ElapTime.stop();
+ wait(float(conf.PWM_Interval-ElapTime.read_us()-2)/1000000);
+ ElapTime.reset();
+ ElapTime.start();
+ PWM_Out(true);
+ }
+
+}
+
+void initialize()
+{
+#ifndef SERIAL_LCD
+ i2c.start(ST7032_ADDR,conf.LCD_Contrast);
+#endif
+ ReadConfig(); //config.inf file read
+// CurTime.start();
+ Channel = 0;
+ ch1.rise(&PulseCheck); //input pulse count
+ wait(0.1);
+ if ( Channel > 30 ) {
+ ch1.rise(&PulseAnalysis);
+ InPulseMode = 'S';
+ }
+ else InPulseMode = 'P';
+ if ( conf.Gyro_Type == _ITG3200 )
+ i2c.start(ITG3200_ADDR0);
+ else
+ i2c.start(L3GD20_ADDR1);
+ CalibrateGyros();
+ i2c.start(LDXL345_ADDR0);
+ CalibrateGyros();
+ CalibrateAccel();
+ i2c.start(LPS331AP_ADDR1);
+ for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+}
+
+void FlashLED(int cnt)
+{
+ for ( int i = 0 ; i < cnt ; i++ ) {
+ led1 = !led1;
+ wait(0.05);
+ led1 = !led1;
+ wait(0.05);
+ }
+}
+
+void ReadConfig()
+{
+#ifdef LocalFileOut
+ LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+ FILE *fp = fopen("/local/setup.inf", "rb"); // Open "out.txt" on the local file system for writing
+ if ( fp != NULL ) {
+ float rev = conf.Revision;
+ int len = fread(&conf,1,sizeof(config),fp);
+ switch ( len ) {
+ case sizeof(config): // File size ok
+ if ( rev == conf.Revision ) break;
+ default:
+ fclose(fp);
+ config init;
+ conf = init;
+ fp = fopen("/local/setup.inf", "wb");
+ fwrite(&conf,1,sizeof(config),fp);
+ }
+ fclose(fp);
+ } else {
+ WriteConfig();
+ wait(2);
+ }
+#else
+ char *send;
+ char *recv;
+ int i,rc;
+ config *conf_ptr;
+
+ if ( sizeof(config) > 255 ) {
+ i2c.printf("config size over");
+ return;
+ }
+ rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
+ if ( rc == SECTOR_NOT_BLANK ) {
+ send = sector_start_adress[TARGET_SECTOR];
+ recv = (char*)&conf;
+ conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
+ if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
+ for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
+// i2c.printf("config read OK");
+// wait(1);
+ return;
+ }
+ }
+ WriteConfig();
+// i2c.printf("config write OK");
+// wait(1);
+#endif
+}
+
+void WriteConfig()
+{
+#ifdef LocalFileOut
+ LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+ FILE *fp = fopen("/local/setup.inf", "wb");
+ fwrite(&conf,1,sizeof(config),fp);
+ fclose(fp);
+#else
+ char mem[MEM_SIZE];
+ char *send;
+ int i;
+ if ( sizeof(config) > 255 ) {
+ printf("config size over");
+ return;
+ }
+ send = (char*)&conf;
+ for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
+ for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ iap.erase( TARGET_SECTOR, TARGET_SECTOR );
+ iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
+ iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
+#endif
+}
+
+void Get_Stick_Pos(void)
+{
+ if ( InPulseMode == 'P' ) {
+ for (int i=0;i<5;i++) CH[i] = ch[i].count;
+ }
+ Stick[ROL] = AIL - conf.Stick_Ref[ROL];
+ Stick[PIT] = ELE - conf.Stick_Ref[PIT];
+ Stick[YAW] = RUD - conf.Stick_Ref[YAW];
+ Stick[COL] = THR - conf.Stick_Ref[COL];
+ Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
+}
+
+void Get_Gyro()
+{
+ int x,y,z;
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
+ else i2c.angular(&y,&x,&z);
+ Gyro[ROL] = ( x - Gyro_Ref[0] ) / 5;
+ Gyro[PIT] = ( y - Gyro_Ref[1] ) / 5;
+ Gyro[YAW] = ( z - Gyro_Ref[2] ) / 5;
+}
+
+void Get_Accel()
+{
+ int x,y,z;
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
+ else i2c.Acceleration(&y,&x,&z);
+ Accel[ROL] = ( x - Accel_Ref[0] );
+ Accel[PIT] = ( y - Accel_Ref[1] );
+ Accel[YAW] = ( z - Accel_Ref[2] );
+}
+void Get_Pressure()
+{
+ Press = i2c.pressure();
+}
+
+void CalibrateGyros(void)
+{
+ int i,j,x,y,z;
+ int k[3]={0,0,0};
+ wait(1);
+ for(i=0; i<16; i++) {
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
+ else i2c.angular(&y,&x,&z);
+ k[0] += x;
+ k[1] += y;
+ k[2] += z;
+ wait(0.005);
+ }
+ for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16;
+ FlashLED(3);
+}
+
+void CalibrateAccel(void)
+{
+ int i,j,x,y,z;
+ int k[3]={0,0,0};
+ wait(1);
+ for(i=0; i<16; i++) {
+ if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
+ else i2c.Acceleration(&y,&x,&z);
+ k[0] += x;
+ k[1] += y;
+ k[2] += z;
+ wait(0.005);
+ }
+ for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16;
+ FlashLED(3);
+}
+
+void PWM_Out(bool mode)
+{
+ int reg[3];
+ int i;
+ float gain;
+
+// wait(0.002);
+ Get_Stick_Pos();
+ Get_Gyro();
+// Get_Accel();
+
+ M1 = M2 = M3 = M4 = Stick[COL];
+ for ( i=0;i<3;i++ )
+ {
+// Stick Angle Mixing
+ if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
+ else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
+ reg[i] = ( Stick[i] * conf.Stick_Mix[i] ) + ( Gyro[i] * gain );
+// if ( Stick[GAIN] < 0 )
+// reg[i] += Accel[i] * conf.Accel_Gain[i];
+ }
+ //Calculate Roll Pulse Width
+ M1 += reg[ROL];
+ M2 -= reg[ROL];
+ M3 -= reg[ROL];
+ M4 += reg[ROL];
+
+ //Calculate Pitch Pulse Width
+ M1 += reg[PIT];
+ M2 += reg[PIT];
+ M3 -= reg[PIT];
+ M4 -= reg[PIT];
+
+ //Calculate Yaw Pulse Width
+ M1 -= reg[YAW];
+ M2 += reg[YAW];
+ M3 -= reg[YAW];
+ M4 += reg[YAW];
+
+ for ( i=0;i<4;i++ )
+ {
+ if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
+ if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // this is the motor idle level
+ }
+
+ if (Stick[COL] < 20 ) M1=M2=M3=M4=0;
+ if ( mode )
+ for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+M[i]);
+
+}
+
+void ESC_SetUp(void) {
+ while(1) {
+ Get_Stick_Pos();
+ for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(Stick[COL]);
+ wait(0.01);
+ }
+};
+
+
+
+
+