syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

main.cpp@3:27407c4984cf, 2014-02-13 (annotated)

Committer:
komaida424
Date:
Thu Feb 13 16:07:07 2014 +0000
Revision:
3:27407c4984cf
Parent:
2:59ac9df97701
Child:
4:4060309b9cc0
revsion.1.30

Who changed what in which revision?

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