void motorsThrottleCalibration() { // flash LED 3 times for(uint8_t i = 0;i < 3;i++) { LED = 1; _delay_ms(25); LED = 0; _delay_ms(25); } while(1) { RxGetChannels(); #ifdef SINGLE_COPTER MotorOut1 = RxInCollective; MotorOut2 = 1400; // Center: 140 MotorOut3 = 1400; MotorOut4 = 1400; MotorOut5 = 1400; #elif defined(DUAL_COPTER) MotorOut1 = RxInCollective; MotorOut2 = RxInCollective; MotorOut3 = 500; // Center: 50 MotorOut4 = 500; #elif defined(TWIN_COPTER) MotorOut1 = RxInCollective; MotorOut2 = RxInCollective; MotorOut3 = 500; // Center: 50 MotorOut4 = 500; MotorOut5 = 500; MotorOut6 = 500; // Center: 50, Reverse #elif defined(TRI_COPTER) MotorOut1 = RxInCollective; MotorOut2 = RxInCollective; MotorOut3 = RxInCollective; MotorOut4 = 500+RxInYaw*2; // Center: 50 #elif defined(QUAD_COPTER) || defined(QUAD_X_COPTER) || defined(Y4_COPTER) MotorOut1 = RxInCollective; MotorOut2 = RxInCollective; MotorOut3 = RxInCollective; MotorOut4 = RxInCollective; #elif defined(HEX_COPTER) || defined(Y6_COPTER) MotorOut1 = RxInCollective; MotorOut2 = RxInCollective; MotorOut3 = RxInCollective; MotorOut4 = RxInCollective; MotorOut5 = RxInCollective; MotorOut6 = RxInCollective; #else #error No Copter configuration defined !!!! #endif output_motor_ppm(); // this regulates rate at which we output signals } }
void Display_rcinput(void) { while(BUTTON1 != 0) { if (BUTTON4 == 0) { //_delay_ms(100); CenterSticks(); } if (BUTTON3 == 0) { //_delay_ms(100); SetFailsafe(); } RxGetChannels(); LCD_Display_Text(241,(prog_uchar*)Verdana8,0,0); LCD_Display_Text(32,(prog_uchar*)Verdana8,0,10); LCD_Display_Text(242,(prog_uchar*)Verdana8,0,20); LCD_Display_Text(35,(prog_uchar*)Verdana8,0,30); LCD_Display_Text(109,(prog_uchar*)Verdana8,70,0); LCD_Display_Text(110,(prog_uchar*)Verdana8,70,10); LCD_Display_Text(111,(prog_uchar*)Verdana8,70,20); LCD_Display_Text(112,(prog_uchar*)Verdana8,70,30); mugui_lcd_puts(itoa(RCinputs[THROTTLE],pBuffer,10),(prog_uchar*)Verdana8,37,0); mugui_lcd_puts(itoa(RCinputs[AILERON],pBuffer,10),(prog_uchar*)Verdana8,37,10); mugui_lcd_puts(itoa(RCinputs[ELEVATOR],pBuffer,10),(prog_uchar*)Verdana8,37,20); mugui_lcd_puts(itoa(RCinputs[RUDDER],pBuffer,10),(prog_uchar*)Verdana8,37,30); mugui_lcd_puts(itoa(RCinputs[GEAR],pBuffer,10),(prog_uchar*)Verdana8,100,0); mugui_lcd_puts(itoa(RCinputs[AUX1],pBuffer,10),(prog_uchar*)Verdana8,100,10); mugui_lcd_puts(itoa(RCinputs[AUX2],pBuffer,10),(prog_uchar*)Verdana8,100,20); mugui_lcd_puts(itoa(RCinputs[AUX3],pBuffer,10),(prog_uchar*)Verdana8,100,30); // Print bottom text and markers LCD_Display_Text(12, (prog_uchar*)Wingdings, 0, 57); // Left LCD_Display_Text(44, (prog_uchar*)Verdana8, 40, 55); // Failsafe LCD_Display_Text(9, (prog_uchar*)Wingdings, 80, 59); // Down LCD_Display_Text(60, (prog_uchar*)Verdana8, 100, 55); // Cal. LCD_Display_Text(9, (prog_uchar*)Wingdings, 119, 59); // Down // Update buffer write_buffer(); clear_buffer(); _delay_ms(100); } // Exit }
void Display_rcinput(void) { // Re-enable interrupts. High speed mode may have left them off init_int(); while(BUTTON1 != 0) { if (BUTTON4 == 0) { CenterSticks(); } if (BUTTON3 == 0) { SetFailsafe(); } RxGetChannels(); LCD_Display_Text(19,(const unsigned char*)Verdana8,0,0); LCD_Display_Text(32,(const unsigned char*)Verdana8,0,10); LCD_Display_Text(20,(const unsigned char*)Verdana8,0,20); LCD_Display_Text(35,(const unsigned char*)Verdana8,0,30); LCD_Display_Text(109,(const unsigned char*)Verdana8,70,0); LCD_Display_Text(110,(const unsigned char*)Verdana8,70,10); LCD_Display_Text(111,(const unsigned char*)Verdana8,70,20); LCD_Display_Text(112,(const unsigned char*)Verdana8,70,30); mugui_lcd_puts(itoa(MonopolarThrottle,pBuffer,10),(const unsigned char*)Verdana8,37,0); mugui_lcd_puts(itoa(RCinputs[AILERON],pBuffer,10),(const unsigned char*)Verdana8,37,10); mugui_lcd_puts(itoa(RCinputs[ELEVATOR],pBuffer,10),(const unsigned char*)Verdana8,37,20); mugui_lcd_puts(itoa(RCinputs[RUDDER],pBuffer,10),(const unsigned char*)Verdana8,37,30); mugui_lcd_puts(itoa(RCinputs[GEAR],pBuffer,10),(const unsigned char*)Verdana8,100,0); mugui_lcd_puts(itoa(RCinputs[AUX1],pBuffer,10),(const unsigned char*)Verdana8,100,10); mugui_lcd_puts(itoa(RCinputs[AUX2],pBuffer,10),(const unsigned char*)Verdana8,100,20); mugui_lcd_puts(itoa(RCinputs[AUX3],pBuffer,10),(const unsigned char*)Verdana8,100,30); // Print bottom text and markers LCD_Display_Text(12, (const unsigned char*)Wingdings, 0, 57); // Left LCD_Display_Text(21, (const unsigned char*)Verdana8, 40, 55); // Failsafe LCD_Display_Text(9, (const unsigned char*)Wingdings, 80, 59); // Down LCD_Display_Text(60, (const unsigned char*)Verdana8, 100, 55); // Cal. LCD_Display_Text(9, (const unsigned char*)Wingdings, 119, 59); // Down // Update buffer write_buffer(buffer); clear_buffer(buffer); } }
void Display_rcinput(void) { while(BUTTON1 != 0) { if (BUTTON4 == 0) { CenterSticks(); } RxGetChannels(); LCD_Display_Text(114,(const unsigned char*)Verdana8,0,0); LCD_Display_Text(32,(const unsigned char*)Verdana8,0,10); LCD_Display_Text(115,(const unsigned char*)Verdana8,0,20); LCD_Display_Text(35,(const unsigned char*)Verdana8,0,30); LCD_Display_Text(109,(const unsigned char*)Verdana8,70,0); LCD_Display_Text(110,(const unsigned char*)Verdana8,70,10); LCD_Display_Text(111,(const unsigned char*)Verdana8,70,20); LCD_Display_Text(112,(const unsigned char*)Verdana8,70,30); mugui_lcd_puts(itoa(MonopolarThrottle,pBuffer,10),(const unsigned char*)Verdana8,37,0); mugui_lcd_puts(itoa(RCinputs[AILERON],pBuffer,10),(const unsigned char*)Verdana8,37,10); mugui_lcd_puts(itoa(RCinputs[ELEVATOR],pBuffer,10),(const unsigned char*)Verdana8,37,20); mugui_lcd_puts(itoa(RCinputs[RUDDER],pBuffer,10),(const unsigned char*)Verdana8,37,30); mugui_lcd_puts(itoa(RCinputs[GEAR],pBuffer,10),(const unsigned char*)Verdana8,100,0); mugui_lcd_puts(itoa(RCinputs[AUX1],pBuffer,10),(const unsigned char*)Verdana8,100,10); mugui_lcd_puts(itoa(RCinputs[AUX2],pBuffer,10),(const unsigned char*)Verdana8,100,20); mugui_lcd_puts(itoa(RCinputs[AUX3],pBuffer,10),(const unsigned char*)Verdana8,100,30); // Print bottom text and markers LCD_Display_Text(12, (const unsigned char*)Wingdings, 0, 57); // Left LCD_Display_Text(60, (const unsigned char*)Verdana8, 100, 55); // Cal. LCD_Display_Text(9, (const unsigned char*)Wingdings, 119, 59); // Down // Update buffer write_buffer(buffer,1); clear_buffer(buffer); _delay_ms(100); } // Exit }
void init(void) { //*********************************************************** // I/O setup //*********************************************************** // Set port directions DDRA = 0x00; // Port A DDRB = 0x0A; // Port B DDRC = 0xFF; // Port C DDRD = 0xF2; // Port D MOTORS = 0; // Hold all PWM outputs low to stop glitches // Preset I/O pins LED1 = 0; // LED1 off LVA = 0; // LVA alarm OFF LCD_CSI = 1; LCD_SCL = 1; LCD_RES = 1; // Set/clear pull-ups (1 = set, 0 = clear) PINB = 0xF5; // Set PB pull-ups PIND = 0x0D; // Set PD pull-ups //*********************************************************** // Timers //*********************************************************** // Timer0 (8bit) - run @ 20MHz (50ns) - max 12.8us // Fast timer for small, precise interval timing TCCR0A = 0; // Normal operation TCCR0B = (1 << CS00); // Clk / 1 = 20MHz = 50ns TIMSK0 = 0; // No interrupts // Timer1 (16bit) - run @ 2.5MHz (400ns) - max 26.2ms // Used to measure Rx Signals & control ESC/servo output rate TCCR1A = 0; TCCR1B = (1 << CS11); // Clk/8 = 2.5MHz // Timer2 8bit - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms // Used to time arm/disarm intervals TCCR2A = 0; TCCR2B = 0x07; // Clk/1024 = 19.531kHz TIMSK2 = 0; TIFR2 = 0; TCNT2 = 0; // Reset counter //*********************************************************** // Interrupts and pin function setup //*********************************************************** // Pin change interrupt enables PCINT1, PCINT2 and PCINT3 (Throttle, Aux and CPPM input) PCICR = 0x0A; // PCINT8 to PCINT15 (PCINT1 group - AUX) // PCINT24 to PCINT31 (PCINT3 group - THR) PCMSK1 |= (1 << PCINT8); // PB0 (Aux pin change mask) PCMSK3 |= (1 << PCINT24); // PD0 (Throttle pin change mask) PCIFR = 0x0F; // Clear PCIF0 interrupt flag // Clear PCIF1 interrupt flag // Clear PCIF2 interrupt flag // Clear PCIF3 interrupt flag // External interrupts INT0 (Elevator) and INT1 (Aileron) and INT2 (Rudder) EIMSK = 0x07; // Enable INT0 (Elevator input) // Enable INT1 (Aileron input) // Enable INT2 (Rudder/CPPM input) EICRA = 0x15; // Any change INT0 // Any change INT1 // Any change INT2 EIFR = 0x07; // Clear INT0 interrupt flag (Elevator) // Clear INT1 interrupt flag (Aileron) // Clear INT2 interrupt flag (Rudder/CPPM) //*********************************************************** RC_Lock = false; // Preset important flags Failsafe = false; AutoLevel = false; Stability = false; FirstTimeIMU = true; // Button acceleration button_multiplier = 1; Initial_EEPROM_Config_Load(); // Loads config at start-up UpdateLimits(); // Update travel limts UpdateIMUvalues(); // Update IMU factors Init_ADC(); // Flash LED LED1 = 1; _delay_ms(150); LED1 = 0; // Initialise the GLCD st7565_init(); st7565_command(CMD_DISPLAY_ON); // Check (AF) st7565_command(CMD_SET_ALLPTS_NORMAL); // Check (A4) st7565_set_brightness(0x26); write_buffer(buffer,0); // Display logo _delay_ms(1000); clear_buffer(buffer); // Clear write_buffer(buffer,1); st7565_command(CMD_SET_COM_NORMAL); // For text clear_buffer(buffer); // Clear // Reset I-terms IntegralGyro[ROLL] = 0; IntegralGyro[PITCH] = 0; IntegralGyro[YAW] = 0; // Calibrate gyros, hopefully after motion minimised CalibrateGyros(); //*********************************************************** //* Reload eeprom settings if all buttons are pressed //*********************************************************** if ((PINB & 0xf0) == 0) { LCD_Display_Text(1,(prog_uchar*)Verdana14,15,10); LCD_Display_Text(2,(prog_uchar*)Verdana14,31,30); write_buffer(buffer,1); clear_buffer(buffer); // Clear Set_EEPROM_Default_Config(); Save_Config_to_EEPROM(); } //*********************************************************** sei(); // Enable global Interrupts // Check to see that gyros are stable ReadGyros(); if ((gyroADC[ROLL] > GYROS_STABLE) || (gyroADC[ROLL] < -GYROS_STABLE) || (gyroADC[PITCH] > GYROS_STABLE) || (gyroADC[PITCH] < -GYROS_STABLE) || (gyroADC[YAW] > GYROS_STABLE) || (gyroADC[YAW] < -GYROS_STABLE)) { General_error |= (1 << SENSOR_ERROR); // Set sensor error bit } // Check to see that throttle is low if in CPPM mode if RC detected // Don't bother if in CamStab mode _delay_ms(100); if ((Config.RxMode == CPPM_MODE) && RC_Lock && (Config.CamStab == OFF)) { RxGetChannels(); if (RCinputs[THROTTLE] > 300) { General_error |= (1 << THROTTLE_HIGH); // Set throttle high error bit } } // Beep that all sensors have been handled menu_beep(1); } // init()
void init(void) { uint8_t i; bool updated; //*********************************************************** // I/O setup //*********************************************************** // Set port directions DDRA = 0x30; // Port A DDRB = 0x0A; // Port B DDRC = 0xFC; // Port C DDRD = 0xF2; // Port D // Hold all PWM outputs low to stop glitches // M5 and M6 are on PortA for KK2.1 MOTORS = 0; M5 = 0; M6 = 0; // Preset I/O pins LED1 = 0; // LED1 off LVA = 0; // LVA alarm OFF LCD_SCL = 1; // GLCD clock high // Set/clear pull-ups (1 = set, 0 = clear) PINB = 0xF5; // Set PB pull-ups PIND = 0x0C; // Set PD pull-ups (Don't pull up RX yet) //*********************************************************** // Spektrum receiver binding. Must be done immediately on power-up // // 3 low pulses: DSM2 1024/22ms // 5 low pulses: DSM2 2048/11ms // 7 low pulses: DSMX 1024/22ms // 9 low pulses: DSMX 2048/11ms //*********************************************************** PIND = 0x0C; // Release RX pull up on PD0 _delay_ms(63); // Pause while satellite wakes up // and pull-ups have time to rise. // Tweak until bind pulses about 68ms after power-up // Bind as master if any single button pressed. // NB: Have to wait until the button pull-ups rise before testing for a button press. // Button 1 if ((PINB & 0xf0) == 0x70) { DDRD = 0xF3; // Switch PD0 to output bind_master(3); } // Button 2 if ((PINB & 0xf0) == 0xb0) { DDRD = 0xF3; // Switch PD0 to output bind_master(5); } // Button 3 if ((PINB & 0xf0) == 0xd0) { DDRD = 0xF3; // Switch PD0 to output bind_master(7); } // Button 4 if ((PINB & 0xf0) == 0xE0) { DDRD = 0xF3; // Switch PD0 to output bind_master(9); } DDRD = 0xF2; // Reset Port D directions PIND = 0x0D; // Set PD pull-ups (now pull up RX as well) //*********************************************************** // Timers //*********************************************************** // Timer0 (8bit) - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms // Slow timer to extend Timer 1 TCCR0A = 0; // Normal operation TCCR0B = 0x05; // Clk / 1024 = 19.531kHz or 51.2us - max 13.1ms TIMSK0 |= (1 << TOIE0); // Enable interrupts TCNT0 = 0; // Reset counter // Timer1 (16bit) - run @ 2.5MHz (400ns) - max 26.2ms // Used to measure Rx Signals & control ESC/servo output rate TCCR1A = 0; TCCR1B |= (1 << CS11); // Clk/8 = 2.5MHz // Timer2 8bit - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms // Used to time arm/disarm intervals TCCR2A = 0; TCCR2B = 0x07; // Clk/1024 = 19.531kHz TIMSK2 = 0; TIFR2 = 0; TCNT2 = 0; // Reset counter //*********************************************************** // Interrupts and pin function setup //*********************************************************** // Pin change interrupt enables PCINT1, PCINT2 and PCINT3 (Throttle, AUX and CPPM input) PCICR = 0x0A; // PCINT8 to PCINT15 (PCINT1 group - AUX) // PCINT24 to PCINT31 (PCINT3 group - THR) PCIFR = 0x0F; // Clear PCIF0 interrupt flag // Clear PCIF1 interrupt flag // Clear PCIF2 interrupt flag // Clear PCIF3 interrupt flag // External interrupts INT0 (Elevator) and INT1 (Aileron) and INT2 (Rudder) EICRA = 0x15; // Any change INT0 // Any change INT1 // Any change INT2 EIFR = 0x07; // Clear INT0 interrupt flag (Elevator) // Clear INT1 interrupt flag (Aileron) // Clear INT2 interrupt flag (Rudder/CPPM) //*********************************************************** // Start up //*********************************************************** // Preset important flags Interrupted = false; // Load EEPROM settings updated = Initial_EEPROM_Config_Load(); // Config now contains valid values //*********************************************************** // RX channel defaults for when no RC connected // Not doing this can result in the FC trying (unsuccessfully) to arm // and makes entry into the menus very hard //*********************************************************** for (i = 0; i < MAX_RC_CHANNELS; i++) { RxChannel[i] = 3750; } RxChannel[THROTTLE] = 2500; // Min throttle //*********************************************************** // GLCD initialisation //*********************************************************** // Initialise the GLCD st7565_init(); // Make sure the LCD is blank without clearing buffer (and so no logo) clear_screen(); //*********************************************************** // ESC calibration //*********************************************************** // Calibrate ESCs if ONLY buttons 1 and 4 pressed if ((PINB & 0xf0) == 0x60) { // Display calibrating message st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) clear_buffer(buffer); LCD_Display_Text(59,(const unsigned char*)Verdana14,10,25); write_buffer(buffer); clear_buffer(buffer); // For each output for (i = 0; i < MAX_OUTPUTS; i++) { // Check for motor marker if (Config.Channel[i].Motor_marker == MOTOR) { // Set output to maximum pulse width ServoOut[i] = MOTOR_100; } else { ServoOut[i] = SERVO_CENTER; } } // Output HIGH pulse (1.9ms) until buttons released while ((PINB & 0xf0) == 0x60) { // Pass address of ServoOut array and select all outputs output_servo_ppm_asm(&ServoOut[0], 0xFF); // Loop rate = 20ms (50Hz) _delay_ms(20); } // Output LOW pulse (1.1ms) after buttons released // For each output for (i = 0; i < MAX_OUTPUTS; i++) { // Check for motor marker if (Config.Channel[i].Motor_marker == MOTOR) { // Set output to maximum pulse width ServoOut[i] = MOTOR_0; } } // Loop forever here while(1) { // Pass address of ServoOut array and select all outputs output_servo_ppm_asm(&ServoOut[0], 0xFF); // Loop rate = 20ms (50Hz) _delay_ms(20); } } //*********************************************************** // Reset EEPROM settings //*********************************************************** // This delay prevents the GLCD flashing up a ghost image of old data _delay_ms(300); // Reload default eeprom settings if middle two buttons are pressed if ((PINB & 0xf0) == 0x90) { // Display reset message st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) clear_buffer(buffer); LCD_Display_Text(262,(const unsigned char*)Verdana14,40,25); // "Reset" write_buffer(buffer); clear_buffer(buffer); // Reset EEPROM settings Set_EEPROM_Default_Config(); Save_Config_to_EEPROM(); // Set contrast to the default value st7565_set_brightness(Config.Contrast); _delay_ms(500); // Save is now too fast to show the "Reset" text long enough } // Display message in place of logo when updating eeprom structure if (updated) { st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) clear_buffer(buffer); LCD_Display_Text(259,(const unsigned char*)Verdana14,30,13); // "Updating" LCD_Display_Text(260,(const unsigned char*)Verdana14,33,37); // "settings" write_buffer(buffer); clear_buffer(buffer); _delay_ms(1000); } else { // Write logo from buffer write_buffer(buffer); _delay_ms(1000); } clear_buffer(buffer); write_buffer(buffer); st7565_init(); // Seems necessary for KK2 mini //*********************************************************** // i2c init //*********************************************************** i2c_init(); init_i2c_gyros(); init_i2c_accs(); //*********************************************************** // Remaining init tasks //*********************************************************** // Display "Hold steady" message clear_buffer(buffer); st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) LCD_Display_Text(263,(const unsigned char*)Verdana14,18,25); // "Hold steady" write_buffer(buffer); clear_buffer(buffer); // Do startup tasks Init_ADC(); init_int(); // Initialise interrupts based on RC input mode init_uart(); // Initialise UART // Initial gyro calibration if (!CalibrateGyrosSlow()) { clear_buffer(buffer); LCD_Display_Text(61,(const unsigned char*)Verdana14,25,25); // "Cal. failed" write_buffer(buffer); _delay_ms(1000); // Reset cli(); wdt_enable(WDTO_15MS); // Watchdog on, 15ms while(1); // Wait for reboot } // Update voltage detection SystemVoltage = GetVbat(); // Check power-up battery voltage UpdateLimits(); // Update travel and trigger limits // Disarm on start-up if Armed setting is ARMABLE if (Config.ArmMode == ARMABLE) { General_error |= (1 << DISARMED); // Set disarmed bit } // Check to see that throttle is low if RC detected if (Interrupted) { RxGetChannels(); if (MonopolarThrottle > THROTTLEIDLE) // THROTTLEIDLE = 50 { General_error |= (1 << THROTTLE_HIGH); // Set throttle high error bit } } // Reset IMU reset_IMU(); // Beep that init is complete LVA = 1; _delay_ms(25); LVA = 0; #ifdef ERROR_LOG // Log reboot add_log(REBOOT); #endif } // init()
void init(void) { //*********************************************************** // I/O setup //*********************************************************** // Set port directions // KK2.0 and KK2.1 are different #ifdef KK21 DDRA = 0x30; // Port A DDRC = 0xFC; // Port C #else DDRA = 0x00; // Port A DDRC = 0xFF; // Port C #endif DDRB = 0x0A; // Port B DDRD = 0xF2; // Port D // Hold all PWM outputs low to stop glitches // M5 and M6 are on PortA for KK2.1 MOTORS = 0; M5 = 0; M6 = 0; // Preset I/O pins LED1 = 0; // LED1 off LVA = 0; // LVA alarm OFF LCD_SCL = 1; // GLCD clock high // Set/clear pull-ups (1 = set, 0 = clear) PINB = 0xF5; // Set PB pull-ups PIND = 0x0C; // Set PD pull-ups (Don't pull up RX yet) //*********************************************************** // Spektrum receiver binding //*********************************************************** _delay_ms(63); // Pause while satellite wakes up // and pull-ups have time to rise. // Tweak until bind pulses about 68ms after power-up // Bind as master if ONLY button 4 pressed if ((PINB & 0xf0) == 0xE0) { DDRD = 0xF3; // Switch PD0 to output bind_master(); } DDRD = 0xF2; // Reset Port D directions // Set/clear pull-ups (1 = set, 0 = clear) PIND = 0x0D; // Set PD pull-ups (now pull up RX as well) //*********************************************************** // Timers //*********************************************************** // Timer0 (8bit) - run @ 20MHz (50ns) - max 12.8us // Fast timer for small, precise interval timing TCCR0A = 0; // Normal operation TCCR0B = (1 << CS00); // Clk / 1 = 20MHz = 50ns TIMSK0 = 0; // No interrupts // Timer1 (16bit) - run @ 2.5MHz (400ns) - max 26.2ms // Used to measure Rx Signals & control ESC/servo output rate TCCR1A = 0; TCCR1B = (1 << CS11); // Clk/8 = 2.5MHz // Timer2 8bit - run @ 20MHz / 1024 = 19.531kHz or 51.2us - max 13.1ms // Used to time arm/disarm intervals TCCR2A = 0; TCCR2B = 0x07; // Clk/1024 = 19.531kHz TIMSK2 = 0; TIFR2 = 0; TCNT2 = 0; // Reset counter //*********************************************************** // Interrupts and pin function setup //*********************************************************** // Pin change interrupt enables PCINT1, PCINT2 and PCINT3 (Throttle, AUX and CPPM input) PCICR = 0x0A; // PCINT8 to PCINT15 (PCINT1 group - AUX) // PCINT24 to PCINT31 (PCINT3 group - THR) PCIFR = 0x0F; // Clear PCIF0 interrupt flag // Clear PCIF1 interrupt flag // Clear PCIF2 interrupt flag // Clear PCIF3 interrupt flag // External interrupts INT0 (Elevator) and INT1 (Aileron) and INT2 (Rudder) EICRA = 0x15; // Any change INT0 // Any change INT1 // Any change INT2 EIFR = 0x07; // Clear INT0 interrupt flag (Elevator) // Clear INT1 interrupt flag (Aileron) // Clear INT2 interrupt flag (Rudder/CPPM) //*********************************************************** // i2c init for KK2.1 //*********************************************************** #ifdef KK21 i2c_init(); init_i2c_gyros(); init_i2c_accs(); #endif //*********************************************************** // Start up //*********************************************************** // Preset important flags Interrupted = false; Main_flags |= (1 << FirstTimeIMU); Main_flags |= (1 << FirstTimeFlightMode); // Initialise the GLCD st7565_init(); st7565_command(CMD_DISPLAY_ON); st7565_command(CMD_SET_ALLPTS_NORMAL); st7565_set_brightness(0x26); st7565_command(CMD_SET_COM_REVERSE); // For logo // Make sure the LCD is blank clear_screen(); // This delay prevents the GLCD flashing up a ghost image of old data _delay_ms(300); // Reload default eeprom settings if middle two buttons are pressed (or all, for older users) if (((PINB & 0xf0) == 0x90) || ((PINB & 0xf0) == 0x00)) { // Display reset message st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) clear_buffer(buffer); LCD_Display_Text(1,(prog_uchar*)Verdana14,40,25); write_buffer(buffer,1); clear_buffer(buffer); Set_EEPROM_Default_Config(); Save_Config_to_EEPROM(); } // Load "Config" global data structure else { Initial_EEPROM_Config_Load(); } // Now set contrast to the previously saved value st7565_set_brightness((uint8_t)Config.Contrast); #ifdef KK21 // Write logo from buffer write_buffer(buffer,0); _delay_ms(500); #endif #ifndef KK21 // Display "Hold steady" message for KK2.0 st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) clear_buffer(buffer); LCD_Display_Text(2,(prog_uchar*)Verdana14,18,25); write_buffer(buffer,1); clear_buffer(buffer); #endif // Do startup tasks UpdateLimits(); // Update travel limts UpdateIMUvalues(); // Update IMU factors Init_ADC(); init_int(); // Intialise interrupts based on RC input mode // Initialise UART init_uart(); // Initial gyro calibration CalibrateGyrosSlow(); // Check to see that gyros are stable ReadGyros(); if ((gyroADC[ROLL] > GYROS_STABLE) || (gyroADC[ROLL] < -GYROS_STABLE) || (gyroADC[PITCH] > GYROS_STABLE) || (gyroADC[PITCH] < -GYROS_STABLE) || (gyroADC[YAW] > GYROS_STABLE) || (gyroADC[YAW] < -GYROS_STABLE)) { General_error |= (1 << SENSOR_ERROR); // Set sensor error bit } // Check to see that throttle is low if in serial mode. // Don't bother if in CamStab mode _delay_ms(100); if ( ( (Config.RxMode == CPPM_MODE) || (Config.RxMode == XTREME) || (Config.RxMode == SBUS) || (Config.RxMode == SPEKTRUM) ) && (Config.CamStab == OFF) ) { RxGetChannels(); if (RCinputs[THROTTLE] > -900) { General_error |= (1 << THROTTLE_HIGH); // Set throttle high error bit } } // Flash LED LED1 = 1; _delay_ms(150); LED1 = 0; // Beep that all sensors have been handled menu_beep(1); // Set text display mode back to normal st7565_command(CMD_SET_COM_NORMAL); // For text (not for logo) } // init()
void Display_sticks(void) { int8_t i; int8_t offset; int8_t temp_aileron, temp_elevator, temp_rudder; bool CalibrateDone = false; // Save original settings in case user aborts temp_aileron = Config.AileronPol; temp_elevator = Config.ElevatorPol; temp_rudder = Config.RudderPol; // Reset to defaults - not ideal, but it works Config.AileronPol = NORMAL; Config.ElevatorPol = NORMAL; Config.RudderPol = NORMAL; // Until exit button pressed while((BUTTON1 != 0) && (!CalibrateDone)) { offset = 0; // Clear screen buffer clear_buffer(buffer); // Draw graphic for (i = 0; i < 2; i++) { drawrect(buffer, 17 + offset, 0, 40, 40, 1); // Box drawline(buffer, 38 + offset,20, 48 + offset, 3, 1); // Line 1 drawline(buffer, 41 + offset,21, 56 + offset, 6, 1); // Line 2 fillcircle(buffer, 38 + offset, 21, 2, 1); // Centre fillcircle(buffer, 51 + offset, 5, 4, 1); // End offset = 52; } // Print bottom text and markers LCD_Display_Text(12, (const unsigned char*)Wingdings, 0, 57); // Left // If uncalibrated if (!CalibrateDone) { RxGetChannels(); // Display "No RX signal" if no input detected if(RxChannel[AILERON] == 0) { LCD_Display_Text(135,(const unsigned char*)Verdana14,14,43); // "No RX signal" } // Sticks have not moved far enough else if ((RxChannel[AILERON] > 3000) && (RxChannel[AILERON] < 4500)) { LCD_Display_Text(136,(const unsigned char*)Verdana14,9,43); // "Hold as shown" } // Sticks should now be in the right position // Reverse wrong input channels else { if (RCinputs[AILERON] < 0) { Config.AileronPol = REVERSED; } if (RCinputs[ELEVATOR] < 0) { Config.ElevatorPol = REVERSED; } if (RCinputs[RUDDER] < 0) { Config.RudderPol = REVERSED; } // If all positive - done! if ((RCinputs[AILERON] > 0) && (RCinputs[ELEVATOR] > 0) && (RCinputs[RUDDER] > 0)) { CalibrateDone = true; } } } // Update buffer write_buffer(buffer,1); _delay_ms(100); } // Save value and return if (CalibrateDone) { LCD_Display_Text(137,(const unsigned char*)Verdana14,40,43); // "Done!" // Update buffer write_buffer(buffer,1); clear_buffer(buffer); Save_Config_to_EEPROM(); } else { // Restore old settings if failed Config.AileronPol = temp_aileron; Config.ElevatorPol = temp_elevator; Config.RudderPol = temp_rudder; } }