syouichi imamori
/
MulticopterQuadX
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