syouichi imamori
Quad X Type Multicopter
Diff: main.cpp
- Revision:
- 8:1db19b529b22
- Parent:
- 7:16bf0085d914
--- a/main.cpp Tue Apr 28 01:48:21 2015 +0000
+++ b/main.cpp Sun Feb 21 05:14:57 2021 +0000
@@ -21,15 +21,18 @@
#include "PulseWidthCounter.h"
#include "string"
#include "SerialLcd.h"
+
+//#if defined(TARGET_NUCLEO_FXXXXX)
+// #include "eeprom_flash.h"
+//#endif
//#include "PID.h"
-#include "SoftPWM.h"
-#include "PulseOut.h"
+//#include "SoftPWM.h"
#include "Limiter.h"
-#ifndef TARGET_NUCLEO_F401RE
- #include "IAP.h"
-#endif
+#include "IAP.h"
+#define DEBUG
//Serial pc(USBTX, USBRX);
+//Serial pc(PA_9,PA_10);
#if defined(TARGET_LPC1768)
DigitalInOut pwmpin[] = { p21,p22,p23,p24 };
@@ -39,59 +42,106 @@
DigitalOut led1(LED1);
// DigitalOut led2(LED2);
InterruptIn ch1(p5);
- PulseWidthCounter ch[6] = { p6,p7,p8,p9,p10,p11 };
+ PulseWidthCounter ch[6] = { PulseWidthCounter(p6),
+ PulseWidthCounter(p7),
+ PulseWidthCounter(p8),
+ PulseWidthCounter(p9),
+ PulseWidthCounter(p10),
+ PulseWidthCounter(p11) };
PwmOut pwm[6] = { p21,p22,p23,p24,p25,p26 };
-// SoftPWM pwm[6] = { p21,p22,p23,p24,p25,p26 };
- SoftPWM buzz(p20);
+// SoftPWM buzz(p20);
I2cPeripherals i2c(p28,p27); //sda scl
SerialLcd lcd(p13,p14);
+ #define PwmNum 6
#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);
+#elif defined(TARGET_STM32F1)
+//#define NAZE32BORD
+ #ifdef NAZE32BORD
+ DigitalOut led1(PC_13);
+ InterruptIn ch1(PA_0);
+ PulseWidthCounter ch[6] = { PA_8,PA_11,PB_6,PB_7,PB_8,PB_9};
+ PwmOut pwm[6] = { PA_0,PA_1,PA_2,PA_3,PA_6,PA_7 };
+// SoftPWM buzz(PA_2);
+// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
+ I2cPeripherals i2c(PB_9,PB_8); //sda scl
+ SerialLcd lcd(PA_2);
+ #define PwmNum 6
+ #define MEM_SIZE 256
+ #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
+ #else
+ DigitalOut led1(PC_13);
+ InterruptIn ch1(PA_0);
+ PulseWidthCounter ch[6] = { PA_11,PA_12,PA_15,PB_3,PB_4,PB_5};
+ PwmOut pwm[6] = { PA_6,PA_7,PB_0,PB_1,PB_10,PB_11 };
+// SoftPWM buzz(PA_2);
+// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
+ I2cPeripherals i2c(PB_9,PB_8); //sda scl
+ SerialLcd lcd(PA_2);
+ #define PwmNum 6
+ #define MEM_SIZE 256
+ #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
+ #endif
+#elif defined(TARGET_STM32F3)
+ DigitalOut led1(PB_3);
+ InterruptIn ch1(PA_0);
+ PulseWidthCounter ch[6] = { PA_0,PA_1,PB_11,PB_10,PB_4,PB_5};
+ PwmOut pwm[6] = { PA_6,PA_7,PA_11,PA_12,PB_8,PB_9 };
+// SoftPWM buzz(PA_2);
+// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
+ I2cPeripherals i2c(PB_7,PB_6); //sda scl
+ SerialLcd lcd(PA_9);
+ #define PwmNum 6
+ #define MEM_SIZE 256
+ #define STM32_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
+#elif defined(TARGET_NUCLEO_F4)
+ DigitalOut led1(PA_5);
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);
+ PwmOut pwm[6] = { D3,D5,D6,D9,D11,D12 };
+// SoftPWM buzz(PB_13);
// I2cPeripherals i2c(I2C_SDA,I2C_SCL); //sda scl
- I2cPeripherals i2c(D5,D7); //sda scl
- SerialLcd lcd(PA_11,PA_12);
+ I2cPeripherals i2c(PB_9,PB_8); //sda scl
+ SerialLcd lcd(PA_11);
+ #define PwmNum 6
#define MEM_SIZE 256
- #define EXTERNAL_EEPROM //24AAXX/24LCXX/24FCXX EEPROM
+ #define STM32_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);
+ PwmOut pwm[4] = { P0_14,P0_2,P0_23,P0_17 };
+// SoftPWM buzz(P1_19);
I2cPeripherals i2c(P0_5,P0_4); //sda scl
SerialLcd lcd(P0_19,P0_18);
+ #define PwmNum 4
#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 };
+// 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);
+// PulseWidthCounter ch[5] = { dp9,dp10,dp11,dp13,dp26 };
+ PwmOut pwm[4] = { dp1,dp2,dp18,dp24 };
+// SoftPWM buzz(dp25);
I2cPeripherals i2c(dp5,dp27); //sda scl
SerialLcd lcd(dp16,dp15);
+ #define PwmNum 4
+ #define MEM_SIZE 256
+ #define EXTERNAL_EEPROM
+#elif defined(TARGET_TEENSY3_1) // Teensy3.1
+ DigitalOut led1(D13);
+ InterruptIn ch1(D2);
+ PwmOut pwm[6] = { D3,D4,D5,D6,d20,D21 };
+ I2cPeripherals i2c(D18,D19); //sda scl
+ SerialLcd lcd(D3,D4); //TX,RX
+ #define PwmNum 6
#define MEM_SIZE 256
#define EXTERNAL_EEPROM
#endif
@@ -99,13 +149,13 @@
Timer CurTime;
//Timer ElapTime;
Timer CycleTime;
-Timer FlyghtTime;
+//Timer FlyghtTime;
config conf;
//PID pid[4];
-Limiter throLimit = 100;
-Limiter gyroLimit[3] = {300,300,300};
-Limiter accLimit[3] = {0.5,0.5,0.5};
-Limiter pwmLimit[4] = {50,50,50,50};
+//Limiter throLimit = 100;
+Limiter gyroLimit[3] = {0.9,0.9,0.9};
+Limiter accLimit[3] = {0.95,0.95,0.95};
+//Limiter pwmLimit[4] = {50,50,50,50};
//PID height;
float TotalTime = 0;;
int channel = 0;
@@ -153,7 +203,7 @@
float Rdata;
void initialize();
-void FlashLED(int );
+void FlashLED(int ,float tm=0.1);
void SetUp();
void SetUpPrompt(config&,I2cPeripherals&);
void PWM_Out(bool);
@@ -172,12 +222,8 @@
void LCD_printf(char *);
void LCD_cls();
void LCD_locate(int,int);
-#ifdef SOFTPWM
-void Tpwm_interrupt()
-{
- for ( int i=0; i<4; i++ ) pwm[i].start();
-}
-#endif
+void wait(float);
+
void PulseCheck() //cppm信号のチェック
{
channel++;
@@ -206,6 +252,18 @@
initialize();
Get_Stick_Pos();
+#ifdef DEBUG
+ char str[18];
+ while ( THR > 1800 ) {
+ LCD_locate(0,0);
+ sprintf(str,"TR=%4d,AL=%4d",THR,AIL);
+ LCD_printf(str);
+ LCD_locate(0,1);
+ sprintf(str,"EL=%4d,RD=%4d",ELE,RUD);
+ LCD_printf(str);
+ Get_Stick_Pos();
+ }
+#endif
while ( Stick[COL] > Thro_Zero || conf.StartMode == 'C'
|| ( conf.Model_Type == Quad_3D && Stick[GAIN] < 0 ) ) //Shrottol Low
{
@@ -231,12 +289,12 @@
{
if ( i > 100 ) //wait 2 sec
{
- FlyghtTime.stop();
+// FlyghtTime.stop();
if ( Stick[PIT] < -Stick_Limit ) { //Elevetor Down
loop_cnt = 0;
FlashLED(5);
- for ( int x=0; x<6; x++ ) {
+ for ( int x=0; x<PwmNum; x++ ) {
pwm[x].pulsewidth_us(PWM_Init[conf.Model_Type][x]);
}
i2c.start(conf.LCD_Contrast);
@@ -258,39 +316,38 @@
void initialize()
{
- buzz.period_us(400);
i2c.start(conf.LCD_Contrast);
- for ( int i=0;i<6;i++ ) pwm[i].pulsewidth_us(0);
+ for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
ReadConfig(); //config.inf file read
channel = 0;
+//#if defined(TARGET_LPC1114) // LPC1114
+
ch1.rise(&PulseCheck); //input pulse count
wait(0.2);
if ( channel > 50 ) {
+ FlashLED(50,0.02);
ch1.rise(&PulseAnalysis);
- InPulseMode = 'S';
- }
+ InPulseMode = 'S'; }
else InPulseMode = 'P';
+//#endif
led1 = 0;
CycleTime.start();
- throLimit.differential(conf.Thro_Limit_Val);
- throLimit.rate(conf.Thro_Limit_Rate);
-#ifdef SOFTPWM
- Tpwm.attach_us(&Tpwm_interrupt,conf.PWM_Interval);
-#endif
- Base_Press = (float)i2c.pressure() / 4096;
- FlashLED(3);
+// throLimit.differential(conf.Thro_Limit_Val);
+// throLimit.rate(conf.Thro_Limit_Rate);
+// Base_Press = (float)i2c.pressure() / 4096;
+ FlashLED(10,0.05);
}
-void FlashLED(int cnt)
+void FlashLED(int cnt,float tm)
{
for ( int i = 0 ; i < cnt ; i++ ) {
led1 = !led1;
- buzz = 0.5f;
- wait(0.1);
+// buzz = 0.5f;
+ wait(tm);
led1 = !led1;
- buzz = 0.0f;
- wait(0.1);
+// buzz = 0.0f;
+ wait(tm);
}
}
@@ -321,32 +378,36 @@
char *send;
char *recv;
int i;
+ char buf[MEM_SIZE];
config *conf_ptr;
if ( sizeof(config) > MEM_SIZE ) {
- LCD_printf("config size over");
+// pc.printf("config size over");
wait(3);
return;
}
//#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) || defined(EXTERNAL_EEPROM) || defined(STM32_EEPROM)
#if defined(INTERNAL_EEPROM)
iap.read_eeprom( (char*)TARGET_EEPROM_ADDRESS, buf, MEM_SIZE );
- #else
+ #elif defined(EXTERNAL_EEPROM)
//External Flash Memory Wreite
short pos = 0;
if ( i2c.read_EEPROM(pos,buf,MEM_SIZE) != 0 ) {
while(1) {
FlashLED(3);
wait(0.5);
-return;
}
}
+ #else //STM32 emulate eeprom
+// EEPROM_Read(0,buf,MEM_SIZE)
+ readEEPROM(0,(uint32_t*)buf,sizeof(config));
#endif
send = buf;
+ conf_ptr = (config*)buf;
+//pc.printf("rev=%f",conf_ptr->Revision);
recv = (char*)&conf;
- conf_ptr = (config*)buf;
+// 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;
@@ -376,11 +437,11 @@
fwrite(&conf,1,sizeof(config),fp);
fclose(fp);
#else
- char mem[MEM_SIZE];
+ char mem[MEM_SIZE]= " ";;
char *send;
int i;
if ( sizeof(config) > MEM_SIZE ) {
- LCD_printf("config size over");
+// pc.printf("config size over");
wait(3);
return;
}
@@ -388,26 +449,38 @@
for ( i=0;i<sizeof(config);i++ ) mem[i] = send[i];
for ( i=sizeof(config);i<MEM_SIZE;i++ ) mem[i] = 0x00;
//#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)
+ #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
+ short pos = 0;
+ i2c.write_EEPROM( pos,mem,MEM_SIZE) ;
+ #elif defined(STM32_EEPROM)
+// EEPROM_Write(0,buf,MEM_SIZE);
+ enableEEPROMWriting();
+ writeEEPROM(0, (uint32_t *)mem,sizeof(config));
+ disableEEPROMWriting();
+// pc.printf("rev=%f,rev=%d",((config*)mem)->Revision,readEEPROMWord(0));
+ #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
+ for ( i=0; i<4; i++ ) {
+ FlashLED(10,0.03);
+ wait(0.5);
+ }
#endif
}
void Get_Stick_Pos(void)
{
+#ifndef TARGET_LPC1114 // LPC1114
if ( InPulseMode == 'P' ) {
for (int i=0;i<5;i++) CH[i] = ch[i].count;
}
+#endif
// Stick_Save[ROL] = Stick[ROL];
// Stick_Save[PIT] = Stick[PIT];
// Stick_Save[YAW] = Stick[YAW];
@@ -543,8 +616,8 @@
TotalTime += interval;
if ( TotalTime > 0.5f ) {
led1 = !led1;
- if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5;
- else buzz=0.0;
+ // if ( ( !buzz ) && ( (float)conf.Flight_Time < FlyghtTime.read() ) ) buzz=0.5;
+ // else buzz=0.0;
TotalTime = 0;
}
@@ -664,7 +737,7 @@
break;
}
j = conf.Model_Type;
- for ( i=0; i<6; i++ ) {
+ for ( i=0; i<PwmNum; i++ ) {
if ( M[i] > Pulse_Max ) M[i] = Pulse_Max;
if ( M[i] < Pulse_Min ) M[i] = Pulse_Min;
if ( Servo_idx[j][i] == 1 )
@@ -685,7 +758,7 @@
}
if ( mode ) {
// h = conf.Stick_Ref[THR];
- for ( i=0;i<6;i++ ) {
+ for ( i=0;i<PwmNum;i++ ) {
// while ( !pwmpin[i] );
if ( conf.PWM_Mode == 1 )
pwm[i].pulsewidth_us(M[i]);
@@ -699,7 +772,7 @@
void ESC_SetUp(void) {
while(1) {
Get_Stick_Pos();
- for ( int i=0;i<4;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
+ for ( int i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(conf.Stick_Ref[COL]+Stick[COL]);
wait(0.015);
}
}
@@ -707,22 +780,17 @@
void Flight_SetUp(void)
{
int i;
-#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++ ) {
+ for ( i=0;i<PwmNum;i++ ) pwm[i].pulsewidth_us(0);
+ for ( i=0;i<PwmNum;i++ ) pwm[i].period_us(conf.PWM_Interval);
+ for ( i=0; i<PwmNum; i++ ) {
pwm[i].pulsewidth_us(PWM_Init[conf.Model_Type][i]);
}
hov_control = false;
- throLimit.differential(conf.Thro_Limit_Val);
- throLimit.rate(conf.Thro_Limit_Rate);
+// throLimit.differential(conf.Thro_Limit_Val);
+// throLimit.rate(conf.Thro_Limit_Rate);
Angle[ROL]=Angle[PIT]=Angle[YAW]=0;
loop_cnt = 0;
- FlyghtTime.start();
+// FlyghtTime.start();
CycleTime.start();
pid_interval = 0;
Stick_Save[COL] = Stick[COL];
@@ -743,8 +811,13 @@
void LCD_printf(char* str)
{
- lcd.printf(str);
- i2c.printf(str);
+ lcd.write(str);
+ i2c.write_lcd(str);
+}
+
+void wait(float tm)
+{
+ wait_us(tm*1000000);
}
;