syouichi imamori / Mbed OS MulticopterQuadX

Quad X Type Multicopter

Dependencies:   IAP

main.cpp@0:cca1c4e84da4, 2013-07-11 (annotated)

Committer:
komaida424
Date:
Thu Jul 11 19:18:44 2013 +0000
Revision:
0:cca1c4e84da4
Child:
2:59ac9df97701
version1.03

Who changed what in which revision?

UserRevisionLine numberNew contents of line
komaida424 0:cca1c4e84da4 1 #include "mbed.h"
komaida424 0:cca1c4e84da4 2 #include "I2cPeripherals.h"
komaida424 0:cca1c4e84da4 3 //#include "I2cLCD.h"
komaida424 0:cca1c4e84da4 4 #include "InterruptIn.h"
komaida424 0:cca1c4e84da4 5 //#include "ITG3200.h"
komaida424 0:cca1c4e84da4 6 //#include "L3GD20.h"
komaida424 0:cca1c4e84da4 7 #include "config.h"
komaida424 0:cca1c4e84da4 8 #include "PulseWidthCounter.h"
komaida424 0:cca1c4e84da4 9 #include "string"
komaida424 0:cca1c4e84da4 10 #include "SerialLcd.h"
komaida424 0:cca1c4e84da4 11 #include "IAP.h"
komaida424 0:cca1c4e84da4 12 //LPC1768 Flash Memory read/write
komaida424 0:cca1c4e84da4 13 #define MEM_SIZE 256
komaida424 0:cca1c4e84da4 14 #define TARGET_SECTOR 29 // use sector 29 as target sector if it is on LPC1768
komaida424 0:cca1c4e84da4 15 IAP iap;
komaida424 0:cca1c4e84da4 16
komaida424 0:cca1c4e84da4 17 #ifdef LPCXpresso
komaida424 0:cca1c4e84da4 18 DigitalOut led1(P0_22);
komaida424 0:cca1c4e84da4 19 #define led2 led1
komaida424 0:cca1c4e84da4 20 #else
komaida424 0:cca1c4e84da4 21 DigitalOut led1(LED1);
komaida424 0:cca1c4e84da4 22 DigitalOut led2(LED2);
komaida424 0:cca1c4e84da4 23 #endif
komaida424 0:cca1c4e84da4 24 InterruptIn ch1(p5);
komaida424 0:cca1c4e84da4 25 PulseWidthCounter ch[5] = { PulseWidthCounter(p6,POSITIVE),
komaida424 0:cca1c4e84da4 26 PulseWidthCounter(p7,POSITIVE),
komaida424 0:cca1c4e84da4 27 PulseWidthCounter(p8,POSITIVE),
komaida424 0:cca1c4e84da4 28 PulseWidthCounter(p9,POSITIVE),
komaida424 0:cca1c4e84da4 29 PulseWidthCounter(p10,POSITIVE)
komaida424 0:cca1c4e84da4 30 };
komaida424 0:cca1c4e84da4 31 PwmOut pwm[4] = { p21,p22,p23,p24 };
komaida424 0:cca1c4e84da4 32 Timer CurTime;
komaida424 0:cca1c4e84da4 33 Timer ElapTime;
komaida424 0:cca1c4e84da4 34 I2cPeripherals i2c(p28,p27); //sda scl
komaida424 0:cca1c4e84da4 35 #ifdef SERIAL_LCD
komaida424 0:cca1c4e84da4 36 SerialLcd lcd(p13);
komaida424 0:cca1c4e84da4 37 #endif
komaida424 0:cca1c4e84da4 38 config conf;
komaida424 0:cca1c4e84da4 39 int StartTime;
komaida424 0:cca1c4e84da4 40 int Channel = 0;
komaida424 0:cca1c4e84da4 41 int CH[5];
komaida424 0:cca1c4e84da4 42 int M[6];
komaida424 0:cca1c4e84da4 43 int Gyro[3];
komaida424 0:cca1c4e84da4 44 int Accel[3];
komaida424 0:cca1c4e84da4 45 int Gyro_Ref[3];
komaida424 0:cca1c4e84da4 46 int Accel_Ref[3];
komaida424 0:cca1c4e84da4 47 int Stick[5];
komaida424 0:cca1c4e84da4 48 float Press;
komaida424 0:cca1c4e84da4 49 char InPulseMode; //Receiver Signal Type 's':Serial, 'P':Parallel
komaida424 0:cca1c4e84da4 50
komaida424 0:cca1c4e84da4 51 void initialize();
komaida424 0:cca1c4e84da4 52 void FlashLED(int );
komaida424 0:cca1c4e84da4 53 void SetUp();
komaida424 0:cca1c4e84da4 54 #ifdef SERIAL_LCD
komaida424 0:cca1c4e84da4 55 void SetUpPrompt(config&,SerialLcd&);
komaida424 0:cca1c4e84da4 56 #else
komaida424 0:cca1c4e84da4 57 void SetUpPrompt(config&,I2cPeripherals&);
komaida424 0:cca1c4e84da4 58 #endif
komaida424 0:cca1c4e84da4 59 void PWM_Out(bool);
komaida424 0:cca1c4e84da4 60 void Get_Stick_Pos();
komaida424 0:cca1c4e84da4 61 void CalibrateGyros(void);
komaida424 0:cca1c4e84da4 62 void CalibrateAccel(void);
komaida424 0:cca1c4e84da4 63 void Get_Gyro();
komaida424 0:cca1c4e84da4 64 void Get_Accel();
komaida424 0:cca1c4e84da4 65 void ReadConfig();
komaida424 0:cca1c4e84da4 66 void WriteConfig();
komaida424 0:cca1c4e84da4 67 void ESC_SetUp(void);
komaida424 0:cca1c4e84da4 68
komaida424 0:cca1c4e84da4 69 void PulseCheck()
komaida424 0:cca1c4e84da4 70 {
komaida424 0:cca1c4e84da4 71 Channel++;
komaida424 0:cca1c4e84da4 72 }
komaida424 0:cca1c4e84da4 73
komaida424 0:cca1c4e84da4 74 void PulseAnalysis() //Interrupt Pin5
komaida424 0:cca1c4e84da4 75 {
komaida424 0:cca1c4e84da4 76 CurTime.stop();
komaida424 0:cca1c4e84da4 77 int PulseWidth = CurTime.read_us();
komaida424 0:cca1c4e84da4 78 CurTime.reset();
komaida424 0:cca1c4e84da4 79 CurTime.start();
komaida424 0:cca1c4e84da4 80 if ( PulseWidth > 3000 ) Channel = 0; //reset pulse count
komaida424 0:cca1c4e84da4 81 else {
komaida424 0:cca1c4e84da4 82 if ( PulseWidth > Pulse_Min && PulseWidth < Pulse_Max ) {
komaida424 0:cca1c4e84da4 83 switch( Channel ) {
komaida424 0:cca1c4e84da4 84 case IR_THR:
komaida424 0:cca1c4e84da4 85 THR = PulseWidth;
komaida424 0:cca1c4e84da4 86 break;
komaida424 0:cca1c4e84da4 87 case IR_AIL:
komaida424 0:cca1c4e84da4 88 AIL = PulseWidth;
komaida424 0:cca1c4e84da4 89 break;
komaida424 0:cca1c4e84da4 90 case IR_ELE:
komaida424 0:cca1c4e84da4 91 ELE = PulseWidth;
komaida424 0:cca1c4e84da4 92 break;
komaida424 0:cca1c4e84da4 93 case IR_RUD:
komaida424 0:cca1c4e84da4 94 RUD = PulseWidth;
komaida424 0:cca1c4e84da4 95 break;
komaida424 0:cca1c4e84da4 96 case IR_AUX:
komaida424 0:cca1c4e84da4 97 AUX = PulseWidth;
komaida424 0:cca1c4e84da4 98 break;
komaida424 0:cca1c4e84da4 99 }
komaida424 0:cca1c4e84da4 100 }
komaida424 0:cca1c4e84da4 101 }
komaida424 0:cca1c4e84da4 102 Channel++;
komaida424 0:cca1c4e84da4 103 }
komaida424 0:cca1c4e84da4 104
komaida424 0:cca1c4e84da4 105 int main()
komaida424 0:cca1c4e84da4 106 {
komaida424 0:cca1c4e84da4 107 int i,j=0;
komaida424 0:cca1c4e84da4 108
komaida424 0:cca1c4e84da4 109 initialize();
komaida424 0:cca1c4e84da4 110 wait(0.5);
komaida424 0:cca1c4e84da4 111 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 112 while ( Stick[COL] > 30 || conf.StartMode == 'C' ) //Shrottol Low
komaida424 0:cca1c4e84da4 113 {
komaida424 0:cca1c4e84da4 114 if ( Stick[COL] > 350 || conf.StartMode == 'C' ) // Shrottle High
komaida424 0:cca1c4e84da4 115 {
komaida424 0:cca1c4e84da4 116 #ifdef SERIAL_LCD
komaida424 0:cca1c4e84da4 117 SetUpPrompt(conf,lcd);
komaida424 0:cca1c4e84da4 118 #else
komaida424 0:cca1c4e84da4 119 SetUpPrompt(conf,i2c);
komaida424 0:cca1c4e84da4 120 #endif
komaida424 0:cca1c4e84da4 121 for ( i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
komaida424 0:cca1c4e84da4 122 break;
komaida424 0:cca1c4e84da4 123 }
komaida424 0:cca1c4e84da4 124 FlashLED(3);
komaida424 0:cca1c4e84da4 125 wait(1);
komaida424 0:cca1c4e84da4 126 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 127 }
komaida424 0:cca1c4e84da4 128 led2 = 1;
komaida424 0:cca1c4e84da4 129 ElapTime.start();
komaida424 0:cca1c4e84da4 130
komaida424 0:cca1c4e84da4 131 while (1)
komaida424 0:cca1c4e84da4 132 {
komaida424 0:cca1c4e84da4 133 if ( Stick[COL] < 30 )
komaida424 0:cca1c4e84da4 134 {
komaida424 0:cca1c4e84da4 135 i = 0;
komaida424 0:cca1c4e84da4 136 ElapTime.stop();
komaida424 0:cca1c4e84da4 137 while ( Stick[YAW] < -Stick_Limit && Stick[COL] < 30 )
komaida424 0:cca1c4e84da4 138 {
komaida424 0:cca1c4e84da4 139 if ( i > 100 ) //wait 2 sec
komaida424 0:cca1c4e84da4 140 {
komaida424 0:cca1c4e84da4 141 CalibrateGyros();
komaida424 0:cca1c4e84da4 142 CalibrateAccel();
komaida424 0:cca1c4e84da4 143 FlashLED(6);
komaida424 0:cca1c4e84da4 144 ElapTime.start();
komaida424 0:cca1c4e84da4 145 break;
komaida424 0:cca1c4e84da4 146 }
komaida424 0:cca1c4e84da4 147 wait(0.01); // wait 10 msec
komaida424 0:cca1c4e84da4 148 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 149 i++;
komaida424 0:cca1c4e84da4 150 }
komaida424 0:cca1c4e84da4 151 }
komaida424 0:cca1c4e84da4 152 j++;
komaida424 0:cca1c4e84da4 153 if (j>100) { j=0; led2 = !led2;}
komaida424 0:cca1c4e84da4 154 ElapTime.stop();
komaida424 0:cca1c4e84da4 155 wait(float(conf.PWM_Interval-ElapTime.read_us()-2)/1000000);
komaida424 0:cca1c4e84da4 156 ElapTime.reset();
komaida424 0:cca1c4e84da4 157 ElapTime.start();
komaida424 0:cca1c4e84da4 158 PWM_Out(true);
komaida424 0:cca1c4e84da4 159 }
komaida424 0:cca1c4e84da4 160
komaida424 0:cca1c4e84da4 161 }
komaida424 0:cca1c4e84da4 162
komaida424 0:cca1c4e84da4 163 void initialize()
komaida424 0:cca1c4e84da4 164 {
komaida424 0:cca1c4e84da4 165 #ifndef SERIAL_LCD
komaida424 0:cca1c4e84da4 166 i2c.start(ST7032_ADDR,conf.LCD_Contrast);
komaida424 0:cca1c4e84da4 167 #endif
komaida424 0:cca1c4e84da4 168 ReadConfig(); //config.inf file read
komaida424 0:cca1c4e84da4 169 // CurTime.start();
komaida424 0:cca1c4e84da4 170 Channel = 0;
komaida424 0:cca1c4e84da4 171 ch1.rise(&PulseCheck); //input pulse count
komaida424 0:cca1c4e84da4 172 wait(0.1);
komaida424 0:cca1c4e84da4 173 if ( Channel > 30 ) {
komaida424 0:cca1c4e84da4 174 ch1.rise(&PulseAnalysis);
komaida424 0:cca1c4e84da4 175 InPulseMode = 'S';
komaida424 0:cca1c4e84da4 176 }
komaida424 0:cca1c4e84da4 177 else InPulseMode = 'P';
komaida424 0:cca1c4e84da4 178 if ( conf.Gyro_Type == _ITG3200 )
komaida424 0:cca1c4e84da4 179 i2c.start(ITG3200_ADDR0);
komaida424 0:cca1c4e84da4 180 else
komaida424 0:cca1c4e84da4 181 i2c.start(L3GD20_ADDR1);
komaida424 0:cca1c4e84da4 182 CalibrateGyros();
komaida424 0:cca1c4e84da4 183 i2c.start(LDXL345_ADDR0);
komaida424 0:cca1c4e84da4 184 CalibrateGyros();
komaida424 0:cca1c4e84da4 185 CalibrateAccel();
komaida424 0:cca1c4e84da4 186 i2c.start(LPS331AP_ADDR1);
komaida424 0:cca1c4e84da4 187 for ( int i=0;i<4;i++ ) pwm[i].period_us(conf.PWM_Interval);
komaida424 0:cca1c4e84da4 188 }
komaida424 0:cca1c4e84da4 189
komaida424 0:cca1c4e84da4 190 void FlashLED(int cnt)
komaida424 0:cca1c4e84da4 191 {
komaida424 0:cca1c4e84da4 192 for ( int i = 0 ; i < cnt ; i++ ) {
komaida424 0:cca1c4e84da4 193 led1 = !led1;
komaida424 0:cca1c4e84da4 194 wait(0.05);
komaida424 0:cca1c4e84da4 195 led1 = !led1;
komaida424 0:cca1c4e84da4 196 wait(0.05);
komaida424 0:cca1c4e84da4 197 }
komaida424 0:cca1c4e84da4 198 }
komaida424 0:cca1c4e84da4 199
komaida424 0:cca1c4e84da4 200 void ReadConfig()
komaida424 0:cca1c4e84da4 201 {
komaida424 0:cca1c4e84da4 202 #ifdef LocalFileOut
komaida424 0:cca1c4e84da4 203 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
komaida424 0:cca1c4e84da4 204 FILE *fp = fopen("/local/setup.inf", "rb"); // Open "out.txt" on the local file system for writing
komaida424 0:cca1c4e84da4 205 if ( fp != NULL ) {
komaida424 0:cca1c4e84da4 206 float rev = conf.Revision;
komaida424 0:cca1c4e84da4 207 int len = fread(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 208 switch ( len ) {
komaida424 0:cca1c4e84da4 209 case sizeof(config): // File size ok
komaida424 0:cca1c4e84da4 210 if ( rev == conf.Revision ) break;
komaida424 0:cca1c4e84da4 211 default:
komaida424 0:cca1c4e84da4 212 fclose(fp);
komaida424 0:cca1c4e84da4 213 config init;
komaida424 0:cca1c4e84da4 214 conf = init;
komaida424 0:cca1c4e84da4 215 fp = fopen("/local/setup.inf", "wb");
komaida424 0:cca1c4e84da4 216 fwrite(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 217 }
komaida424 0:cca1c4e84da4 218 fclose(fp);
komaida424 0:cca1c4e84da4 219 } else {
komaida424 0:cca1c4e84da4 220 WriteConfig();
komaida424 0:cca1c4e84da4 221 wait(2);
komaida424 0:cca1c4e84da4 222 }
komaida424 0:cca1c4e84da4 223 #else
komaida424 0:cca1c4e84da4 224 char *send;
komaida424 0:cca1c4e84da4 225 char *recv;
komaida424 0:cca1c4e84da4 226 int i,rc;
komaida424 0:cca1c4e84da4 227 config *conf_ptr;
komaida424 0:cca1c4e84da4 228
komaida424 0:cca1c4e84da4 229 if ( sizeof(config) > 255 ) {
komaida424 0:cca1c4e84da4 230 i2c.printf("config size over");
komaida424 0:cca1c4e84da4 231 return;
komaida424 0:cca1c4e84da4 232 }
komaida424 0:cca1c4e84da4 233 rc = iap.blank_check( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 234 if ( rc == SECTOR_NOT_BLANK ) {
komaida424 0:cca1c4e84da4 235 send = sector_start_adress[TARGET_SECTOR];
komaida424 0:cca1c4e84da4 236 recv = (char*)&conf;
komaida424 0:cca1c4e84da4 237 conf_ptr = (config*)sector_start_adress[TARGET_SECTOR];
komaida424 0:cca1c4e84da4 238 if ( conf_ptr->Revision == conf.Revision && conf_ptr->Struct_Size == sizeof(config) ) {
komaida424 0:cca1c4e84da4 239 for ( i=0;i<sizeof(config);i++ ) recv[i] = send[i];
komaida424 0:cca1c4e84da4 240 // i2c.printf("config read OK");
komaida424 0:cca1c4e84da4 241 // wait(1);
komaida424 0:cca1c4e84da4 242 return;
komaida424 0:cca1c4e84da4 243 }
komaida424 0:cca1c4e84da4 244 }
komaida424 0:cca1c4e84da4 245 WriteConfig();
komaida424 0:cca1c4e84da4 246 // i2c.printf("config write OK");
komaida424 0:cca1c4e84da4 247 // wait(1);
komaida424 0:cca1c4e84da4 248 #endif
komaida424 0:cca1c4e84da4 249 }
komaida424 0:cca1c4e84da4 250
komaida424 0:cca1c4e84da4 251 void WriteConfig()
komaida424 0:cca1c4e84da4 252 {
komaida424 0:cca1c4e84da4 253 #ifdef LocalFileOut
komaida424 0:cca1c4e84da4 254 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
komaida424 0:cca1c4e84da4 255 FILE *fp = fopen("/local/setup.inf", "wb");
komaida424 0:cca1c4e84da4 256 fwrite(&conf,1,sizeof(config),fp);
komaida424 0:cca1c4e84da4 257 fclose(fp);
komaida424 0:cca1c4e84da4 258 #else
komaida424 0:cca1c4e84da4 259 char mem[MEM_SIZE];
komaida424 0:cca1c4e84da4 260 char *send;
komaida424 0:cca1c4e84da4 261 int i;
komaida424 0:cca1c4e84da4 262 if ( sizeof(config) > 255 ) {
komaida424 0:cca1c4e84da4 263 printf("config size over");
komaida424 0:cca1c4e84da4 264 return;
komaida424 0:cca1c4e84da4 265 }
komaida424 0:cca1c4e84da4 266 send = (char*)&conf;
komaida424 0:cca1c4e84da4 267 for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
komaida424 0:cca1c4e84da4 268 for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
komaida424 0:cca1c4e84da4 269 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 270 iap.erase( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 271 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
komaida424 0:cca1c4e84da4 272 iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
komaida424 0:cca1c4e84da4 273 #endif
komaida424 0:cca1c4e84da4 274 }
komaida424 0:cca1c4e84da4 275
komaida424 0:cca1c4e84da4 276 void Get_Stick_Pos(void)
komaida424 0:cca1c4e84da4 277 {
komaida424 0:cca1c4e84da4 278 if ( InPulseMode == 'P' ) {
komaida424 0:cca1c4e84da4 279 for (int i=0;i<5;i++) CH[i] = ch[i].count;
komaida424 0:cca1c4e84da4 280 }
komaida424 0:cca1c4e84da4 281 Stick[ROL] = AIL - conf.Stick_Ref[ROL];
komaida424 0:cca1c4e84da4 282 Stick[PIT] = ELE - conf.Stick_Ref[PIT];
komaida424 0:cca1c4e84da4 283 Stick[YAW] = RUD - conf.Stick_Ref[YAW];
komaida424 0:cca1c4e84da4 284 Stick[COL] = THR - conf.Stick_Ref[COL];
komaida424 0:cca1c4e84da4 285 Stick[GAIN] = ( AUX - conf.Stick_Ref[GAIN] ) / 4;
komaida424 0:cca1c4e84da4 286 }
komaida424 0:cca1c4e84da4 287
komaida424 0:cca1c4e84da4 288 void Get_Gyro()
komaida424 0:cca1c4e84da4 289 {
komaida424 0:cca1c4e84da4 290 int x,y,z;
komaida424 0:cca1c4e84da4 291 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
komaida424 0:cca1c4e84da4 292 else i2c.angular(&y,&x,&z);
komaida424 0:cca1c4e84da4 293 Gyro[ROL] = ( x - Gyro_Ref[0] ) / 5;
komaida424 0:cca1c4e84da4 294 Gyro[PIT] = ( y - Gyro_Ref[1] ) / 5;
komaida424 0:cca1c4e84da4 295 Gyro[YAW] = ( z - Gyro_Ref[2] ) / 5;
komaida424 0:cca1c4e84da4 296 }
komaida424 0:cca1c4e84da4 297
komaida424 0:cca1c4e84da4 298 void Get_Accel()
komaida424 0:cca1c4e84da4 299 {
komaida424 0:cca1c4e84da4 300 int x,y,z;
komaida424 0:cca1c4e84da4 301 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
komaida424 0:cca1c4e84da4 302 else i2c.Acceleration(&y,&x,&z);
komaida424 0:cca1c4e84da4 303 Accel[ROL] = ( x - Accel_Ref[0] );
komaida424 0:cca1c4e84da4 304 Accel[PIT] = ( y - Accel_Ref[1] );
komaida424 0:cca1c4e84da4 305 Accel[YAW] = ( z - Accel_Ref[2] );
komaida424 0:cca1c4e84da4 306 }
komaida424 0:cca1c4e84da4 307 void Get_Pressure()
komaida424 0:cca1c4e84da4 308 {
komaida424 0:cca1c4e84da4 309 Press = i2c.pressure();
komaida424 0:cca1c4e84da4 310 }
komaida424 0:cca1c4e84da4 311
komaida424 0:cca1c4e84da4 312 void CalibrateGyros(void)
komaida424 0:cca1c4e84da4 313 {
komaida424 0:cca1c4e84da4 314 int i,j,x,y,z;
komaida424 0:cca1c4e84da4 315 int k[3]={0,0,0};
komaida424 0:cca1c4e84da4 316 wait(1);
komaida424 0:cca1c4e84da4 317 for(i=0; i<16; i++) {
komaida424 0:cca1c4e84da4 318 if ( conf.Gyro_Dir[3] ==1 ) i2c.angular(&x,&y,&z);
komaida424 0:cca1c4e84da4 319 else i2c.angular(&y,&x,&z);
komaida424 0:cca1c4e84da4 320 k[0] += x;
komaida424 0:cca1c4e84da4 321 k[1] += y;
komaida424 0:cca1c4e84da4 322 k[2] += z;
komaida424 0:cca1c4e84da4 323 wait(0.005);
komaida424 0:cca1c4e84da4 324 }
komaida424 0:cca1c4e84da4 325 for( j=0; j<3; j++ ) Gyro_Ref[j] = k[j]/16;
komaida424 0:cca1c4e84da4 326 FlashLED(3);
komaida424 0:cca1c4e84da4 327 }
komaida424 0:cca1c4e84da4 328
komaida424 0:cca1c4e84da4 329 void CalibrateAccel(void)
komaida424 0:cca1c4e84da4 330 {
komaida424 0:cca1c4e84da4 331 int i,j,x,y,z;
komaida424 0:cca1c4e84da4 332 int k[3]={0,0,0};
komaida424 0:cca1c4e84da4 333 wait(1);
komaida424 0:cca1c4e84da4 334 for(i=0; i<16; i++) {
komaida424 0:cca1c4e84da4 335 if ( conf.Gyro_Dir[3] ==1 ) i2c.Acceleration(&x,&y,&z);
komaida424 0:cca1c4e84da4 336 else i2c.Acceleration(&y,&x,&z);
komaida424 0:cca1c4e84da4 337 k[0] += x;
komaida424 0:cca1c4e84da4 338 k[1] += y;
komaida424 0:cca1c4e84da4 339 k[2] += z;
komaida424 0:cca1c4e84da4 340 wait(0.005);
komaida424 0:cca1c4e84da4 341 }
komaida424 0:cca1c4e84da4 342 for( j=0; j<3; j++ ) Accel_Ref[j] = k[j]/16;
komaida424 0:cca1c4e84da4 343 FlashLED(3);
komaida424 0:cca1c4e84da4 344 }
komaida424 0:cca1c4e84da4 345
komaida424 0:cca1c4e84da4 346 void PWM_Out(bool mode)
komaida424 0:cca1c4e84da4 347 {
komaida424 0:cca1c4e84da4 348 int reg[3];
komaida424 0:cca1c4e84da4 349 int i;
komaida424 0:cca1c4e84da4 350 float gain;
komaida424 0:cca1c4e84da4 351
komaida424 0:cca1c4e84da4 352 // wait(0.002);
komaida424 0:cca1c4e84da4 353 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 354 Get_Gyro();
komaida424 0:cca1c4e84da4 355 // Get_Accel();
komaida424 0:cca1c4e84da4 356
komaida424 0:cca1c4e84da4 357 M1 = M2 = M3 = M4 = Stick[COL];
komaida424 0:cca1c4e84da4 358 for ( i=0;i<3;i++ )
komaida424 0:cca1c4e84da4 359 {
komaida424 0:cca1c4e84da4 360 // Stick Angle Mixing
komaida424 0:cca1c4e84da4 361 if ( conf.Gyro_Gain_Setting == 1 ) gain = conf.Gyro_Gain[i] * conf.Gyro_Dir[i];
komaida424 0:cca1c4e84da4 362 else gain = ( (float)abs(Stick[GAIN])/100 + conf.Gyro_Gain[i+3] ) * conf.Gyro_Dir[i];
komaida424 0:cca1c4e84da4 363 reg[i] = ( Stick[i] * conf.Stick_Mix[i] ) + ( Gyro[i] * gain );
komaida424 0:cca1c4e84da4 364 // if ( Stick[GAIN] < 0 )
komaida424 0:cca1c4e84da4 365 // reg[i] += Accel[i] * conf.Accel_Gain[i];
komaida424 0:cca1c4e84da4 366 }
komaida424 0:cca1c4e84da4 367 //Calculate Roll Pulse Width
komaida424 0:cca1c4e84da4 368 M1 += reg[ROL];
komaida424 0:cca1c4e84da4 369 M2 -= reg[ROL];
komaida424 0:cca1c4e84da4 370 M3 -= reg[ROL];
komaida424 0:cca1c4e84da4 371 M4 += reg[ROL];
komaida424 0:cca1c4e84da4 372
komaida424 0:cca1c4e84da4 373 //Calculate Pitch Pulse Width
komaida424 0:cca1c4e84da4 374 M1 += reg[PIT];
komaida424 0:cca1c4e84da4 375 M2 += reg[PIT];
komaida424 0:cca1c4e84da4 376 M3 -= reg[PIT];
komaida424 0:cca1c4e84da4 377 M4 -= reg[PIT];
komaida424 0:cca1c4e84da4 378
komaida424 0:cca1c4e84da4 379 //Calculate Yaw Pulse Width
komaida424 0:cca1c4e84da4 380 M1 -= reg[YAW];
komaida424 0:cca1c4e84da4 381 M2 += reg[YAW];
komaida424 0:cca1c4e84da4 382 M3 -= reg[YAW];
komaida424 0:cca1c4e84da4 383 M4 += reg[YAW];
komaida424 0:cca1c4e84da4 384
komaida424 0:cca1c4e84da4 385 for ( i=0;i<4;i++ )
komaida424 0:cca1c4e84da4 386 {
komaida424 0:cca1c4e84da4 387 if ( M[i] > Thro_Hi ) M[i] = Thro_Hi;
komaida424 0:cca1c4e84da4 388 if ( M[i] < Thro_Lo ) M[i] = Thro_Lo; // this is the motor idle level
komaida424 0:cca1c4e84da4 389 }
komaida424 0:cca1c4e84da4 390
komaida424 0:cca1c4e84da4 391 if (Stick[COL] < 20 ) M1=M2=M3=M4=0;
komaida424 0:cca1c4e84da4 392 if ( mode )
komaida424 0:cca1c4e84da4 393 for ( i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+M[i]);
komaida424 0:cca1c4e84da4 394
komaida424 0:cca1c4e84da4 395 }
komaida424 0:cca1c4e84da4 396
komaida424 0:cca1c4e84da4 397 void ESC_SetUp(void) {
komaida424 0:cca1c4e84da4 398 while(1) {
komaida424 0:cca1c4e84da4 399 Get_Stick_Pos();
komaida424 0:cca1c4e84da4 400 for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(Stick[COL]);
komaida424 0:cca1c4e84da4 401 wait(0.01);
komaida424 0:cca1c4e84da4 402 }
komaida424 0:cca1c4e84da4 403 };
komaida424 0:cca1c4e84da4 404
komaida424 0:cca1c4e84da4 405
komaida424 0:cca1c4e84da4 406
komaida424 0:cca1c4e84da4 407
komaida424 0:cca1c4e84da4 408