syouichi imamori
Quad X Type Multicopter
Revision 0:cca1c4e84da4, committed 2013-07-11
- Comitter:
- komaida424
- Date:
- Thu Jul 11 19:18:44 2013 +0000
- Child:
- 1:caafe8935da6
- Commit message:
- version1.03
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/I2cPeripherals/I2cPeripherals.cpp Thu Jul 11 19:18:44 2013 +0000
@@ -0,0 +1,232 @@
+#include "mbed.h"
+#include "I2cPeripherals.h"
+
+I2cPeripherals::I2cPeripherals(PinName sda, PinName scl): _i2c(sda,scl)
+{
+ _i2c.frequency(400000);
+ LCD_contrast = 60;
+}
+
+void I2cPeripherals::start(char I2cAddr,int contrast)
+{
+ LCD_contrast = contrast;
+ start(I2cAddr);
+}
+
+void I2cPeripherals::start(char I2cAddr)
+{
+ char tx[2];
+
+ switch (I2cAddr) {
+ case ST7032_ADDR: //I2C LCD
+ LCD_addr = I2cAddr;
+ LCD_data = 0x40;
+ tx[0] = 0x00;
+ tx[1] = 0x38;
+ _i2c.write(I2cAddr,tx,2);
+ tx[1] = 0x39;
+ _i2c.write(I2cAddr,tx,2);
+ tx[1] = 0x1F;
+ _i2c.write(I2cAddr,tx,2);
+ tx[1] = 0x70 | (LCD_contrast & 0x0F);
+ _i2c.write(I2cAddr,tx,2);
+ tx[1] = 0x5C | ((LCD_contrast >> 4) & 0x03);
+ _i2c.write(I2cAddr,tx,2);
+ tx[1] = 0x6A;
+ _i2c.write(I2cAddr,tx,2);
+ wait(0.3);
+ tx[1] = 0x38;
+ _i2c.write(I2cAddr,tx,2); // function set
+ tx[1] = 0x0C;
+ _i2c.write(I2cAddr,tx,2); // Display On
+ break;
+ case ACM1602_ADDR: //I2C LCD
+ LCD_addr = I2cAddr;
+ LCD_data = 0x80;
+ wait(0.015);
+ tx[0] = 0x00;
+ tx[1] = 0x01;
+ _i2c.write(I2cAddr,tx,2);
+ wait(0.005);
+ tx[1] = 0x38;
+ _i2c.write(I2cAddr,tx,2);
+ wait(0.005);
+ tx[1] = 0x0F;
+ _i2c.write(I2cAddr,tx,2);
+ wait(0.005);
+ tx[1] = 0x06;
+ _i2c.write(I2cAddr,tx,2);
+ wait(0.005);
+ break;
+
+ case ITG3200_ADDR0: //gyro
+ case ITG3200_ADDR1:
+ Gyro_addr = I2cAddr;
+ Gyro_data = 0x1D;
+ tx[0] = 0x16;
+ tx[1] = 0x18; // 0x1D
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x3E;
+ tx[1] = 0x01;
+ _i2c.write(I2cAddr,tx,2);
+ wait_ms(100);
+ break;
+
+ case L3GD20_ADDR0: //gyro
+ case L3GD20_ADDR1:
+ Gyro_addr = I2cAddr;
+ Gyro_data = 0x28;
+ tx[0] = 0x20;
+ tx[1] = 0xFF;
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x21;
+ tx[1] = 0x09;
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x23;
+ tx[1] = 0xF0;
+ _i2c.write(I2cAddr,tx,2);
+ break;
+
+ case LDXL345_ADDR0: //accelametor
+ case LDXL345_ADDR1:
+ Accel_addr = I2cAddr;
+ Accel_data = 0x32;
+ tx[0] = 0x2D;
+ tx[1] = 0x00;
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x31;
+ tx[1] = 0x08; //full range 08:2g 09:4g 0A:8g 0B:16g
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x2C;
+ tx[1] = 0x0C; //out rate 0xC:400Hz 0xD:800Hz
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x2D;
+ tx[1] = 0x08;
+ _i2c.write(I2cAddr,tx,2);
+ break;
+
+ case LPS331AP_ADDR0: //barometor
+ case LPS331AP_ADDR1:
+ barometor_addr = I2cAddr;
+ barometor_data = 0x2B;
+ tx[0] = 0x10; //RES_CNF
+ tx[1] = 0x19;
+ _i2c.write(I2cAddr,tx,2);
+ tx[0] = 0x20; // CTL_REG1
+ tx[1] = 0x80; // power on
+ _i2c.write(I2cAddr,tx,2);
+ tx[0]= 0x21; // CTL_REG2
+ tx[1]= 0x01; // one shot start
+ _i2c.write(I2cAddr,tx,2);
+ break;
+ }
+}
+
+int I2cPeripherals::_putc(int value) {
+ _i2c.start();
+ _i2c.write(LCD_addr);
+ _i2c.write(LCD_data);
+ _i2c.write(value);
+ _i2c.stop();
+ return value;
+}
+
+int I2cPeripherals::_getc() {
+ return -1;
+}
+
+void I2cPeripherals::cls()
+{
+ char tx[2] = { 0x00, 0x01 };
+ _i2c.write(LCD_addr,tx,2);
+}
+
+void I2cPeripherals::locate(int clm,int row)
+{
+ char tx[2];
+
+ tx[0] = 0x00;
+ tx[1] = 0x80 + (row * 0x40) + clm;
+ _i2c.write(LCD_addr,tx,2);
+
+}
+
+int I2cPeripherals::angular(int *x,int *y,int *z)
+{
+ char rx[6];
+ char tx[1];
+ tx[0] = Gyro_data | 0x80;
+ if ( _i2c.write(Gyro_addr,tx,1,true) != 0 ) {
+ x=y=z=0;
+ return false;
+ }
+ _i2c.read(Gyro_addr,rx,6);
+ *x = short(rx[0] << 8 | (uint8_t)rx[1]);
+ *y = short(rx[2] << 8 | (uint8_t)rx[3]);
+ *z = short(rx[4] << 8 | (uint8_t)rx[5]);
+ return true;
+}
+int I2cPeripherals::Acceleration(int *x,int *y,int *z)
+{
+ //AXDL345 Data Read
+ char rx[6];
+ char tx[1];
+ tx[0] = Accel_data | 0x80;
+ if ( _i2c.write(Accel_addr,tx,1,true) != 0 ) {
+ x=y=z=0;
+ return false;
+ }
+ _i2c.read(Accel_addr,rx,6);
+ *y = -short(rx[1] << 8 | (uint8_t)rx[0]);
+ *x = short(rx[3] << 8 | (uint8_t)rx[2]);
+ *z = short(rx[5] << 8 | (uint8_t)rx[4]);
+ return true;
+}
+
+int I2cPeripherals::pressure()
+{
+ char tx[2];
+ char rx[3];
+ int press;
+
+ tx[0]= 0x21;
+ rx[0] = 0;
+ if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
+ return 0;
+ _i2c.read (barometor_addr,rx,1);
+ while( rx[0]&0x01 ) {
+ wait(0.0001);
+ _i2c.write(barometor_addr,tx,1,true);
+ _i2c.read (barometor_addr,rx,1);
+ }
+
+ tx[0]= 0x28 | 0x80;
+ _i2c.write(barometor_addr,tx,1,true);
+ _i2c.read (barometor_addr,rx,3);
+
+ tx[0]= 0x21; // CTL_REG2
+ tx[1]= 0x01; // one shot start
+ _i2c.write(barometor_addr, tx, 2);
+
+ press =int( rx[2]<<16 | rx[1]<<8 | rx[0] );
+ return press;
+}
+
+
+//Update-Methode
+int I2cPeripherals::temperature()
+{
+ char tx[1];
+ char rx[2];
+ int temp;
+
+ tx[0]= 0x2B | 0x80;
+ if ( _i2c.write(barometor_addr,tx,1,true) != 0 )
+ return 0;
+ _i2c.read (barometor_addr,rx,2);
+
+ temp = short(rx[1]<<8 | rx[0]);
+ return temp;
+};
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/I2cPeripherals/I2cPeripherals.h Thu Jul 11 19:18:44 2013 +0000
@@ -0,0 +1,46 @@
+#ifndef MBED_I2cPeripherals_H
+#define MBED_I2cPeripherals_H
+
+#include "mbed.h"
+#include "stdarg.h"
+
+#define LPS331AP_ADDR0 0xB8
+#define LPS331AP_ADDR1 0xBA //select
+#define L3GD20_ADDR0 0xD4
+#define L3GD20_ADDR1 0xD6 //select
+#define ITG3200_ADDR0 0xD0 //select
+#define ITG3200_ADDR1 0xB2
+#define ACM1602_ADDR 0xA0
+#define ST7032_ADDR 0x7C
+#define LDXL345_ADDR0 0xA6 //select
+#define LDXL345_ADDR1 0x3A
+
+class I2cPeripherals : public Stream
+{
+public:
+ I2cPeripherals(PinName sda, PinName scl);
+
+ void start(char);
+ void start(char,int);
+ void cls();
+ void locate(int,int);
+ int angular(int*,int*,int*);
+ int Acceleration(int*,int*,int*);
+ int pressure();
+ int temperature();
+private:
+ virtual int _putc(int value);
+ virtual int _getc();
+ I2C _i2c;
+ int LCD_addr;
+ char LCD_cmd;
+ char LCD_data;
+ int Gyro_addr;
+ char Gyro_data;
+ int Accel_addr;
+ char Accel_data;
+ int barometor_addr;
+ char barometor_data;
+ int LCD_contrast;
+};
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IAP.lib Thu Jul 11 19:18:44 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/okano/code/IAP/#ff906ad52cf9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PulseWidthCounter.lib Thu Jul 11 19:18:44 2013 +0000 @@ -0,0 +1,1 @@ +PulseWidthCounter#d37d6388c179
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SerialLcd.lib Thu Jul 11 19:18:44 2013 +0000 @@ -0,0 +1,1 @@ +SerialLcd#f30bad3f815d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/config.cpp Thu Jul 11 19:18:44 2013 +0000
@@ -0,0 +1,555 @@
+#include "mbed.h"
+#include "I2cPeripherals.h"
+//#include "I2cLCD.h"
+#include "InterruptIn.h"
+//#include "ITG3200.h"
+//#include "LPS331AP.h"
+#include "config.h"
+#include "PulseWidthCounter.h"
+#include "SerialLcd.h"
+
+void FlashLED(int);
+char Check_Stick_Dir(char);
+void Param_Set_Prompt1(const char *,int *,int,int,int,int,char);
+void Param_Set_Prompt1(const char *,float *,int,float,float,float,char);
+void Set_Arrow(int dir);
+void Get_Stick_Pos();
+void CalibrateGyros(void);
+void CalibrateAccel(void);
+void Get_Gyro();
+void Get_Accel();
+void PWM_Out(bool);
+void WriteConfig();
+void ESC_SetUp(void);
+void Get_Pressure();
+
+Timer elaps;
+
+extern int CH[5];
+extern int M[6];
+extern int Gyro[3];
+extern int Accel[3];
+extern int Gyro_Ref[3];
+extern int Stick[5];
+extern float Press;
+const char steering[3][6]= {"Roll ","Pitch","Yaw "};
+short mode;
+char sw,ret_mode;
+short vnum,hnum,vmax,hmax;
+short idx,i;
+char str[33];
+
+#ifdef SERIAL_LCD
+SerialLcd *lcdptr;
+void SetUpPrompt(config& conf,SerialLcd& lcd)
+#else
+I2cPeripherals *lcdptr;
+void SetUpPrompt(config& conf,I2cPeripherals& lcd)
+#endif
+{
+
+ lcdptr = &lcd;
+ lcd.cls();
+ mode = 0;
+ vnum = 0;
+ hnum = 0;
+ vmax = 11;
+
+ while( 1 ) {
+ FlashLED(1);
+ ret_mode = 'W';
+ mode = vnum * 10 + hnum;
+
+ switch ( mode ) {
+ case 0:
+ lcd.locate(0,0);
+ sprintf(str,"Quad-X Ver %4.2f",conf.Revision);
+ lcd.printf(str);
+ lcd.locate(4,1);
+ lcd.printf("By AZUKITEN");
+ hmax = 0;
+ break;
+ case 10: //Calibrate Transmitter
+ lcd.printf("Calibrate Transmitter");
+ Set_Arrow(1);
+ hmax = 1;
+ break;
+ case 11: //Calibrate Transmitter
+ lcd.printf("Start Calibrate");
+ for(i=0; i<4; i++) {
+ conf.Stick_Ref[i] = 0;
+ }
+ for(i=0; i<16; i++) {
+ wait(0.3);
+ conf.Stick_Ref[ROL] += AIL;
+ conf.Stick_Ref[PIT] += ELE;
+ conf.Stick_Ref[YAW] += RUD;
+ conf.Stick_Ref[COL] += THR;
+// conf.Stick_Ref[GAIN] += AUX;
+ }
+ for(i=0; i<4; i++) {
+ conf.Stick_Ref[i] = conf.Stick_Ref[i]/16;
+ }
+ lcd.cls(); //Clear LCD
+ lcd.printf("Calibrate Completed");
+ Set_Arrow(3);
+ FlashLED(5);
+ break;
+ case 20: //Set Gyro Gain
+ lcd.printf("Set Gyro Gain");
+ Set_Arrow(1);
+ hmax = 4;
+ break;
+ case 21: //Set Gyro Gain Roll
+ if ( conf.Gyro_Gain_Setting == 1 )
+ Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[0],2,0.00f,1.00f,0.01f,sw);
+ else
+ Param_Set_Prompt1("GyroGain>Roll",&conf.Gyro_Gain[3],2,-1.00f,1.00f,0.01f,sw);
+ break;
+ case 22:
+ if ( conf.Gyro_Gain_Setting == 1 )
+ Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[1],2,0.00f,1.00f,0.01f,sw);
+ else
+ Param_Set_Prompt1("GyroGain>Pitch",&conf.Gyro_Gain[4],2,-1.00f,1.00f,0.01f,sw);
+ break;
+ case 23:
+ if ( conf.Gyro_Gain_Setting == 1 )
+ Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[2],2,0.00f,1.00f,0.01f,sw);
+ else
+ Param_Set_Prompt1("GyroGain>Yaw",&conf.Gyro_Gain[5],2,-1.00f,1.00f,0.01f,sw);
+ break;
+ case 24:
+// ret_mode = 'R';
+ lcd.printf("GyroGain>setting");
+ lcd.locate(0,1);
+ switch ( sw ) {
+ case 'U':
+ case 'D':
+ conf.Gyro_Gain_Setting *= -1;
+ }
+ if ( conf.Gyro_Gain_Setting == 1 )
+ lcd.printf("Controller");
+ else
+ lcd.printf("Transmitter");
+ Set_Arrow(3);
+ break;
+ case 30: //Set Gyro Direction
+ lcd.printf("Gyro Direction");
+ Set_Arrow(1);
+ hmax = 4;
+ break;
+ case 31: //Set Gyro Direction Roll
+ case 32:
+ case 33:
+ case 34:
+ // ret_mode = 'R';
+ idx = mode - 31;
+ if ( mode == 34 )
+ lcd.printf("Gyro>Swap X-Y");
+ else {
+ lcd.printf("Gyro>Dir>");
+ lcd.locate(9,0);
+ lcd.printf(steering[idx]);
+ }
+ lcd.locate(0,1);
+ switch ( sw ) {
+ case 'U':
+ case 'D':
+ conf.Gyro_Dir[idx] *= -1;
+ }
+ if ( conf.Gyro_Dir[idx] == 1 )
+ lcd.printf("Normal ");
+ else
+ lcd.printf("Reverse");
+ if ( mode == 34 )
+ Set_Arrow(3);
+ else
+ Set_Arrow(2);
+ break;
+ case 40: //Set Accelerometer
+ lcd.printf("Acceleration");
+ lcd.locate(5,1);
+ lcd.printf("Gain");
+ Set_Arrow(1);
+ hmax = 3;
+ break;
+ case 41:
+ Param_Set_Prompt1("Accel>Roll",&conf.Accel_Gain[0],2,0.00f,1.00f,0.01f,sw);
+ break;
+ case 42: //Set Accelerometer
+ Param_Set_Prompt1("Accel>Pith",&conf.Accel_Gain[1],2,0.00f,1.00f,0.01f,sw);
+ break;
+ case 43: //Set Accelerometer
+ ret_mode = 'R';
+ Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Gain[2],3,0.00f,1.00f,0.01f,sw);
+ break;
+
+ case 50: //Set Stick Mixing
+ lcd.printf("Set Stick Mixing");
+ Set_Arrow(1);
+ hmax = 3;
+ break;
+ case 51: //Set Stick Mixing
+ Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw);
+ break;
+ case 52:
+ Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw);
+ break;
+ case 53:
+ Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw);
+ break;
+ case 60: //Display Pulse Width
+ lcd.printf("Disp Pulse Width");
+ Set_Arrow(1);
+ hmax = 2;
+ break;
+ case 61: //Display Pulse Width
+// DisplayPulseWidth(THR,AIL,ELE,RUD,AUX);
+ ret_mode = 'R';
+ lcd.locate(0,0);
+ sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
+ lcd.printf(str);
+ lcd.locate(0,1);
+ sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
+ lcd.printf(str);
+ break;
+ case 62: //Display Stick Ref
+// DisplayPulseWidth(Stick[COL],Stick[ROL],Stick[PIT],Stick[YAW],Stick[GAIN]);
+ ret_mode = 'R';
+ Get_Stick_Pos();
+ lcd.locate(0,0);
+ sprintf(str,"TR=%4d,AL=%4d",Stick[COL],Stick[ROL]);
+ lcd.printf(str);
+ lcd.locate(0,1);
+ sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]);
+ lcd.printf(str);
+ break;
+ case 70: //Display Sensor Value
+ lcd.printf("Disp Senser");
+ Set_Arrow(1);
+ hmax = 5;
+ break;
+ case 71: //Gyro
+ Get_Gyro();
+ lcd.locate(0,0);
+ sprintf(str,"Gyro//X=%5d",Gyro[ROL]);
+ lcd.printf(str);
+ wait(0.02);
+ lcd.locate(0,1);
+ sprintf(str,"y=%5d,Z=%5d",Gyro[PIT],Gyro[YAW]);
+ lcd.printf(str);
+ wait(0.02);
+ ret_mode = 'R';
+ break;
+ case 72: //Accelerometer
+ Get_Accel();
+ lcd.locate(0,0);
+ sprintf(str,"Accel//X=%5d",Accel[ROL]);
+ lcd.printf(str);
+ lcd.locate(0,1);
+ sprintf(str,"Y=%5d,Z=%5d",Accel[PIT],Accel[YAW]);
+ lcd.printf(str);
+// Set_Arrow(2);
+ ret_mode = 'R';
+ break;
+ case 73: // Pressure
+ elaps.reset();
+ elaps.start();
+ Get_Pressure();
+ elaps.stop();
+ lcd.locate(0,0);
+ sprintf(str,"Pressure=%9.3f",Press/4096);
+ lcd.printf(str);
+ lcd.locate(0,1);
+ sprintf(str,"Elaps=%6d",elaps.read_us());
+ lcd.printf(str);
+// Set_Arrow(2);
+ ret_mode = 'R';
+ break;
+ case 74:
+ elaps.reset();
+ elaps.start();
+ PWM_Out(false);
+ elaps.stop();
+ lcd.locate(0,0);
+ sprintf(str,"ElapsTime=%5d",elaps.read_us());
+ lcd.printf(str);
+ Set_Arrow(2);
+ ret_mode = 'R';
+ break;
+ case 75: //Sensor Calibration
+// CalibrateAccel();
+ CalibrateGyros();
+ CalibrateAccel();
+ lcd.printf("Calibrate completed");
+ Set_Arrow(3);
+ break;
+ case 80: //Display PMW Condition
+ lcd.printf("Display PMW ");
+ Set_Arrow(1);
+ hmax = 1;
+ break;
+ case 81: //Display PMW Width
+ PWM_Out(false);
+// DisplayInt(Motor_HD[0],M1,M2,M4,M3);
+ ret_mode = 'R';
+ lcd.locate(0,0);
+ sprintf(str,"M1=%4d,M2=%4d",M1,M2);
+ lcd.printf(str);
+ lcd.locate(0,1);
+ sprintf(str,"M4=%4d,M3=%4d",M4,M3);
+ lcd.printf(str);
+ break;
+ case 90: //パラメーター設定
+ lcd.printf("Parameter Set");
+ Set_Arrow(1);
+ hmax = 4;
+ break;
+ case 91:
+ Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw);
+ break;
+ case 92:
+ lcd.locate(0,0);
+ lcd.printf("PWM>Mode");
+ lcd.locate(0,1);
+ switch ( sw ) {
+ case 'U':
+ case 'D':
+ conf.PWM_Mode *= -1;
+ }
+ if ( conf.PWM_Mode == 1 )
+ lcd.printf("ESC ");
+ else
+ lcd.printf("Moter");
+ Set_Arrow(2);
+ break;
+ case 93:
+ Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw);
+// for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ break;
+ case 94:
+ lcd.locate(0,0);
+ lcd.printf("Gyro>Type");
+ lcd.locate(0,1);
+ switch ( sw ) {
+ case 'U':
+ case 'D':
+ if ( conf.Gyro_Type == _L3GD20 )
+ conf.Gyro_Type = _ITG3200;
+ else
+ conf.Gyro_Type = _L3GD20;
+ }
+ if ( conf.Gyro_Type == _L3GD20 )
+ lcd.printf("L3GD20");
+ else
+ lcd.printf("ITG3200");
+ Set_Arrow(2);
+ break;
+ case 100: //E2PROM Store
+ lcd.printf("Config Save");
+ Set_Arrow(1);
+ hmax = 1;
+ break;
+ case 101:
+ WriteConfig();
+ FlashLED(5);
+ lcd.locate(0,0);
+ sprintf(str,"Config %3dbyte",sizeof(config));
+ lcd.printf(str);
+ lcd.locate(0,1);
+ lcd.printf("Save Sucssesuful");
+ Set_Arrow(3);
+ wait(2);
+ break;
+ case 110: //ESC Set up
+ lcd.locate(0,0);
+ lcd.printf("ESC Set UP");
+ Set_Arrow(1);
+ hmax = 1;
+ break;
+ case 111:
+ if ( conf.PWM_Mode == -1 ) {
+ hnum = 0;
+ break;
+ }
+ lcd.locate(0,0);
+ lcd.printf("Setup Start");
+ wait(1.5);
+ lcd.cls(); //Clear LCD
+ lcd.locate(0,0);
+ lcd.printf("Please power-off");
+ lcd.locate(1,0);
+ lcd.printf(" after Setup");
+
+ ESC_SetUp();
+ break;
+
+ default:
+ if ( hnum == 0 )
+ vnum++;
+ }
+
+
+ sw = Check_Stick_Dir(ret_mode); //Wait Mode
+
+ switch ( sw ) {
+ case 'L':
+ hnum--;
+ if ( hnum <= 0 ) hnum = 0;
+ lcd.cls(); //Clear LCD
+ break;
+ case 'R':
+ lcd.cls();
+ if ( hnum < hmax ) hnum++;
+ break;
+ case 'U':
+ if ( hnum == 0 ) {
+ if ( vnum < vmax ) vnum++;
+ else vnum = 0;
+ lcd.cls(); //Clear LCD
+ }
+ break;
+ case 'D':
+ if ( hnum == 0 ) {
+ if ( vnum > 0 ) vnum--;
+ else vnum = vmax;
+ lcd.cls(); //Clear LCD
+ }
+ break;
+ case 'E':
+ lcd.cls(); //Clear LCD
+ lcd.locate(0,0);
+ lcd.printf("PWM Started");
+ return;
+ }
+ }
+
+}
+
+char Check_Stick_Dir(char act)
+{
+ int i;
+ while ( 1 ) {
+ Get_Stick_Pos();
+// DisplayInt(Stick[0],Stick[1],Stick[2],Stick[3]);
+// wait(0.2);
+ if ( Stick[YAW] > Stick_Limit ) {
+ i = 0;
+ while ( Stick[YAW] > Stick_Limit && Stick[COL] < 30 ) {
+ if ( i > 2000 ) return 'E'; //wait 2 sec
+ wait(0.001); // wait 1 msec
+ Get_Stick_Pos();
+ i++;
+ }
+ }
+ if ( Stick[ROL] > Stick_Limit ) {
+ wait(0.03);
+ Get_Stick_Pos();
+ if ( !(Stick[ROL] > Stick_Limit) ) continue;
+ while ( Stick[ROL] > Stick_Limit ) {
+ Get_Stick_Pos();
+ }
+ return 'R';
+ }
+ if ( Stick[ROL] < -Stick_Limit ) {
+ wait(0.03);
+ Get_Stick_Pos();
+ if ( !(Stick[ROL] < -Stick_Limit) ) continue;
+ while ( Stick[ROL] < -Stick_Limit ) {
+ Get_Stick_Pos();
+ }
+ return 'L';
+ }
+ if ( Stick[PIT] < -Stick_Limit ) {
+ wait(0.03);
+ Get_Stick_Pos();
+ if ( !(Stick[PIT] < -Stick_Limit) ) continue;
+ if ( act == 'R' ) {
+ wait(0.03);
+ return 'D';
+ }
+ while ( Stick[PIT] < -Stick_Limit ) {
+ Get_Stick_Pos();
+ }
+ return 'D';
+ }
+ if ( Stick[PIT] > Stick_Limit ) {
+ wait(0.03);
+ Get_Stick_Pos();
+ if ( !( Stick[PIT] > Stick_Limit) ) continue;
+ if ( act == 'R' ) {
+ wait(0.03);
+ return 'U';
+ }
+ while ( Stick[PIT] > Stick_Limit ) {
+ Get_Stick_Pos();
+ }
+ return 'U';
+ }
+ if ( act == 'R' )
+ return ' ';
+ }
+}
+
+void Param_Set_Prompt1(const char *hd,int *num,int arrow,int min,int max,int increase,char sw)
+{
+ ret_mode = 'R';
+ lcdptr->locate(0,0);
+ lcdptr->printf(hd);
+ lcdptr->locate(0,1);
+ lcdptr->printf("%d",*num);
+ Set_Arrow(arrow);
+ switch ( sw ) {
+ case 'U':
+ *num -= increase;
+ if ( *num <= min )
+ *num = min;
+ break;
+ case 'D':
+ *num += increase;
+ if ( *num >= max )
+ *num = max;
+ }
+}
+void Param_Set_Prompt1(const char *hd,float *num,int arrow,float min,float max,float increase,char sw)
+{
+ ret_mode = 'R';
+ lcdptr->locate(0,0);
+ lcdptr->printf(hd);
+ lcdptr->locate(0,1);
+ sprintf(str,"%7.2f",*num);
+ lcdptr->printf(str);
+ Set_Arrow(arrow);
+ switch ( sw ) {
+ case 'U':
+ *num -= increase;
+ if ( *num <= min )
+ *num = min;
+ break;
+ case 'D':
+ *num += increase;
+ if ( *num >= max )
+ *num = max;
+ }
+}
+
+void Set_Arrow(int dir)
+{
+ lcdptr->locate(12,1);
+ switch ( dir ) {
+ case 1:
+ lcdptr->printf(" >>");
+ break;
+ case 2:
+ lcdptr->printf("<<>>");
+ break;
+ case 3:
+ lcdptr->printf(" <<");
+ }
+}
+
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h Thu Jul 11 19:18:44 2013 +0000
@@ -0,0 +1,106 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+#define SERIAL_LCD
+//#define LPCXpresso
+//#define LocalFileOut
+
+#define TX_TYPE 0 // 0:FM 1:IR
+#define Thro_Min 25
+#define Thro_Lo 75
+#define Thro_Hi 950
+#define Pulse_Min 1000
+#define Pulse_Max 2000
+#define Stick_Limit 150
+#define M1 M[0]
+#define M2 M[1]
+#define M3 M[2]
+#define M4 M[3]
+#define M5 M[4]
+#define M6 M[5]
+#define THR CH[0]
+#define AIL CH[1]
+#define ELE CH[2]
+#define RUD CH[3]
+#define AUX CH[4]
+#define _ITG3200 0x00
+#define _L3GD20 0x01
+
+//enum PortNum{Port_THR=1,Port_AIL=3,Port_ELE=5,Port_RUD=7,Port_AUX=6};
+enum IRNum{IR_THR=1,IR_AIL,IR_ELE,IR_RUD,IR_AUX}; //Input Pulse Sequence
+enum channel{ROL=0,PIT,YAW,COL,GAIN};
+
+
+struct config {
+ float Revision;
+ int Struct_Size;
+ int Stick_Ref[5]; //Stick Neutral Position
+ float Stick_Mix[3]; //Mixing ratio of stick operation
+ signed char Gyro_Dir[4];
+ float Gyro_Gain[6];
+ signed char Accel_Dir[4];
+ float Accel_Gain[3];
+ int Gyro_Gain_Setting;
+ int Gyro_Divider;
+ int Gyro_LPF_Value;
+ int Gyro_SamplRate_Divider;
+ int LCD_Contrast;
+ int PWM_Mode;
+ float Gyro_Stick_offset_effect;
+ int PWM_Interval;
+// int Accel_Rang;
+// int Accel_Rate;
+// int PWM_Resolution;
+ char StartMode;
+ char Gyro_Type;
+public:
+ config() {
+ Revision = 1.03;
+ Struct_Size = sizeof(config);
+ Stick_Ref[0] = 1500;
+ Stick_Ref[1] = 1500;
+ Stick_Ref[2] = 1500;
+ Stick_Ref[3] = 1100;
+ Stick_Ref[4] = 1500;
+ Stick_Mix[0] = 0.5;
+ Stick_Mix[1] = 0.5;
+ Stick_Mix[2] = 1.0;
+ Gyro_Dir[0] = 1;
+ Gyro_Dir[1] = 1;
+ Gyro_Dir[2] = 1;
+ Gyro_Dir[3] = -1;
+ Gyro_Gain[0] = 0.40;
+ Gyro_Gain[1] = 0.40;
+ Gyro_Gain[2] = 0.40;
+ Gyro_Gain[3] = 0.00;
+ Gyro_Gain[4] = 0.00;
+ Gyro_Gain[5] = 0.00;
+ Accel_Dir[0] = 1;
+ Accel_Dir[1] = 1;
+ Accel_Dir[2] = 1;
+ Accel_Dir[3] = 1;
+ Accel_Gain[0] = 0.50;
+ Accel_Gain[1] = 0.50;
+ Accel_Gain[2] = 0.50;
+ Gyro_Gain_Setting = -1;
+ Gyro_Divider=7;
+ Gyro_LPF_Value=0;
+ Gyro_SamplRate_Divider=1;
+ LCD_Contrast = 60;
+ PWM_Mode = 1;
+ Gyro_Stick_offset_effect = 0.00;
+ PWM_Interval = 3000;
+// Accel_Rang = 2;
+// Accel_Rate = 14;
+// PWM_Resolution = 0;
+ StartMode = 'C';
+ Gyro_Type = _ITG3200;
+ }
+};
+#endif
+
+
+
+
+
+
--- /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);
+ }
+};
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jul 11 19:18:44 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file