/*!
Call this once with the node ID (0-31), frequency band (0-3), and
optional group (0-255 for RF12B, only 212 allowed for RF12).
*/
void rf12_initialize () {
spi_initialize();
pinMode(RFM_IRQ, INPUT);
digitalWrite(RFM_IRQ, 1); // pull-up
rf12_xfer(0x0000); // intitial SPI transfer added to avoid power-up problem
rf12_xfer(RF_SLEEP_MODE); // DC (disable clk pin), enable lbd
// wait until RFM12B is out of power-up reset, this takes several *seconds*
rf12_xfer(RF_TXREG_WRITE); // in case we're still in OOK mode
while (digitalRead(RFM_IRQ) == 0)
rf12_xfer(0x0000);
rf12_xfer(0x80C7 | (RF12_433MHZ << 4)); // EL (ena TX), EF (ena RX FIFO), 12.0pF
rf12_xfer(0xA640); // 868MHz
rf12_xfer(0xC606); // approx 49.2 Kbps, i.e. 10000/29/(1+6) Kbps
rf12_xfer(0x94A2); // VDI,FAST,134kHz,0dBm,-91dBm
rf12_xfer(0xC2AC); // AL,!ml,DIG,DQD4
rf12_xfer(0xCA8B); // FIFO8,1-SYNC,!ff,DR
rf12_xfer(0xCE2D); // SYNC=2D；
rf12_xfer(0xC483); // @PWR,NO RSTRIC,!st,!fi,OE,EN
rf12_xfer(0x9850); // !mp,90kHz,MAX OUT
rf12_xfer(0xCC77); // OB1，OB0, LPX,！ddy，DDIT，BW0
rf12_xfer(0xE000); // NOT USE
rf12_xfer(0xC800); // NOT USE
rf12_xfer(0xC049); // 1.66MHz,3.1V
rxstate = TXIDLE;
attachInterrupt(0, rf12_interrupt, LOW);
}
/**
* @brief Initialize the system and interrupts
* @return Void
*/
void initialize_system(void)
{
timer_interrupt = false; // Clear the timer interrupt flag
playing_sound = false;
message_count = 0;
valid_message = true;
PORTA = 0x00;
DDRA = 0xFF;
PORTA = 0x00;
// Set the data direction register values
DDRD |= _BV(5) | _BV(6) | _BV(7);
TCCR1A = 0x00;
TCCR1B = 0x0D;
OCR1A = 390; // 1s interval
TIMSK1 |= (1<<OCIE1A); // Enable interrupt
init_usart_pc();
log_msg("SABT initialising...\n\r");
log_msg("Setting flags...OK\n\r");
log_msg("PC USART...OK\n\r");
log_msg("Keypad USART...");
init_usart_keypad();
log_msg("OK\n\r");
log_msg("SPI...");
spi_initialize();
log_msg("OK\n\r");
log_msg("Interrupt flag...");
sei(); // sets the interrupt flag (enables interrupts)
log_msg("OK\n\r");
init_sd_card(true);
log_msg("SD card...OK\n\r");
play_mp3_file((unsigned char*)"SYS_WELC.mp3");
ui_check_modes();
log_msg("Parsing modes...OK\n\r");
log_msg("Type info\n\r");
log_msg("char: %d bytes\n\r", sizeof(char));
log_msg("int: %d bytes\n\r", sizeof(int));
log_msg("short: %d bytes\n\r", sizeof(short));
log_msg("long: %d bytes\n\r", sizeof(long));
log_msg("void*: %d bytes\n\r", sizeof(void*));
play_mp3("SYS_","MENU");
}
int do_sd_initialize (sd_context_t *sdc)
{
/* Initialize the SPI controller */
spi_initialize();
/* Set the maximum SPI clock rate possible */
spi_set_divisor(PERIPH_CLOCKRATE/400000);
/* Initialization OK? */
if (sd_initialize(sdc) != 1)
return 0;
spi_set_divisor(2); //---- 2011 0905 spi full speed
return 1;
}
void setup_lcd_fs(void){
_disable_interrupts();
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
BCSCTL1 = CALBC1_16MHZ; /* Set DCO for 16 MHz */
DCOCTL = CALDCO_16MHZ;
delay_ms(100);
// Initialize SD card:
spi_initialize();
dres = disk_initialize();
tft_init_hw();
tft_begin();
tft_fillScreen(ILI9340_WHITE);
tft_setCursor(0, 1);
tft_setTextColor(ILI9340_BLACK);
tft_setTextSize(1);
if (dres){
sprintf(buffer, "Disk error = %d\n", (int) dres);
tft_writeString(buffer);
while (1);
}
pres = pf_mount(&fs);
if (pres){
sprintf(buffer, "PFF error 1 = %d\n", (int) pres);
tft_writeString(buffer);
while (1);
}
tft_fillScreen(ILI9340_WHITE);
P1DIR |= BIT0 | BIT1;
P1OUT &= ~ (BIT0 | BIT1);
}
struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
unsigned int max_hz, unsigned int mode)
{
#ifdef CONFIG_AML_SPICC
if (bus == BUS_SPICC) {
return spicc_setup_slave(bus, cs, max_hz, mode);
}
#endif
struct aml_spi_slave * amls;
amls = ( struct aml_spi_slave *)malloc(sizeof(struct aml_spi_slave));
if (!amls)
return NULL;
spi_initialize();
amls->slave.bus = bus;
amls->slave.cs = cs;
amls->adr_base =(void*)0xCC000000;
amls->mode=mode;
return &amls->slave;
}
/*!
* @brief Initializes the whole system and runs the desired application
*
* This is the main function of the project. It calls initialization functions
* of the MCU and the sensors. In the infinite loop it repeatedly checks
* the USART module read buffer and Streams sensor data periodically (100 ms) via USART.
*
*/
int main(void)
{
/********************* Initialize global variables **********************/
bmf055_input_state = USART_INPUT_STATE_PRINT_DATA;
/************************* Initializations ******************************/
/*Initialize SAMD20 MCU*/
system_init();
/*Initialize clock module of SAMD20 MCU - Internal RC clock*/
//clock_initialize(); // done via conf_clocks.h --> ASF
/*SPI master for communicating with sensors*/
spi_initialize();
/*eeprom emulator for configuration storage */
eeprom_emulator_initialize();
/*Initialize timers */
tc_initialize();
/*Initialize UART for communication with PC*/
usart_initialize();
/*Enable the system interrupts*/
system_interrupt_enable_global();/* All interrupts have a priority of level 0 which is the highest. */
/* Initialize the sensors */
bmf055_sensors_initialize();
readEEPROM();
checkFirstTime(0);
//readEEPROM();
configureReceiver();
initSensors();
previousTime = micros();
calibratingG = 400;
f.SMALL_ANGLES_25=1; // important for gyro only conf
if(conf.copterType == 0){//0=Bi,1=Tri,2=QUADP,3=QUADX,4=Y4,5=Y6,6=H6P,7=H6X,8=Vtail4
MULTITYPE = 4;
NUMBER_MOTOR = 2;
}
if(conf.copterType == 1){
MULTITYPE = 1;
NUMBER_MOTOR = 3;
}
if(conf.copterType == 2){
MULTITYPE = 2;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 3){
MULTITYPE = 3;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 4){
MULTITYPE = 9;
NUMBER_MOTOR = 4;
}
if(conf.copterType == 5){
MULTITYPE = 6;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 6){
MULTITYPE = 7;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 7){
MULTITYPE = 10;
NUMBER_MOTOR = 6;
}
if(conf.copterType == 8){
MULTITYPE = 17;
NUMBER_MOTOR = 4;
}
initOutput();
/************************** Infinite Loop *******************************/
while (true)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t beepon = 0;
uint8_t axis,i;
int16_t error,errorAngle;
int16_t delta,deltaSum;
int16_t PTerm=0,ITerm=0,PTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0,DTerm=0;
static int16_t lastGyro[3] = {0,0,0};
static int16_t delta1[3],delta2[3];
static int16_t errorGyroI[3] = {0,0,0};
static int16_t errorAngleI[2] = {0,0};
static uint32_t rcTime = 0;
static uint32_t BeepTime = 0;
static uint8_t stickarmed = 0;
//static int16_t initialThrottleHold;
if(!rcOptions[BOXARM] && stickarmed == 0 && f.ARMED == 0){
if(rcData[YAW]<conf.MINCHECK && rcData[ROLL]>conf.MAXCHECK){
conf.calibState=1;
writeParams(1);
while(true){
//blinkLED(10,30,1);
}
}
}
while(SetupMode == 1){
checkSetup();
}
if(conf.RxType == 1 || conf.RxType == 2){
if (rcFrameComplete) computeRC();
}
if(!rcOptions[BOXARM] && stickarmed == 0) {
f.ARMED = 0;
}
if (currentTime > rcTime ) { // 50Hz
rcTime = currentTime + 20000;
if(failsave < 250)failsave++;
debug[0] = failsave;
if(conf.RxType != 1 && conf.RxType != 2){
computeRC();
}
if ((rcData[THROTTLE] < conf.MINCHECK && s3D == 0) || (rcData[THROTTLE] > (1500-conf.MIDDLEDEADBAND) && rcData[THROTTLE] < (1500+conf.MIDDLEDEADBAND) && s3D == 1 && f.ARMED == 0)) {
errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
rcDelayCommand++;
if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK && !f.ARMED) {
if (rcDelayCommand == 20 && failsave < 20) {
calibratingG=400;
}
}else if (rcData[YAW] > conf.MAXCHECK && rcData[PITCH] > conf.MAXCHECK && !f.ARMED) {
if (rcDelayCommand == 20) {
previousTime = micros();
}
}else if (conf.activate[BOXARM] > 0) {
if ( rcOptions[BOXARM] && f.OK_TO_ARM && good_calib) {
f.ARMED = 1;
stickarmed = 0;
} else if (f.ARMED) f.ARMED = 0;
rcDelayCommand = 0;
} else if ( (rcData[YAW] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[YAW] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
if (rcDelayCommand == 20 && good_calib) {
f.ARMED = 1;
stickarmed = 1;
}
} else if ( (rcData[ROLL] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
} else if ( (rcData[ROLL] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
if (rcDelayCommand == 20 && good_calib) {
f.ARMED = 1;
stickarmed = 1;
}
} else
rcDelayCommand = 0;
} else if (rcData[THROTTLE] > conf.MAXCHECK && !f.ARMED) {
if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK) { // throttle=max, yaw=left, pitch=min
if (rcDelayCommand == 20) calibratingA=400;
rcDelayCommand++;
} else if (rcData[PITCH] > conf.MAXCHECK) {
conf.angleTrim[PITCH]+=2;writeParams(1);
} else if (rcData[PITCH] < conf.MINCHECK) {
conf.angleTrim[PITCH]-=2;writeParams(1);
} else if (rcData[ROLL] > conf.MAXCHECK) {
conf.angleTrim[ROLL]+=2;writeParams(1);
} else if (rcData[ROLL] < conf.MINCHECK) {
conf.angleTrim[ROLL]-=2;writeParams(1);
} else {
rcDelayCommand = 0;
}
}
uint16_t auxState = 0;
for(i=0;i<4;i++)
auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
for(i=0;i<CHECKBOXITEMS;i++)
rcOptions[i] = (auxState & conf.activate[i])>0;
if(failsave > 200 && f.ARMED){
rcOptions[BOXACC] = 1;
s3D = 0;
rcData[THROTTLE] = 1190;
rcCommand[THROTTLE] = 1190;
}
if (rcOptions[BOXACC] && s3D == 0) {
// bumpless transfer to Level mode
if (!f.ACC_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.ACC_MODE = 1;
}
} else {
// failsafe support
f.ACC_MODE = 0;
}
if (rcOptions[BOXBEEP]) {
f.FSBEEP = 1;
if(currentTime > BeepTime){
BeepTime = currentTime + 50000;
if(beepon == 0){
if(conf.RxType == 0){
//digitalWrite(A2,HIGH);
}else{
//digitalWrite(8,HIGH);
}
beepon = 1;
}else{
if(conf.RxType == 0){
//digitalWrite(A2,LOW);
}else{
//digitalWrite(8,LOW);
}
beepon = 0;
}
}
} else {
f.FSBEEP = 0;
if(conf.RxType == 0){
//digitalWrite(A2,LOW);
}else{
//digitalWrite(8,LOW);
}
}
if (rcOptions[BOXHORIZON] && s3D == 0) {
// bumpless transfer to Horizon mode
if (!f.HORIZON_MODE) {
errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
f.HORIZON_MODE = 1;
}
} else {
f.HORIZON_MODE = 0;
}
if (rcOptions[BOX3D] && conf.F3D == 1) {
if(f.ARMED == 0 && s3D == 0){
s3D = 1;
f.ACC_MODE = 0;
f.HORIZON_MODE = 0;
}
} else if(f.ARMED == 0){
s3D = 0;
}
if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
}
computeIMU();
int16_t prop;
if (f.HORIZON_MODE)
prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500]
if (f.ACC_MODE){
if(Zadd > 0)Zadd--;
if(Zadd < 0)Zadd++;
}else{
Zadd = 0;
}
//**** PITCH & ROLL & YAW PID ****
for(axis=0;axis<3;axis++) {
if ((f.ACC_MODE || f.HORIZON_MODE) && axis<2 ) { //LEVEL MODE
// 50 degrees max inclination
errorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
#ifdef LEVEL_PDF
PTermACC = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
#else
PTermACC = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
#endif
PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITermACC = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
}
if ( !f.ACC_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
if (abs(rcCommand[axis])<350) error = rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000 16 bits is ok for result if P8>2 (P>0.2)
else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here
if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis]/125*conf.I8[axis])>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if ( f.HORIZON_MODE && axis<2) {
PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500;
ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500;
} else {
if ( f.ACC_MODE && axis<2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
if (abs(gyroData[axis])<160) PTerm -= gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000 16 bits is ok for result
else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5; // 16 bits is needed for calculation 640*50 = 32000 16 bits is ok for result
else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5; // 32 bits is needed for calculation
axisPID[axis] = PTerm + ITerm - DTerm;
}
