syouichi imamori
/
MulticopterQuadX
Quad X Type Multicopter
Revision 4:4060309b9cc0, committed 2014-10-14
- Comitter:
- komaida424
- Date:
- Tue Oct 14 08:15:03 2014 +0000
- Parent:
- 3:27407c4984cf
- Child:
- 5:7b02775787a9
- Commit message:
- rev.2.1
Changed in this revision
--- a/I2cPeripherals/I2cPeripherals.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/I2cPeripherals/I2cPeripherals.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -11,6 +11,9 @@ Gyro_addr = 0; Accel_addr = 0; barometor_addr = 0; + ultrasonic_addr = 0; + ultrasonic_distance = 0; +//pc.printf("start"); find(); start(); @@ -71,26 +74,28 @@ void I2cPeripherals::find() { +#define sensnum 11 char tx[2]; char rx[1]; - SensorInf sens[10] = { 0xD0,0x68,0x00, //ITG3200 + SensorInf sens[sensnum] = { 0xD0,0x68,0x00, //ITG3200 gyro 0xD2,0x68,0x00, - 0xD0,0x68,0x75, //MPU6050 + 0xD0,0x68,0x75, //MPU6050 gyro/accel 0xD2,0x68,0x75, - 0xD4,0xD4,0x0F, //L3GD20 + 0xD4,0xD4,0x0F, //L3GD20 gyro 0xD6,0xD4,0x0F, - 0xA6,0xE5,0x00, //ADXL345 + 0xA6,0xE5,0x00, //ADXL345 accel 0x3A,0xE5,0x00, - 0xB8,0xBB,0x0F, //LPS331 - 0xBA,0xBB,0x0F - }; + 0xB8,0xBB,0x0F, //LPS331 baro + 0xBA,0xBB,0x0F, + 0xE0,0x18,0x01 //SRF02 ultrasonic + }; - for ( int num = 0; num < 10; num++ ) { + for ( int num = 0; num < sensnum; num++ ) { tx[0]= sens[num].whoaddr; if ( _i2c.write(sens[num].addr,tx,1,true) != 0 ) continue; _i2c.read (sens[num].addr,rx,1); if ( (rx[0]&0x7E) != (sens[num].who&0x7E) ) continue; - +//pc.printf("who=%2x",rx[0]); switch ( num ) { #ifdef ITG3200 case 0: //ITG3200 @@ -185,6 +190,14 @@ wait(1); break; #endif + case 10: //SFR02 ultrasonic + ultrasonic_addr = sens[num].addr; + ultrasonic_data = 0x02; + tx[0] = 0x00; + tx[1] = 0x52; + _i2c.write(sens[num].addr,tx,2); + wait(0.01); + break; } } } @@ -205,7 +218,7 @@ return -1; } -void I2cPeripherals::write_reg(char I2cAddr,char reg_addr,char* data,int len) +void I2cPeripherals::write_reg(int I2cAddr,char reg_addr,char* data,int len) { char tx[17]; if ( len >16 ) len = 16; @@ -215,19 +228,45 @@ _i2c.write(I2cAddr,tx,len+1); } -void I2cPeripherals::read_reg(char I2cAddr,char reg_addr,char* data,int len) +void I2cPeripherals::read_reg(int I2cAddr,char reg_addr,char* data,int len) { char tx[1]; tx[0] = reg_addr | 0x80; _i2c.write(I2cAddr,tx,1,true); _i2c.read(I2cAddr,data,len); } +int I2cPeripherals::write_EEPROM(short reg_addr,char* data,int len) +{ + char tx[3]; + int rc=0,i; + int I2cAddr = I2C_EEPROM_ADDR; + for (i=0; i<len; i++) { + tx[0] = reg_addr >> 8; + tx[1] = reg_addr & 0xFF; + tx[2] = data[i]; + rc = _i2c.write(I2cAddr,tx,3); + if ( rc ) break; + wait(0.01); + reg_addr ++; + } + return rc; +} + +int I2cPeripherals::read_EEPROM(short reg_addr,char* data,int len) +{ + int I2cAddr = I2C_EEPROM_ADDR; + int rc = _i2c.write(I2cAddr,(char*)& reg_addr,2,true); + if ( rc ) return rc; + rc = _i2c.read(I2cAddr,data,len); + return rc; +} void I2cPeripherals::cls() { if ( LCD_addr == 0 ) return; char tx[2] = { 0x00, 0x01 }; _i2c.write(LCD_addr,tx,2); + wait(0.001); } void I2cPeripherals::locate(int clm,int row) @@ -238,7 +277,7 @@ tx[0] = 0x00; tx[1] = 0x80 + (row * 0x40) + clm; _i2c.write(LCD_addr,tx,2); - + wait(0.001); } int I2cPeripherals::angular(float *x,float *y,float *z) @@ -276,7 +315,7 @@ tx[0] = Accel_data | 0x80; if ( _i2c.write(Accel_addr,tx,1,true) != 0 ) { *x=*y=0; - *z=9.8; + *z=1; return false; } _i2c.read(Accel_addr,rx,6); @@ -285,8 +324,10 @@ case 0xD2: #ifdef MPU6050 lsb = 0.000488; //16g -// lsb = 0.000122; //4g - *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb; +// lsb = 0.000244; //8g +// lsb = 0.000122; //4g +// lsb = 0.000061; //2g + *y = ( -(short(rx[0] << 8 | (uint8_t)rx[1])) ) * lsb;//re *x = ( short(rx[2] << 8 | (uint8_t)rx[3]) ) * lsb; *z = ( short(rx[4] << 8 | (uint8_t)rx[5]) ) * lsb; #endif @@ -294,9 +335,10 @@ case 0xA6: case 0x3A: #ifdef ADXL345 - *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * 0.04; - *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * 0.04; - *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * 0.04; + lsb = 0.004; + *y = ( -(short(rx[1] << 8 | (uint8_t)rx[0])) ) * lsb;//re + *x = ( short(rx[3] << 8 | (uint8_t)rx[2]) ) * lsb; + *z = ( short(rx[5] << 8 | (uint8_t)rx[4]) ) * lsb; break; #endif } @@ -332,6 +374,40 @@ return temp; } +float I2cPeripherals::height_cm() +{ + return (float)ultrasonic(0x52)*0.0175f; +} +float I2cPeripherals::height_mm() +{ + return (float)ultrasonic(0x52)*0.175f; +} + +float I2cPeripherals::height_us() +{ + return (float)ultrasonic(0x52)/1000000; +} + +int I2cPeripherals::ultrasonic(char unit) +{ + char rx[2],tx[2]; + tx[0] = 0x00; + if ( ultrasonic_addr == 0 ) return -1; + _i2c.write(ultrasonic_addr,tx,1,true); + _i2c.read (ultrasonic_addr,rx,1); + if ( rx[0] != 0xFF ) { + tx[0]= 0x02; + _i2c.write(ultrasonic_addr,tx,1,true); + _i2c.read (ultrasonic_addr,rx,2); + ultrasonic_distance = short(rx[0] << 8 | (uint8_t)rx[1]); + tx[0] = 0x00; + tx[1] = unit; + _i2c.write(ultrasonic_addr,tx,2); + } + return ultrasonic_distance; + +} + void I2cPeripherals::i2c_write(int i2caddr,char* tx,int len) { char rx[1]; @@ -355,6 +431,8 @@ return LCD_addr; case BARO_ADDR: return barometor_addr; + case ULTRASONIC_ADDR: + return ultrasonic_addr; } return 0; } @@ -364,3 +442,6 @@ + + +
--- a/I2cPeripherals/I2cPeripherals.h Thu Feb 13 16:07:07 2014 +0000 +++ b/I2cPeripherals/I2cPeripherals.h Tue Oct 14 08:15:03 2014 +0000 @@ -15,15 +15,19 @@ #define ACCEL_ADDR 1 #define LCD_ADDR 3 #define BARO_ADDR 4 +#define ULTRASONIC_ADDR 5 +#define I2C_EEPROM_ADDR 0xA0 class I2cPeripherals : public Stream { public: - I2cPeripherals(PinName sda=p28, PinName scl=p27); + I2cPeripherals(PinName , PinName ); void start(int contrast=60); - void write_reg(char,char,char*,int); - void read_reg(char,char,char*,int); + void write_reg(int,char,char*,int); + void read_reg(int,char,char*,int); + int write_EEPROM(short,char*,int); + int read_EEPROM(short,char*,int); void cls(); void locate(int,int); int GetAddr(int); @@ -31,11 +35,15 @@ int Acceleration(float*,float*,float*); int pressure(); int temperature(); + float height_cm(); + float height_mm(); + float height_us(); private: virtual int _putc(int value); virtual int _getc(); void i2c_write(int,char*,int); void find(); + int ultrasonic(char); I2C _i2c; int LCD_addr; char LCD_cmd; @@ -46,6 +54,9 @@ char Accel_data; int barometor_addr; char barometor_data; + int ultrasonic_addr; + char ultrasonic_data; + int ultrasonic_distance; int LCD_contrast; struct SensorInf { char addr;
--- a/IAP/IAP.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/IAP/IAP.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -30,7 +30,7 @@ #define USER_FLASH_AREA_START_STR( x ) STR( x ) #define STR( x ) #x -unsigned char user_area[ USER_FLASH_AREA_SIZE ] __attribute__((section( ".ARM.__at_" USER_FLASH_AREA_START_STR( USER_FLASH_AREA_START ) ), zero_init)); +//unsigned char user_area[ USER_FLASH_AREA_SIZE ] __attribute__((section( ".ARM.__at_" USER_FLASH_AREA_START_STR( USER_FLASH_AREA_START ) ), zero_init)); /* @@ -56,7 +56,7 @@ IAPCommand_Compare, IAPCommand_Reinvoke_ISP, IAPCommand_Read_device_serial_number, -#if defined(TARGET_LPC11U24) +#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) IAPCommand_EEPROM_Write = 61, IAPCommand_EEPROM_Read, #endif @@ -228,7 +228,7 @@ return ( USER_FLASH_AREA_SIZE ); } -#if defined(TARGET_LPC11U24) +#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) /** Copy RAM to EEPROM (LPC11U24) * * @param source_addr Source RAM address from which data bytes are to be read. @@ -270,3 +270,5 @@ } #endif + +
--- a/IAP/IAP.h Thu Feb 13 16:07:07 2014 +0000 +++ b/IAP/IAP.h Tue Oct 14 08:15:03 2014 +0000 @@ -29,7 +29,7 @@ #include "mbed.h" -#if defined(TARGET_LPC1768) +#if defined(TARGET_LPC1768)// || defined(TARGET_NUCLEO_F401RE) #define USER_FLASH_AREA_START FLASH_SECTOR_29 //#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE_16_TO_29 * 1) @@ -148,6 +148,58 @@ (char *)FLASH_SECTOR_29 }; +#elif defined(TARGET_NUCLEO_F401RE) //dummy + +#define FLASH_SECTOR_29 0x00078000 +#define FLASH_SECTOR_SIZE_0_TO_15 ( 4 * 1024) +#define FLASH_SECTOR_SIZE_16_TO_29 (32 * 1024) +#define USER_FLASH_AREA_START FLASH_SECTOR_29 +//#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE_16_TO_29 * 1) +#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE_16_TO_29 * 1) + + +#elif defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) + +#define USER_FLASH_AREA_START FLASH_SECTOR_15 +#define USER_FLASH_AREA_SIZE (FLASH_SECTOR_SIZE * 1) + +#define FLASH_SECTOR_0 0x00000000 +#define FLASH_SECTOR_1 0x00001000 +#define FLASH_SECTOR_2 0x00002000 +#define FLASH_SECTOR_3 0x00003000 +#define FLASH_SECTOR_4 0x00004000 +#define FLASH_SECTOR_5 0x00005000 +#define FLASH_SECTOR_6 0x00006000 +#define FLASH_SECTOR_7 0x00007000 +#define FLASH_SECTOR_8 0x00008000 +#define FLASH_SECTOR_9 0x00009000 +#define FLASH_SECTOR_10 0x0000A000 +#define FLASH_SECTOR_11 0x0000B000 +#define FLASH_SECTOR_12 0x0000C000 +#define FLASH_SECTOR_13 0x0000D000 +#define FLASH_SECTOR_14 0x0000E000 +#define FLASH_SECTOR_15 0x0000F000 +#define FLASH_SECTOR_SIZE (4 * 1024) + +static char * sector_start_adress[] = { + (char *)FLASH_SECTOR_0, + (char *)FLASH_SECTOR_1, + (char *)FLASH_SECTOR_2, + (char *)FLASH_SECTOR_3, + (char *)FLASH_SECTOR_4, + (char *)FLASH_SECTOR_5, + (char *)FLASH_SECTOR_6, + (char *)FLASH_SECTOR_7, + (char *)FLASH_SECTOR_8, + (char *)FLASH_SECTOR_9, + (char *)FLASH_SECTOR_10, + (char *)FLASH_SECTOR_11, + (char *)FLASH_SECTOR_12, + (char *)FLASH_SECTOR_13, + (char *)FLASH_SECTOR_14, + (char *)FLASH_SECTOR_15 +}; + #elif defined(TARGET_LPC11U24) || defined(TARGET_LPC1114) #define USER_FLASH_AREA_START FLASH_SECTOR_7 @@ -269,7 +321,7 @@ char *reserved_flash_area_start( void ); int reserved_flash_area_size( void ); -#if defined(TARGET_LPC11U24) +#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) int write_eeprom( char *source_addr, char *target_addr, int size ); int read_eeprom( char *source_addr, char *target_addr, int size ); #endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Limiter/Limiter.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -0,0 +1,26 @@ +#include "mbed.h" +#include "Limiter.h" + +Limiter::Limiter(float LIMIT) +{ + limit = LIMIT; + last= 0; + skip = false; +} + +float Limiter::calc(float now) +{ + if ( !skip ) { + skip = true; + return last = now; + } + float differential = ( now - last ); + if ( differential < limit && differential > -limit ) last = now; + if ( differential > limit ) last += limit * 0.05f; + else { + if ( differential < -limit ) last -= limit * 0.05f; + else last = now; + } + return last; +} +;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Limiter/Limiter.h Tue Oct 14 08:15:03 2014 +0000 @@ -0,0 +1,17 @@ +#ifndef MBED_LIMITER_H +#define MBED_LIMITER_H + +#include "mbed.h" +#include "stdarg.h" + +class Limiter +{ +public: + Limiter(float limit); + float calc(float now); +private: + float last; + float limit; + bool skip; +}; +#endif \ No newline at end of file
--- a/PID/PID.cpp Thu Feb 13 16:07:07 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,56 +0,0 @@ -#include "mbed.h" -#include "SerialLcd.h" -#include "PID.h" - -PID::PID() -{ - interval = 0.05; - differential_limit = 200; - pid_limit = 300; - kp = 0.5; - ki = 0.5; - kd = 0.2; - integral = 0; - old = 0; -} - -void PID::init(float KP,float KI,float KD,float PID_LIM,float DIFFERENTIAL_LIM) -{ -// interval = INTERVAL; - pid_limit = PID_LIM; - differential_limit = DIFFERENTIAL_LIM; - kp = KP; - ki = KI; - kd = KD; - integral = 0; - old = 0; -} - -float PID::calc(float value,float target,float interval) -{ - float differential; - cur = value - target; -// if ( (old >= 0 && cur <= 0) || (old <= 0 && cur >= 0) || fabsf(cur) < 1 ) -// integral = 0; - integral += ( cur + old ) * interval * 0.5f; -// if ( integral < -differential_limit ) integral = -differential_limit; -// if ( integral > integral_limit ) integral = integral_limit; - differential = ( kd * ( cur - old ) / interval ); - if ( differential < -differential_limit ) differential = -differential_limit; - if ( differential > differential_limit ) differential = differential_limit; - float pid = ( kp * cur ) + ( ki * integral ) + differential; - if ( pid < -pid_limit ) pid = -pid_limit; - if ( pid > pid_limit ) pid = pid_limit; - old = cur; - return pid; -} - -void PID::reset() -{ - integral = 0; - old = 0; -} -; - - -
--- a/PID/PID.h Thu Feb 13 16:07:07 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,22 +0,0 @@ -#ifndef MBED_PID_H -#define MBED_PID_H - -#include "mbed.h" -#include "stdarg.h" - -class PID -{ -public: - PID(); - void init(float KP,float KI,float KD,float PID_LIM,float INTGRAL_LIM); - float calc(float value,float target,float interval); - void reset(); -private: - float integral; - float interval; - float cur,old; - float kp,ki,kd; - float differential_limit; - float pid_limit; -}; -#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PulseOut/PulseOut.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -0,0 +1,45 @@ +#include "mbed.h" +#include "InterruptIn.h" +#include "PulseOut.h" + +PulseOut::PulseOut(PinName _outpin,bool _positive) : pulse(_outpin) //constructa +{ + if ( _positive ) + pulse = 0; + else + pulse = 1; + positive = _positive; + width = 0; +} + +void PulseOut::pulsewidth_us(int _width) +{ + width = _width; +} + +void PulseOut::start() +{ + if ( width == 0 ) return; + _timeout.attach_us(this,&PulseOut::stop,width); + if ( positive ) + pulse = 1; + else + pulse = 0; +} + +void PulseOut::period_us(int period) +{ + //dummy +} + +void PulseOut::stop() +{ + if ( positive ) + pulse = 0; + else + pulse = 1; + _timeout.detach(); +} +; + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PulseOut/PulseOut.h Tue Oct 14 08:15:03 2014 +0000 @@ -0,0 +1,23 @@ +#ifndef PULSEOUT_H +#define PULSEOUT_H +#define POSITIVE true +#define NEGATIVE false + +#include "mbed.h" + +class PulseOut +{ +private: + Timeout _timeout; + void stop(); + DigitalOut pulse; + bool positive; + int width; + +public: + PulseOut(PinName,bool mode=true); + void pulsewidth_us(int); + void period_us(int); + void start(); +}; +#endif \ No newline at end of file
--- a/SerialLcd/SerialLcd.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/SerialLcd/SerialLcd.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -1,7 +1,7 @@ #include "mbed.h" #include "SerialLcd.h" -SerialLcd::SerialLcd(PinName TX): _lcd(TX,NC) +SerialLcd::SerialLcd(PinName TX,PinName RX): _lcd(TX,RX) { // LCD_contrast = 60; } @@ -28,4 +28,4 @@ _lcd.putc( 0x80 + (row * 0x40) + clm ); wait(0.01); } -; \ No newline at end of file +;
--- a/SerialLcd/SerialLcd.h Thu Feb 13 16:07:07 2014 +0000 +++ b/SerialLcd/SerialLcd.h Tue Oct 14 08:15:03 2014 +0000 @@ -7,7 +7,7 @@ class SerialLcd : public Stream { public: - SerialLcd(PinName); + SerialLcd(PinName,PinName=NC); void cls(); void locate(int,int);
--- a/SoftPWM/SoftPWM.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/SoftPWM/SoftPWM.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -16,16 +16,16 @@ float SoftPWM::read() { - if ( width <= 0.0 ) return 0.0; - if ( width > 1.0 ) return 1.0; + if ( width <= 0.0f ) return 0.0; + if ( width > 1.0f ) return 1.0; return width / interval; } void SoftPWM::write(float duty) { width = interval * duty; - if ( duty <= 0.0 ) width = 0.0; - if ( duty > 1.0 ) width = interval; + if ( duty <= 0.0f ) width = 0.0; + if ( duty > 1.0f ) width = interval; } void SoftPWM::start() @@ -64,7 +64,7 @@ void SoftPWM::pulsewidth(float _width) { width = _width; - if ( width < 0.0 ) width = 0.0; + if ( width < 0.0f ) width = 0.0; } void SoftPWM::pulsewidth_ms(int _width)
--- a/SoftPWM/SoftPWM.h Thu Feb 13 16:07:07 2014 +0000 +++ b/SoftPWM/SoftPWM.h Tue Oct 14 08:15:03 2014 +0000 @@ -30,14 +30,14 @@ void period_us(int); void stop(); operator float() { - if ( width <= 0.0 ) return 0.0; - if ( width > 1.0 ) return 1.0; + if ( width <= 0.0f ) return 0.0f; + if ( width > 1.0f ) return 1.0f; return width / interval; } SoftPWM& operator=(float duty) { width = interval * duty; - if ( duty <= 0.0 ) width = 0.0; - if ( duty > 1.0 ) width = interval; + if ( duty <= 0.0f ) width = 0.0f; + if ( duty > 1.0f ) width = interval; return *this; }
--- a/config.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/config.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -4,22 +4,27 @@ #include "config.h" #include "PulseWidthCounter.h" #include "SerialLcd.h" -#include "PID.h" +//#include "PID.h" //Serial pc(USBTX, USBRX); -#define CALIBURATE 10 -#define GYROGAIN 20 -#define GYRODIR 30 -#define ACCELCORRECT 40 -#define PIDSET 50 -#define STICKMIX 60 -#define DISPPULSE 70 -#define DISPSENSOR 80 -#define DISPPWM 90 -#define PARMSET 100 -#define CONFSTORE 110 -#define CONFRESET 120 +enum DispNum { CALIBURATE=1, + GYROGAIN, + GYRODIR, + SERVODIR, +// ACCELGAIN, + ACCELCORRECT, +// PIDSET, +// PIDHEIGHT, +// GIMBAL, + STICKMIX, + DISPPULSE, + DISPSENSOR, + DISPPWM, + PARMSET, + CONFSTORE, + CONFRESET, + FINAL }; void FlashLED(int); char Check_Stick_Dir(char); @@ -42,23 +47,27 @@ Timer elaps; -extern volatile int CH[5]; +extern volatile int CH[9]; extern volatile int M[6]; extern volatile float Gyro[3]; extern volatile float Accel[3]; +extern volatile float Accel_Save[3]; +extern volatile float Accel_Angle[3]; extern volatile float Angle[3]; extern volatile float Gyro_Ref[3]; -extern volatile int Stick[5]; +extern volatile int Stick[6]; extern volatile float Press; extern volatile float interval; //extern bool tick_flag; -extern PID pid[3]; -extern int pid_reg[3]; +//extern PID pid[4]; +//extern PID height; +//extern int pid_reg[4]; const char steering[3][6]= {"Roll ","Pitch","Yaw "}; -short mode; +const char ModelName[5][9] = { "Quad-X ","Quad-H ","Delta ","Delta-TW","AirPlane" }; +int mode;// char sw,ret_mode; -short vnum,hnum,vmax,hmax; -short idx,i; +int vnum,hnum,vmax,hmax;// +int idx,i;// char str[33]; config init; @@ -69,7 +78,7 @@ mode = 0; vnum = 0; hnum = 0; - vmax = 12; + vmax = FINAL - 1; while( 1 ) { // FlashLED(1); @@ -77,20 +86,26 @@ mode = vnum * 10 + hnum; switch ( mode ) { + + //初期画面 case 0: LCD_locate(0,0); - sprintf(str,"Quad-X Ver %4.2f",conf.Revision); + LCD_printf( (char*)ModelName[conf.Model_Type] ); + LCD_locate(8,0); + sprintf(str,"Ver %4.2f",conf.Revision); LCD_printf(str); LCD_locate(4,1); LCD_printf("By AZUKITEN"); hmax = 0; break; - case CALIBURATE: //Calibrate Transmitter + + //送信機信号のキャリブレーション + case CALIBURATE*10: //Calibrate Transmitter LCD_printf("Calibrate"); Set_Arrow(1); hmax = 1; break; - case CALIBURATE+1: //Calibrate Transmitter + case CALIBURATE*10+1: //Calibrate Transmitter LCD_printf("Start Calibrate"); wait(1); for(i=0; i<4; i++) { @@ -108,6 +123,7 @@ for(i=0; i<4; i++) { conf.Stick_Ref[i] = conf.Stick_Ref[i]/16; } + CalibrateGyros(); CalibrateAccel(); LCD_cls(); //Clear LCD @@ -116,30 +132,32 @@ FlashLED(5); hnum = 0; break; - case GYROGAIN: //Set Gyro Gain + + //ジャイロ感度の設定 + case GYROGAIN*10: //Set Gyro Gain LCD_printf("Set Gyro Gain"); Set_Arrow(1); hmax = 4; break; - case GYROGAIN+1: //Set Gyro Gain Roll + case GYROGAIN*10+1: //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 GYROGAIN+2: + case GYROGAIN*10+2: 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 GYROGAIN+3: + case GYROGAIN*10+3: 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 GYROGAIN+4: + case GYROGAIN*10+4: // ret_mode = 'R'; LCD_printf("GyroGain>setting"); LCD_locate(0,1); @@ -154,18 +172,20 @@ LCD_printf("Transmitter"); Set_Arrow(3); break; - case GYRODIR: //Set Gyro Direction + + //ジャイロの効きの逆転 + case GYRODIR*10: //Set Gyro Direction LCD_printf("Gyro Direction"); Set_Arrow(1); hmax = 4; break; - case GYRODIR+1: //Set Gyro Direction Roll - case GYRODIR+2: - case GYRODIR+3: - case GYRODIR+4: + case GYRODIR*10+1: //Set Gyro Direction Roll + case GYRODIR*10+2: + case GYRODIR*10+3: + case GYRODIR*10+4: //xy軸の入れ替え // ret_mode = 'R'; - idx = mode - (GYRODIR+1); - if ( mode == (GYRODIR+4) ) + idx = mode - (GYRODIR*10+1); + if ( mode == (GYRODIR*10+4) ) LCD_printf("Gyro>Swap X-Y"); else { LCD_printf("Gyro>Dir>"); @@ -182,82 +202,86 @@ LCD_printf("Normal "); else LCD_printf("Reverse"); - if ( mode == (GYRODIR+4) ) + if ( mode == (GYRODIR*10+4) ) + Set_Arrow(3); + else + Set_Arrow(2); + break; + + //サーボの向きの逆転 + case SERVODIR*10: //Set Servo Direction + LCD_printf("Servo Direction"); + Set_Arrow(1); + hmax = 6; + break; + case SERVODIR*10+1: //Set Gyro Direction Roll + case SERVODIR*10+2: + case SERVODIR*10+3: + case SERVODIR*10+4: + case SERVODIR*10+5: + case SERVODIR*10+6: +// ret_mode = 'R'; + idx = mode - (SERVODIR*10+1); + sprintf(str,"Servo>Dir>M%d",idx+1); + LCD_printf(str); + LCD_locate(0,1); + switch ( sw ) { + case 'U': + case 'D': + conf.Servo_Dir[idx] *= -1; + } + if ( conf.Servo_Dir[idx] == 1 ) + LCD_printf("Normal "); + else + LCD_printf("Reverse"); + if ( mode == (SERVODIR*10+4) ) Set_Arrow(3); else Set_Arrow(2); break; - case ACCELCORRECT: + + //加速度計の水平レベルの校正 + case ACCELCORRECT*10: LCD_printf("Acceleration"); LCD_locate(2,1); - LCD_printf("correction"); + LCD_printf("Trim"); Set_Arrow(1); hmax = 3; break; - case ACCELCORRECT+1: + case ACCELCORRECT*10+1: Param_Set_Prompt1("Accel>Rol",&conf.Accel_Ref[ROL],2,-10.0,10.0f,0.001f,sw); break; - case ACCELCORRECT+2: + case ACCELCORRECT*10+2: Param_Set_Prompt1("Accel>Pitch",&conf.Accel_Ref[PIT],2,-10.0,10.0f,0.001f,sw); break; - case ACCELCORRECT+3: + case ACCELCORRECT*10+3: Param_Set_Prompt1("Accel>Yaw",&conf.Accel_Ref[YAW],3,-10.0,10.0f,0.001f,sw); break; - case PIDSET: //PID Setting - LCD_printf("PID Setting"); - Set_Arrow(1); - hmax = 9; - break; - case PIDSET+1: - Param_Set_Prompt1("PID>RoolPitch>kp",&conf.kp[0],2,0.00f,15.00f,0.01f,sw); - conf.kp[1] = conf.kp[0]; - break; - case PIDSET+2: - Param_Set_Prompt1("PID>RoolPitch>ki",&conf.ki[0],2,0.00f,5.00f,0.01f,sw); - conf.ki[1] = conf.ki[0]; - break; - case PIDSET+3: - Param_Set_Prompt1("PID>RoolPitch>kd",&conf.kd[0],2,0.00f,5.00f,0.01f,sw); - conf.kd[1] = conf.kd[0]; - break; - case PIDSET+4: - Param_Set_Prompt1("PID>YAW>kp",&conf.kp[2],2,0.00f,15.00f,0.01f,sw); - break; - case PIDSET+5: - Param_Set_Prompt1("PID>YAW>ki",&conf.ki[2],2,0.00f,5.00f,0.01f,sw); - break; - case PIDSET+6: - Param_Set_Prompt1("PID>YAW>kd",&conf.kd[2],2,0.00f,5.00f,0.01f,sw); - break; - case PIDSET+7: - Param_Set_Prompt1("PID Interval",&conf.PID_Interval,2,0.002f,0.015f,0.001f,sw); - break; - case PIDSET+8: - Param_Set_Prompt1("PID Limit",&conf.PID_Limit,2,0.00f,400.00f,10.00f,sw); - break; - case PIDSET+9: - Param_Set_Prompt1("differential Lim",&conf.Differential_Limit,3,0.00f,300.00f,10.00f,sw); - break; - case STICKMIX: //Set Stick Mixing + + //スティック操作量の設定 + case STICKMIX*10: //Set Stick Mixing LCD_printf("Set Stick Mixing"); Set_Arrow(1); hmax = 3; break; - case STICKMIX+1: //Set Stick Mixing + case STICKMIX*10+1: //Set Stick Mixing Param_Set_Prompt1("Mixing>Roll",&conf.Stick_Mix[0],2,0.00f,2.00f,0.01f,sw); break; - case STICKMIX+2: + case STICKMIX*10+2: Param_Set_Prompt1("Mixing>Pitch",&conf.Stick_Mix[1],2,0.00f,2.00f,0.01f,sw); break; - case STICKMIX+3: + case STICKMIX*10+3: Param_Set_Prompt1("Mixing>Yaw",&conf.Stick_Mix[2],3,0.00f,2.00f,0.01f,sw); break; - case DISPPULSE: //Display Pulse Width + + //送信機パルス長の表示 + case DISPPULSE*10: //Display Pulse Width LCD_printf("Disp Pulse Width"); Set_Arrow(1); - hmax = 2; + hmax = 3; + x = conf.ESC_Low; break; - case DISPPULSE+1: //Display Pulse Width + case DISPPULSE*10+1: //Display Pulse Width // DisplayPulseWidth(THR,AIL,ELE,RUD,AUX); ret_mode = 'R'; LCD_locate(0,0); @@ -267,7 +291,17 @@ sprintf(str,"EL=%4d,RD=%4d",ELE,RUD); LCD_printf(str); break; - case DISPPULSE+2: //Display Stick Ref + case DISPPULSE*10+2: //Display AUX,AX2 + ret_mode = 'R'; + Get_Stick_Pos(); + LCD_locate(0,0); + sprintf(str,"A1=%4d,A2=%4d",AUX,AX2); + LCD_printf(str); + LCD_locate(0,1); + sprintf(str,"A3=%4d,A4=%4d",AX3,AX4); + LCD_printf(str); + break; + case DISPPULSE*10+3: //Display Stick Ref ret_mode = 'R'; Get_Stick_Pos(); LCD_locate(0,0); @@ -277,17 +311,16 @@ sprintf(str,"EL=%4d,RD=%4d",Stick[PIT],Stick[YAW]); LCD_printf(str); break; - case DISPSENSOR: //Display Sensor Value - LCD_printf("Disp Senser"); + + //センサー値の表示 + case DISPSENSOR*10: //Display Sensor Value + LCD_printf("Disp Sensor"); Set_Arrow(1); - hmax = 7; - for ( i=0; i<3; i++ ) { - pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i]*(float)abs(Stick[GAIN])/50.0 - ,conf.PID_Limit,conf.Differential_Limit); - Angle[i] = 0; - } + hmax = 6; + Angle[ROL]=Angle[PIT]=Angle[YAW]=0; + Accel[ROL]=Accel[PIT]=Accel[YAW]=0; break; - case DISPSENSOR+1: //Gyro + case DISPSENSOR*10+1: //Gyro // Get_Gyro(); if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z); else i2c.angular(&y,&x,&z); @@ -302,15 +335,14 @@ LCD_printf(str); ret_mode = 'R'; break; - case DISPSENSOR+2: //Accelerometer -// Get_Accel(); + case DISPSENSOR*10+2: //Gravity if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z); else i2c.Acceleration(&y,&x,&z); x -= conf.Accel_Ref[0]; y -= conf.Accel_Ref[1]; z -= conf.Accel_Ref[2]; LCD_locate(0,0); - sprintf(str,"[Accel]X=%5.2f",x); + sprintf(str,"[Gravity]X=%5.2f",x); LCD_printf(str); LCD_locate(0,1); sprintf(str,"Y=%5.2f,Z=%5.2f",y,z); @@ -318,7 +350,7 @@ // Set_Arrow(2); ret_mode = 'R'; break; - case DISPSENSOR+3: //angle + case DISPSENSOR*10+3: //angle PWM_Out(false); LCD_locate(0,0); sprintf(str,"[Angle]X=%6.1f",Angle[ROL]); @@ -329,33 +361,22 @@ // Set_Arrow(2); ret_mode = 'R'; break; - case DISPSENSOR+4: - PWM_Out(false); - LCD_locate(0,0); - sprintf(str,"t=%5d,a=%6.2f",-Stick[YAW]*45/400,Angle[YAW]); - LCD_printf(str); - LCD_locate(0,1); - sprintf(str,"pid=%5d",pid_reg[YAW]); - LCD_printf(str); - Set_Arrow(2); - ret_mode = 'R'; - break; - case DISPSENSOR+5: // Pressure + case DISPSENSOR*10+4: // Pressure elaps.reset(); elaps.start(); Get_Pressure(); elaps.stop(); LCD_locate(0,0); - sprintf(str,"Press=%9.1f",Press/4096); + sprintf(str,"Press=%4.1fhp",Press); LCD_printf(str); LCD_locate(0,1); - sprintf(str,"Elaps=%6d",elaps.read_us()); + sprintf(str,"Height=%7.2fcm",i2c.height_cm()); LCD_printf(str); // Set_Arrow(2); ret_mode = 'R'; wait(0.05); break; - case DISPSENSOR+6: + case DISPSENSOR*10+5: elaps.reset(); elaps.start(); PWM_Out(false); @@ -364,39 +385,53 @@ LCD_locate(0,0); sprintf(str,"ElapsTime=%6d",i); LCD_printf(str); - Set_Arrow(2); +// Set_Arrow(2); ret_mode = 'R'; break; - case DISPSENSOR+7: //Sensor Calibration + case DISPSENSOR*10+6: //Sensor Calibration CalibrateGyros(); FlashLED(3); LCD_printf("Calibrate completed"); Set_Arrow(3); break; - case DISPPWM: //Display PWM Condition + + + //ESC用PWMパルス長の表示 + case DISPPWM*10: //Display PWM Condition LCD_printf("Display PWM "); Set_Arrow(1); - hmax = 1; + hmax = 2; break; - case DISPPWM+1: //Display PWM Width + case DISPPWM*10+1: //Display PWM Width ret_mode = 'R'; PWM_Out(false); + i = conf.ESC_Low; LCD_locate(0,0); - sprintf(str,"M1=%4d,M2=%4d",M1,M2); + sprintf(str,"M1=%4d,M2=%4d",i+M1,i+M2); LCD_printf(str); LCD_locate(0,1); - sprintf(str,"M4=%4d,M3=%4d",M4,M3); + sprintf(str,"M4=%4d,M3=%4d",i+M4,i+M3); LCD_printf(str); break; - case PARMSET: //パラメーター設定 + case DISPPWM*10+2: //Display PWM Width + ret_mode = 'R'; + PWM_Out(false); + i = conf.ESC_Low; + LCD_locate(0,0); + sprintf(str,"M5=%4d,M6=%4d",i+M5,i+M6); + LCD_printf(str); + break; + + //その他パラメータ値の設定 + case PARMSET*10: //パラメーター設定 LCD_printf("Parameter Set"); Set_Arrow(1); - hmax = 6; + hmax = 8; break; - case PARMSET+1: + case PARMSET*10+1: Param_Set_Prompt1("LCD>Contrast",&conf.LCD_Contrast,2,0,63,1,sw); break; - case PARMSET+2: + case PARMSET*10+2: LCD_locate(0,0); LCD_printf("PWM>Mode"); LCD_locate(0,1); @@ -411,27 +446,45 @@ LCD_printf("Moter"); Set_Arrow(2); break; - case PARMSET+3: - Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,10000,10,sw); + case PARMSET*10+3: + Param_Set_Prompt1("PWM>Interval",&conf.PWM_Interval,2,Thro_Hi,20000,100,sw); break; - case PARMSET+4: + case PARMSET*10+4: Param_Set_Prompt1("Gyro>CutoffFreq",&conf.Cutoff_Freq,2,0.00f,10.0f,0.01f,sw); break; - case PARMSET+5: + case PARMSET*10+5: Param_Set_Prompt1("ESC>Low Position",&conf.ESC_Low,2,Pulse_Min,Pulse_Max,1,sw); break; - case PARMSET+6: + case PARMSET*10+6: Param_Set_Prompt1("Flight Timer",&conf.Flight_Time,2,0,600,10,sw); break; -// case PARMSET+7: -// Param_Set_Prompt1("Cont. swap Angle",&conf.Control_Exchange_Angle,3,0.0f,40.0f,1.0f,sw); -// break; - case CONFSTORE: //E2PROM Store + case PARMSET*10+7: + LCD_locate(0,0); + LCD_printf("Model Type"); + LCD_locate(0,1); + switch ( sw ) { + case 'D': + if ( conf.Model_Type > 0 ) conf.Model_Type -= 1; + else conf.Model_Type = 3; + break; + case 'U': + if ( conf.Model_Type < 4 ) conf.Model_Type += 1; + else conf.Model_Type = 0; + } + LCD_printf( (char*)ModelName[conf.Model_Type] ); + Set_Arrow(2); + break; + case PARMSET*10+8: + Param_Set_Prompt1("Active Gyro Gain",&conf.Active_Jyro_Gain,3,0.0f,1.0f,0.01f,sw); + break; + + //設定データの保存 + case CONFSTORE*10: //E2PROM Store LCD_printf("Config Save"); Set_Arrow(1); hmax = 1; break; - case CONFSTORE+1: + case CONFSTORE*10+1: WriteConfig(); LCD_locate(0,0); sprintf(str,"Config %3dbyte",sizeof(config)); @@ -443,18 +496,20 @@ FlashLED(5); hnum = 0; break; - case CONFRESET: //E2PROM reset + + //設定データの初期化 + case CONFRESET*10: //E2PROM reset LCD_printf("Config Reset"); Set_Arrow(1); hmax = 3; break; - case CONFRESET+1: + case CONFRESET*10+1: LCD_printf("Ailron stick"); LCD_locate(0,1); LCD_printf("Move to right"); Set_Arrow(2); break; - case CONFRESET+2: //E2PROM reset + case CONFRESET*10+2: //E2PROM reset conf = init; LCD_printf("Rset sucssesuful"); Set_Arrow(3); @@ -620,52 +675,4 @@ case 3: LCD_printf(" <<"); } -} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +};
--- a/config.h Thu Feb 13 16:07:07 2014 +0000 +++ b/config.h Tue Oct 14 08:15:03 2014 +0000 @@ -24,27 +24,42 @@ #define ELE CH[2] #define RUD CH[3] #define AUX CH[4] +#define AX2 CH[5] +#define AX3 CH[6] +#define AX4 CH[7] +#define AX5 CH[8] +#define HEIGHT 3 #define _ITG3200 0x00 #define _L3GD20 0x01 #define TICK_TIME 0.05 #define GYRO_ADJUST 2 -//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}; - +enum PlaneType{Quad_X=0,Quad_H,Delta,Delta_TW,AirPlane}; +enum JR{_THR=0,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4}; //JR 信号の順番 +//enum Futaba{_AIL=0,_ELE,_THR,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4}; //Futaba 信号の順番 +enum stknum{ROL=0,PIT,YAW,COL,GAIN,AUX2}; // Stick[]の順番 +/* +struct pidinf { + float kp; + float ki; + float kd; + float limit; + float integral_limit; + float differential_limit; +};*/ struct config { float Revision; int Struct_Size; char StartMode; //'c':config mode 'f':flight mode + char Model_Type; // 0x00:Quad-X 0x01:Quad-H-3D + signed char Gyro_Gain_Setting; 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]; + signed char Servo_Dir[6]; float Accel_Ref[3]; - float Accel_Gain[3]; - int Gyro_Gain_Setting; +// float Accel_Gain[3]; float PID_Interval; float Cutoff_Freq; int Flight_Time; @@ -52,71 +67,54 @@ int PWM_Mode; int ESC_Low; int PWM_Interval; -// int Accel_Rang; -// int Accel_Rate; -// int PWM_Resolution; - char Angle_Control; - float kp[3]; - float ki[3]; - float kd[3]; - float PID_Limit; - float Differential_Limit; +// pidinf pid[4]; + float Active_Jyro_Gain; +// float Gimbal_Gain; +// int Gimbal_Neutral_Width; +// int Gimbal_Dir; public: config() { - Revision = 1.30; + Revision = 2.10; 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.3; - Stick_Mix[1] = 0.3; + Stick_Mix[0] = 0.4; + Stick_Mix[1] = 0.4; Stick_Mix[2] = 0.65; 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[0] = 0.50; + Gyro_Gain[1] = 0.50; + Gyro_Gain[2] = 0.50; 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; + Servo_Dir[0] = 1; + Servo_Dir[1] = 1; + Servo_Dir[2] = 1; + Servo_Dir[3] = 1; Accel_Ref[0] = 0; Accel_Ref[1] = 0; Accel_Ref[2] = 0; - Accel_Gain[0] = 0.50; - Accel_Gain[1] = 0.50; - Accel_Gain[2] = 0.50; +// Accel_Gain[0] = 0.50; +// Accel_Gain[1] = 0.50; +// Accel_Gain[2] = 0.50; + Model_Type = 0; Gyro_Gain_Setting = -1; Cutoff_Freq=0.15; Flight_Time=360; LCD_Contrast = 60; PWM_Mode = 1; ESC_Low= 1080; - PWM_Interval = 2200; -// Accel_Rang = 2; -// Accel_Rate = 14; -// PWM_Resolution = 0; + PWM_Interval = 5000; StartMode = 'C'; - Angle_Control = 'H'; // H:horizn A:angle - kp[0] = 1.5; - kp[1] = 1.5; - kp[2] = 1.5; - ki[0] = 0.0; - ki[1] = 0.0; - ki[2] = 0.0; - kd[0] = 0.8; - kd[1] = 0.8; - kd[2] = 0.8; - PID_Limit = 350; - Differential_Limit = 200; - PID_Interval = 0.003; + PID_Interval = 0.005; + Active_Jyro_Gain = 0.5; } }; #endif @@ -160,3 +158,14 @@ + + + + + + + + + + +
--- a/main.cpp Thu Feb 13 16:07:07 2014 +0000 +++ b/main.cpp Tue Oct 14 08:15:03 2014 +0000 @@ -1,3 +1,17 @@ +/* PWM Output + type | M1 | M2 | M3 | M4 | M5 | M6 + ------------+-----------+-----------+-----------+-----------+--------------------- + Quad-X |FL Thr ESC |FR Thr ESC |BL Thr ESC |BR Thr ESC | - | - + Quad-H |FL VP Ser |FR VP Ser |BL VP Ser |BR VP Ser |Thr ESC | - + Delta |FL Tro ESC |L Ale Ser |R Ale Ser |Rud Ser | - | - + Delta-TW |FL Tro ESC |L Ale Ser |R Ale Ser |Rud Ser |FR Thr ESC | - + Airplane |Thr ESC |Ail Ser |Ele Ser |Rud Ser |Ail Ser | - + + Ele:Elevator, Rud:Rudder, Ail:Aileron, Thr:Throttle, Ale:Alevon + ESC:for ESC, Ser:for Servo, VP:Variable Pitch + F:Front, B:Back, L:Light, R:Right +*/ + #include "mbed.h" #include "math.h" #include "I2cPeripherals.h" @@ -6,107 +20,127 @@ #include "PulseWidthCounter.h" #include "string" #include "SerialLcd.h" -#include "IAP.h" -#include "PID.h" +//#include "PID.h" #include "SoftPWM.h" +#include "PulseOut.h" +#include "Limiter.h" +#ifndef TARGET_NUCLEO_F401RE + #include "IAP.h" +#endif //Serial pc(USBTX, USBRX); -//Gravity at Earth's surface in m/s/s -#define g0 9.812865328 -//Convert from radians to degrees. -#define toDegrees(x) (x * 57.2957795) -//Convert from degrees to radians. -#define toRadians(x) (x * 0.01745329252) -//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). -#define GYROSCOPE_GAIN (1 / 14.375) -//Full scale resolution on the ADXL345 is 4mg/LSB. -#define ACCELEROMETER_GAIN (0.004 * g0) -//Updating filter at 40Hz. -#define FILTER_RATE 0.05 -//At rest the gyroscope is centred around 0 and goes between about -//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly -//5/15 = 0.3 degrees/sec. +#if defined(TARGET_LPC1768) + DigitalInOut pwmpin[] = { p21,p22,p23,p24 }; +// #ifdef LPCXpresso +// #define LED1 P0_22 +// #endif + DigitalOut led1(LED1); +// DigitalOut led2(LED2); + InterruptIn ch1(p5); + PulseWidthCounter ch[6] = { p6,p7,p8,p9,p10,p11 }; + PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 }; +// SoftPWM pwm[6] = { p21,p22,p23,p24,p25,p26 }; + SoftPWM buzz(p20); + I2cPeripherals i2c(p28,p27); //sda scl + SerialLcd lcd(p13,p14); + #define MEM_SIZE 256 + #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768 + IAP iap; +#elif defined(TARGET_NUCLEO_F401RE) + DigitalOut led1(LED1); + InterruptIn ch1(PC_2); +// PulseWidthCounter ch[6] = { PA_0,PA_1,PA_4,PB_0,PC_1,PC_0 }; + PulseWidthCounter ch[6] = { A0,A1,A2,A3,A4,A5 }; + PwmOut pwm[6] = { D8,D9,D10,D11,D12,D14 }; +// PwmOut pwm[6] = { D2,D3,D4,D5,D6,D7 }; +// SoftPWM pwm[6] = { PB_3,PB_4,PB_10,PC_6,PB_6,PA_7 }; + SoftPWM buzz(PB_13); +// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl + I2cPeripherals i2c(D5,D7); //sda scl + SerialLcd lcd(PA_11,PA_12); + #define MEM_SIZE 256 + #define EXTERNAL_EEPROM //24AAXX/24LCXX/24FCXX EEPROM +#elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) + DigitalInOut pwmpin[] = { P0_14,P0_2,P0_23,P0_17 }; + DigitalOut led1(P0_21); +// DigitalOut led2(P0_21); + InterruptIn ch1(P0_9); + PulseWidthCounter ch[5] = { P0_8,P0_10,P0_7,P0_22,P1_15 }; +// SoftPWM pwm[4] = { P0_14,P0_2,P0_23,P0_17 }; + PulseOut pwm[6] = { P0_14,P0_2,P0_23,P0_17,p0_20,p015 }; + Ticker Tpwm; + #define SOFTPWM + SoftPWM buzz(P1_19); + I2cPeripherals i2c(P0_5,P0_4); //sda scl + SerialLcd lcd(P0_19,P0_18); + #define MEM_SIZE 256 + #define TARGET_EEPROM_ADDRESS 64 +// #define EXTERNAL_EEPROM + #define INTERNAL_EEPROM + IAP iap; +#elif defined(TARGET_LPC1114) // LPC1114 + DigitalInOut pwmpin[] = { dp1,dp2,dp18,dp24 }; + DigitalOut led1(dp28); + InterruptIn ch1(dp4); + PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 }; +// SoftPWM pwm[4] = { dp1,dp2,dp18,dp24 }; + PulseOut pwm[6] = { dp1,dp2,dp18,dp24,dp17,dp6 }; + Ticker Tpwm; + #define SOFTPWM + SoftPWM buzz(dp25); + I2cPeripherals i2c(dp5,dp27); //sda scl + SerialLcd lcd(dp16,dp15); + #define MEM_SIZE 256 + #define EXTERNAL_EEPROM +#endif -#ifdef TARGET_LPC1114 // LPC1114 -// PulseOut pwm[4] = { dp1,dp2,dp18,dp24 ); - #define LED1 dp28 - #define p5 dp4 - #define p6 dp9 - #define p7 dp10 - #define p8 dp11 - #define p9 dp13 - #define p10 dp26 - #define p13 dp16 - #define p21 dp1 - #define p22 dp2 - #define p23 dp18 - #define p24 dp24 - #define p26 dp25 - #define p27 dp27 - #define p28 dp5 -#else //LPC1768 - #ifdef LPCXpresso - #define LED1 P0_22 - #endif -#endif -DigitalInOut pwmpin[] = { p21,p22,p23,p24 }; -DigitalOut led1(LED1); -DigitalOut led2(LED2); -InterruptIn ch1(p5); -PulseWidthCounter ch[5] = { p6,p7,p8,p9,p10 }; -#if defined(SOFT_PWM) || defined(TARGET_LPC1114) -SoftPWM pwm[4] = { p21,p22,p23,p24 }; -#else -PwmOut pwm[4] = { p21,p22,p23,p24 }; -#endif -SoftPWM buzz(p26); Timer CurTime; //Timer ElapTime; Timer CycleTime; Timer FlyghtTime; -//Ticker tick; -//Ticker tick100ms; -//Ticker mixTime; -I2cPeripherals i2c(p28,p27); //sda scl -SerialLcd lcd(p13); config conf; -PID pid[3]; -#ifdef TARGET_LPC1114 -//LPC1114 Flash Memory read/write - #define MEM_SIZE 256 - #define TARGET_SECTOR 7 // use sector 29 as target sector if it is on LPC1768 -#else -//LPC1768 Flash Memory read/write - #define MEM_SIZE 256 - #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768 -#endif -#ifndef LocalFileOut -IAP iap; -#endif +//PID pid[4]; +Limiter gyroLimit[3] = {300,300,300}; +Limiter accLimit[3] = {0.5,0.5,0.5}; +Limiter pwmLimit[4] = {50,50,50,50}; +//PID height; float TotalTime = 0;; int channel = 0; -//int intrupt_cnt = 0; -//int intrupt_cnt2 = 0; -int volatile CH[5]; +int Signal[9] = { _THR,_AIL,_ELE,_RUD,_GYRO,_AUX1,_AUX2,_AUX3,_AUX4 }; +volatile int CH[9]; volatile int M[6]; volatile float Gyro[3]; -volatile float Accel[3]; -volatile float Accel_Save[3]; +volatile float Accel[3]= {0,0,0}; +volatile float Accel_Angle[3]; +volatile float Accel_Save[3]= {0,0,0}; volatile float Angle[3]; volatile float Gyro_Ref[3]; volatile float Gyro_Save[3]; -volatile int Stick[5]; -volatile int Stick_Save[3]; +volatile int Stick[6]; +volatile int Stick_Save[6]; +int PWM_Init[5][6] = { 1080,1080,1080,1080,1080,1080, //Quad_X + 1500,1500,1500,1500,1080,1080, //Quad_H + 1080,1500,1500,1500,1500,1080, //Delta + 1080,1500,1500,1500,1080,1500, //Delta_TW + 1080,1500,1500,1500,1500,1080 //AirPlane + }; //int Stick_Max[3]; float Press; +float Base_Press; char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel //volatile bool tick_flag; //volatile bool buzz_flag; -float interval; +volatile float interval; float pid_interval; -int pid_reg[3]; +//int pid_reg[4]; int loop_cnt; +float target_height; +float cuurent_height; +float base_throttol; +int throttol; +bool hov_control; +float Rdata; void initialize(); void FlashLED(int ); @@ -128,18 +162,19 @@ void LCD_printf(char *); void LCD_cls(); void LCD_locate(int,int); -/* -void tick_interrupt() +void Servo_Reverse(int,int); +#ifdef SOFTPWM +void Tpwm_interrupt() { - tick_flag = true; + for ( int i=0; i<4; i++ ) pwm[i].start(); } -*/ -void PulseCheck() +#endif +void PulseCheck() //cppm信号のチェック { channel++; } -void PulseAnalysis() //Interrupt Pin5 +void PulseAnalysis() //cppm信号の解析 { CurTime.stop(); int PulseWidth = CurTime.read_us(); @@ -147,25 +182,9 @@ 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; - } - } + if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max && channel < 10 ) { + CH[Signal[channel-1]] = PulseWidth; + } } channel++; } @@ -180,8 +199,8 @@ Get_Stick_Pos(); while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C' ) //Shrottol Low { - if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High - ESC_SetUp(); +// if ( Stick[COL] > 890 && -Stick[YAW] < Stick_Limit ) // Shrottle High +// ESC_SetUp(); if ( Stick[COL] > 890 || conf.StartMode == 'C' ) // Shrottle High { loop_cnt = 0; @@ -206,7 +225,10 @@ if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down loop_cnt = 0; FlashLED(5); - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min); + + for ( int x=0; x<6; x++ ) { + pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]); + } i2c.start(conf.LCD_Contrast); SetUpPrompt(conf,i2c); } @@ -227,9 +249,10 @@ void initialize() { buzz.period_us(400); + i2c.start(conf.LCD_Contrast); + for ( int i=0;i<6;i++ ) pwm[i].pulsewidth_us(0); ReadConfig(); //config.inf file read - i2c.start(conf.LCD_Contrast); channel = 0; ch1.rise(&PulseCheck); //input pulse count wait(0.2); @@ -240,7 +263,10 @@ else InPulseMode = 'P'; led1 = 0; CycleTime.start(); - for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); +#ifdef SOFTPWM + Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); +#endif + Base_Press = (float)i2c.pressure() / 4096; FlashLED(3); } @@ -282,15 +308,40 @@ #else char *send; char *recv; - int i,rc; + int i; config *conf_ptr; - if ( sizeof(config) > 255 ) { + if ( sizeof(config) > MEM_SIZE ) { LCD_printf("config size over"); wait(3); return; } - rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR ); +//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) +#if defined(INTERNAL_EEPROM) || defined(EXTERNAL_EEPROM) + char buf[MEM_SIZE]; + #if defined(INTERNAL_EEPROM) + iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE ); + #else + //External Flash Memory Wreite + short pos = 0; + if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 ) { + while(1) { + FlashLED(3); + wait(0.5); +return; + } + } + #endif + send = buf; + recv = (char*)&conf; + conf_ptr = (config*)buf; + if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) { + for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i]; + return; + } + +#else + int rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR ); if ( rc == SECTOR_NOT_BLANK ) { send = sector_start_adress[TARGET_SECTOR]; recv = (char*)&conf; @@ -300,6 +351,7 @@ return; } } +#endif WriteConfig(); #endif } @@ -315,25 +367,27 @@ char mem[MEM_SIZE]; char *send; int i; -//pc.printf("start\n\r"); - if ( sizeof(config) > 255 ) { + if ( sizeof(config) > MEM_SIZE ) { LCD_printf("config size over"); wait(3); return; } -//pc.printf("iap start\n\r"); 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; -//pc.printf("prepare start\n\r"); - int rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); -//pc.printf("prepare1(%5d)\n\r",rc); - rc = iap.erase( TARGET_SECTOR, TARGET_SECTOR ); -//pc.printf("erase(%5d)\n\r",rc); - rc = iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); -//pc.printf("prepare2(%5d)\n\r",rc); - rc = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE ); -//pc.printf("write(%5d)\n\r",rc); +//#if defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401) || defined(TARGET_LPC11U35_501) +#if defined(INTERNAL_EEPROM) + iap.write_eeprom( mem, (char*)TARGET_EEPROM_ADDRESS, MEM_SIZE ); +#elif defined(EXTERNAL_EEPROM) +//External Flash Memory Wreite + short pos = 0; + i2c.write_EEPROM( pos,mem,MEM_SIZE) ; +#else + 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 #endif } @@ -345,12 +399,14 @@ // Stick_Save[ROL] = Stick[ROL]; // Stick_Save[PIT] = Stick[PIT]; // Stick_Save[YAW] = Stick[YAW]; +// Stick_Save[COL] = Stick[COL]; 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; + Stick[AUX2] = AX2 - conf.Stick_Ref[COL]; } void Get_Gyro(float interval) @@ -364,49 +420,37 @@ if ( conf.Gyro_Dir[3] ==1 ) err=i2c.angular(&x,&y,&z); else err=i2c.angular(&y,&x,&z); if ( err == false ) return; - Gyro[ROL] = x - Gyro_Ref[0] ; - Gyro[PIT] = y - Gyro_Ref[1] ; - Gyro[YAW] = z - Gyro_Ref[2] ; -/* - x -= Gyro_Ref[0]; - y -= Gyro_Ref[1]; - z -= Gyro_Ref[2]; - float i = 3.14 * 2 * 10 * interval; LPF 10Hz - Gyro[ROL] += -Gyro[ROL] * i + x * i; - Gyro[PIT] += -Gyro[PIT] * i + y * i; - Gyro[YAW] += -Gyro[YAW] * i + z * i; -*/ - if ( fabs(Gyro[ROL]) > 300.0 ) Gyro[ROL] = Gyro_Save[ROL]; - if ( fabs(Gyro[PIT]) > 300.0 ) Gyro[PIT] = Gyro_Save[PIT]; - if ( fabs(Gyro[YAW]) > 300.0 ) Gyro[YAW] = Gyro_Save[YAW]; + Gyro[ROL] = gyroLimit[0].calc(x) - Gyro_Ref[0] ; + Gyro[PIT] = gyroLimit[1].calc(y) - Gyro_Ref[1] ; + Gyro[YAW] = gyroLimit[2].calc(z) - Gyro_Ref[2] ; +//pc.printf("%6.1f,%6.1f\r\n",x,Gyro[ROL]); + } bool Get_Accel(float interval) { float x,y,z; bool err; - Accel_Save[ROL] = Accel[ROL]; - Accel_Save[PIT] = Accel[PIT]; - Accel_Save[YAW] = Accel[YAW]; +// Accel_Save[ROL] = Accel_Angle[ROL]; +// Accel_Save[PIT] = Accel_Angle[PIT]; +// Accel_Save[YAW] = Accel_Angle[YAW]; if ( conf.Gyro_Dir[3] ==1 ) err=i2c.Acceleration(&x,&y,&z); else err=i2c.Acceleration(&y,&x,&z); if ( err == false ) return false; -// Accel[ROL] = x - conf.Accel_Ref[0]; -// Accel[PIT] = y - conf.Accel_Ref[1]; -// Accel[YAW] = z - conf.Accel_Ref[2]; +//pc.printf("%6.4f,%6.4f,%6.4f\r\n",x,y,z); x -= conf.Accel_Ref[0]; y -= conf.Accel_Ref[1]; z -= conf.Accel_Ref[2]; -// float i = 3.14 * 2 * 10 * interval; //LPF 10Hz -// Accel[ROL] += -Accel[ROL] * i + x * i; -// Accel[PIT] += -Accel[PIT] * i + y * i; -// Accel[YAW] += -Accel[YAW] * i + z * i; - Accel[ROL] = atan(x/sqrt( pow(y,2)+pow(z,2)))*180/3.14; - Accel[PIT] = atan(y/sqrt( pow(x,2)+pow(z,2)))*180/3.14; -// Accel[YAW] = atan(sqrt( pow(x,2)+pow(y,2))/z)*180/3.14; - return true; + Accel[ROL] = accLimit[0].calc(x); + Accel[PIT] = accLimit[1].calc(y); + Accel[YAW] = accLimit[2].calc(z); +//pc.printf("%6.4f,%6.4f,%6.4f\r\n",Accel[ROL],Accel[PIT],Accel[YAW]); + Accel[ROL] = atan(x/sqrtf( powf(y,2)+powf(z,2)))*180/3.14f; + Accel[PIT] = atan(y/sqrtf( powf(x,2)+powf(z,2)))*180/3.14f; + Accel[YAW] = atan(sqrtf( powf(x,2)+powf(y,2))/z)*180/3.14f; + return true; } void Get_Angle(float interval) @@ -415,9 +459,9 @@ Get_Accel(interval); Get_Gyro(interval); - x = ( (Gyro[ROL] + Gyro_Save[ROL]) ) * 0.5; - y = ( (Gyro[PIT] + Gyro_Save[PIT]) ) * 0.5; - z = ( (Gyro[YAW] + Gyro_Save[YAW]) ) * 0.5; + x = ( Gyro[ROL] + Gyro_Save[ROL] ) * 0.5f; + y = ( Gyro[PIT] + Gyro_Save[PIT] ) * 0.5f; + z = ( Gyro[YAW] + Gyro_Save[YAW] ) * 0.5f; if ( Get_Accel(interval) == true ) { float i = 3.14 * 2 * conf.Cutoff_Freq * interval; Angle[ROL] += -Angle[ROL] * i + Accel[ROL] * i + x * interval; @@ -428,12 +472,15 @@ Angle[PIT] += y * interval; } Angle[YAW] += z * interval; +//pc.printf("%6.1f,%6.1f,%6.3f\r\n",Angle[ROL],Gyro[ROL],Accel[ROL]); } void Get_Pressure() { - Press = i2c.pressure(); -} + float P = (float)i2c.pressure()/4096; +// Press = 153.8 * ( T + 273.2 ) * ( 1.0 - ( P / Base_Press ) ^ 0.1902f ); + Press = 8.43f * ( P - Base_Press ); +} void CalibrateGyros(void) { @@ -471,109 +518,167 @@ conf.Accel_Ref[0] = k[0]/16; conf.Accel_Ref[1] = k[1]/16; conf.Accel_Ref[2] = k[2]/16-1; +// conf.Accel_Ref[2] = k[2]/16; // FlashLED(3); } void PWM_Out(bool mode) { - int reg[3]; + int reg[3]; int i; float gain; +// float cur_height; interval = CycleTime.read(); CycleTime.reset(); - if ( interval > 0.2 ) return; + if ( interval > 0.2f ) return; TotalTime += interval; - if ( TotalTime > 0.5 ) { + if ( TotalTime > 0.5f ) { led1 = !led1; - if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5f; + if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5; else buzz=0.0; TotalTime = 0; } Get_Angle(interval); pid_interval += interval; - if ( pid_interval < conf.PID_Interval && Stick[GAIN] < 0 ) return; + if ( (pid_interval < (float)conf.PWM_Interval/1000000) && (Stick[GAIN] < 0) ) return; + pid_interval = 0; + Get_Stick_Pos(); - M1 = M2 = M3 = M4 = Stick[COL]; - + switch ( conf.Model_Type ) { + case Quad_X: + M1 = M2 = M3 = M4 = Stick[COL]; + break; + case Quad_H: + M1 = M2 = M3 = M4 = Stick[AUX2]; + M5 = Stick[COL]; + break; + case Delta: + case Delta_TW: + case AirPlane: + M1 = Stick[COL]; + M2 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL]; + M3 = conf.Stick_Ref[PIT] - conf.Stick_Ref[COL]; + M4 = conf.Stick_Ref[YAW] - conf.Stick_Ref[COL]; + if ( conf.Model_Type == AirPlane ) + M5 = conf.Stick_Ref[ROL] - conf.Stick_Ref[COL]; + else M5 = Stick[COL]; + break; + } + 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]; + if ( fabsf(Stick[i]) > 0 ) gain -= gain * ( fabsf(Stick[i]) / 500 ) * conf.Active_Jyro_Gain; if ( Stick[GAIN] > 0 - || i == YAW + || i == YAW + || conf.Model_Type > 0 ) { reg[i] = Stick[i] * conf.Stick_Mix[i]; reg[i] += Gyro[i] * gain * GYRO_ADJUST; } else { -// reg[i] = Stick[i] * conf.Stick_Mix[i]; -// reg[i] += pid[i].calc(Angle[i],0,pid_interval) * conf.Gyro_Dir[i]; - float x = -(float)Stick[i]*45/400; - reg[i] = pid[i].calc(Angle[i],x*conf.Gyro_Dir[i],pid_interval) * conf.Gyro_Dir[i]; - if ( (i == YAW) || (i2c.GetAddr(ACCEL_ADDR)==0) ) { - if ( ( fabsf(Angle[i]) < fabsf(x) ) && ( fabsf(Angle[i]) > fabsf(x*0.7) ) ) { Angle[i] = 0; pid[i].reset(); } - if ( abs(Stick_Save[i]) <= abs(Stick[i]) ) { - Stick_Save[i] = Stick[i]; - } - else { - if ( (abs(Stick_Save[i]) - abs(Stick[i])) > 10 ) { - Stick_Save[i] = 0; - pid[i].reset(); - Angle[i] = 0; - } - } - } -// else { -// if ( fabsf(Accel[i]) < 0.001 ) { -// pid[i].reset(); -// Angle[i] = 0; -// } -// } + reg[i] = ( Angle[i]*conf.Gyro_Dir[i]*400/50 + (float)Stick[i] ) * conf.Stick_Mix[i]; + reg[i] += Gyro[i] * gain * GYRO_ADJUST; } - pid_reg[i] = reg[i]; + +// pid_reg[i] = reg[i]; } pid_interval = 0; - //Calculate Roll Pulse Width - M1 += reg[ROL]; - M2 -= reg[ROL]; - M3 -= reg[ROL]; - M4 += reg[ROL]; + + switch ( conf.Model_Type ) { + case Quad_X: + case Quad_H: + //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 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 ( Stick[COL] > 150 && M[i] < 150 ) M[i] = 150; - if ( M[i] > Thro_Hi ) M[i] = Thro_Hi; - if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level + //Calculate Yaw Pulse Width + M1 -= reg[YAW]; + M2 += reg[YAW]; + M3 -= reg[YAW]; + M4 += reg[YAW]; + break; + case Delta: + case Delta_TW: + //Calculate Roll Pulse Width + M2 += reg[ROL]; + M3 -= reg[ROL]; + //Calculate Pitch Pulse Width + M2 += reg[PIT]; + M3 += reg[PIT]; + //Calculate Yaw Pulse Width + M4 -= reg[YAW]; + //Calculate Yaw Pulse Width + if ( conf.Model_Type == Delta_TW ) { + M1 += reg[YAW]; + M5 -= reg[YAW]; + } + break; + case AirPlane: + //Calculate Roll Pulse Width + M2 -= reg[ROL]; + M5 -= reg[ROL]; + //Calculate Pitch Pulse Width + M3 += reg[PIT]; + //Calculate Yaw Pulse Width + M4 -= reg[YAW]; + break; + } + if ( conf.Model_Type != AirPlane ) { + for ( i=0;i<4;i++ ) + { + if ( M[i] > Thro_Hi ) M[i] = Thro_Hi; + if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // motor idle level + } } - if (Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0; - + switch ( conf.Model_Type ) { + case Quad_X: + if ( Stick[COL] < Thro_Zero ) M1=M2=M3=M4=0; + break; + case Quad_H: + if ( Stick[COL] < Thro_Zero ) M5=0; + Servo_Reverse(1,4); + break; + case Delta: + case Delta_TW: + if ( Stick[COL] < Thro_Zero ) M1=M5=0; + Servo_Reverse(2,4); + break; + case AirPlane: + Servo_Reverse(2,5); + break; + } if ( mode ) { - for ( i=0;i<4;i++ ) { - while ( !pwmpin[i] ); - if ( conf.PWM_Mode == 1 ) + for ( i=0;i<6;i++ ) { +// while ( !pwmpin[i] ); + if ( conf.PWM_Mode == 1 ) pwm[i].pulsewidth_us(conf.ESC_Low+M[i]); - else pwm[i].pulsewidth_us(M[i]); + else pwm[i].pulsewidth_us(M[i]); } } } +void Servo_Reverse(int start,int end) { + for ( int i=start-1; i<end; i++ ) { + if ( conf.Servo_Dir[i] == -1 ) + M[i] = 1500 + ( ( conf.Stick_Ref[COL] + M[i] - 1500 ) * conf.Servo_Dir[i] ) - conf.Stick_Ref[COL]; + } +} + void ESC_SetUp(void) { while(1) { Get_Stick_Pos(); @@ -585,16 +690,23 @@ void Flight_SetUp(void) { int i; - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(0); - for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval); - for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(Pulse_Min); - for ( i=0;i<4;i++ ) pid[i].init(conf.kp[i],conf.ki[i],conf.kd[i] - ,conf.PID_Limit,conf.Differential_Limit); +#ifdef SOFTPWM + Tpwm.detach(); + Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval); +#else + for ( i=0;i<6;i++ ) pwm[i].pulsewidth_us(0); + for ( i=0;i<6;i++ ) pwm[i].period_us(conf.PWM_Interval); +#endif + for ( i=0; i<6; i++ ) { + pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]); + } + hov_control = false; Angle[ROL]=Angle[PIT]=Angle[YAW]=0; loop_cnt = 0; FlyghtTime.start(); CycleTime.start(); pid_interval = 0; + Stick_Save[COL] = Stick[COL]; FlashLED(5); } @@ -625,3 +737,9 @@ + + + + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 14 08:15:03 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1 \ No newline at end of file
--- a/mbed.lib Thu Feb 13 16:07:07 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed/#673126e12c73